Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

getInfo2NonVirtual() merged to getInfo2NonVirtualUsingFrameOffset() to reduce code size

Browse Source
This commit is contained in:
rponom
2010-02-04 23:56:08 +00:00
parent 0cb44b169d
commit c31e2ed064
1 changed files with 115 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
View File

@@ -217,16 +217,17 @@ void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
{
if(m_useOffsetForConstraintFrame)
{
// if(m_useOffsetForConstraintFrame)
// {
getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
}
else
{
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
}
// }
// else
// {
// getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
// }
}
#if 0 // replaced with getInfo2NonVirtualUsingFrameOffset
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
{
//prepare constraint
@@ -403,7 +404,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[nrow] = m_cfmDirLin;
info->cfm[srow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
@@ -520,7 +521,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[nrow] = m_cfmLimAng;
info->cfm[srow] = m_cfmLimAng;
}
if(lostop == histop)
{
@@ -573,6 +574,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
} // if(limit)
} // if angular limit or powered
}
#endif
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
@@ -919,15 +921,25 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
factA = btScalar(0.5f);
}
factB = btScalar(1.0f) - factA;
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
btVector3 ax1, p, q;
btVector3 ax1A = trA.getBasis().getColumn(0);
btVector3 ax1B = trB.getBasis().getColumn(0);
btVector3 ax1 = ax1A * factA + ax1B * factB;
if(m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
btVector3 p, q;
btPlaneSpace1 (ax1, p, q);
}
else
{ // old way - use frameA
ax1 = trA.getBasis().getColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA.getBasis().getColumn(1);
q = trA.getBasis().getColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
@@ -993,7 +1005,9 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
btVector3 tmpA, tmpB, relA, relB;
btVector3 tmpA, tmpB, relA, relB, c;
if(m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to slider axis
@@ -1041,6 +1055,21 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
}
else
{ // old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
btVector3 tmp = c.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
tmp = c.cross(q);
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
}
// compute two elements of right hand side
// k = info->fps * info->erp * getSoftnessOrthoLin();
@@ -1086,6 +1115,8 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if(m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if(!hasStaticBody)
{
@@ -1098,6 +1129,18 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
info->m_J2angularAxis[srow+1] = -tmpB[1];
info->m_J2angularAxis[srow+2] = -tmpB[2];
}
}
else
{ // The old way. May be incorrect if bodies are not on the slider axis
btVector3 ltd; // Linear Torque Decoupling vector (a torque)
ltd = c.cross(ax1);
info->m_J1angularAxis[srow+0] = factA*ltd[0];
info->m_J1angularAxis[srow+1] = factA*ltd[1];
info->m_J1angularAxis[srow+2] = factA*ltd[2];
info->m_J2angularAxis[srow+0] = factB*ltd[0];
info->m_J2angularAxis[srow+1] = factB*ltd[1];
info->m_J2angularAxis[srow+2] = factB*ltd[2];
}
// right-hand part
btScalar lostop = getLowerLinLimit();
btScalar histop = getUpperLinLimit();
@@ -1113,7 +1156,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
{
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{
info->cfm[nrow] = m_cfmDirLin;
info->cfm[srow] = m_cfmDirLin;
}
btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
@@ -1229,7 +1272,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{
info->cfm[nrow] = m_cfmLimAng;
info->cfm[srow] = m_cfmLimAng;
}
if(lostop == histop)
{