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

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

@@ -217,16 +217,17 @@ void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
void btSliderConstraint::getInfo2(btConstraintInfo2* 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()); getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
} // }
else // else
{ // {
getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass()); // 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 ) void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
{ {
//prepare constraint //prepare constraint
@@ -403,7 +404,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
{ {
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{ {
info->cfm[nrow] = m_cfmDirLin; info->cfm[srow] = m_cfmDirLin;
} }
btScalar tag_vel = getTargetLinMotorVelocity(); btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); 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; info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{ {
info->cfm[nrow] = m_cfmLimAng; info->cfm[srow] = m_cfmLimAng;
} }
if(lostop == histop) if(lostop == histop)
{ {
@@ -573,6 +574,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
} // if(limit) } // if(limit)
} // if angular limit or powered } // if angular limit or powered
} }
#endif
void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
@@ -919,15 +921,25 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
factA = btScalar(0.5f); factA = btScalar(0.5f);
} }
factB = btScalar(1.0f) - factA; factB = btScalar(1.0f) - factA;
// get the desired direction of slider axis btVector3 ax1, p, q;
// as weighted sum of X-orthos of frameA and frameB in WCS
btVector3 ax1A = trA.getBasis().getColumn(0); btVector3 ax1A = trA.getBasis().getColumn(0);
btVector3 ax1B = trB.getBasis().getColumn(0); btVector3 ax1B = trB.getBasis().getColumn(0);
btVector3 ax1 = ax1A * factA + ax1B * factB; if(m_useOffsetForConstraintFrame)
ax1.normalize(); {
// construct two orthos to slider axis // get the desired direction of slider axis
btVector3 p, q; // as weighted sum of X-orthos of frameA and frameB in WCS
btPlaneSpace1 (ax1, p, q); ax1 = ax1A * factA + ax1B * factB;
ax1.normalize();
// construct two orthos to slider axis
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 // make rotations around these orthos equal
// the slider axis should be the only unconstrained // the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to // rotational axis, the angular velocity of the two bodies perpendicular to
@@ -993,54 +1005,71 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
int s2 = nrow * s; int s2 = nrow * s;
nrow++; nrow++;
int s3 = nrow * s; int s3 = nrow * s;
btVector3 tmpA, tmpB, relA, relB; btVector3 tmpA, tmpB, relA, relB, c;
// get vector from bodyB to frameB in WCS if(m_useOffsetForConstraintFrame)
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
btScalar sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{ {
p /= btSqrt(len2); // get vector from bodyB to frameB in WCS
relB = trB.getOrigin() - bodyB_trans.getOrigin();
// get its projection to slider axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = trA.getOrigin() - bodyA_trans.getOrigin();
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
btScalar sliderOffs = m_linPos - m_depth[0];
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
btVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
btScalar len2 = p.length2();
if(len2 > SIMD_EPSILON)
{
p /= btSqrt(len2);
}
else
{
p = trA.getBasis().getColumn(1);
}
// make one more ortho
q = ax1.cross(p);
// fill two rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[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 else
{ { // old way - maybe incorrect if bodies are not on the slider axis
p = trA.getBasis().getColumn(1); // 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];
} }
// make one more ortho
q = ax1.cross(p);
// fill two rows
tmpA = relA.cross(p);
tmpB = relB.cross(p);
for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
tmpA = relA.cross(q);
tmpB = relB.cross(q);
if(hasStaticBody && getSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[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];
// compute two elements of right hand side // compute two elements of right hand side
// k = info->fps * info->erp * getSoftnessOrthoLin(); // k = info->fps * info->erp * getSoftnessOrthoLin();
@@ -1086,17 +1115,31 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
// constraint force is applied at must lie along the same ax1 axis. // constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free // a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum. // bodies from gaining angular momentum.
// this is needed only when bodyA and bodyB are both dynamic. if(m_useOffsetForConstraintFrame)
if(!hasStaticBody)
{ {
tmpA = relA.cross(ax1); // this is needed only when bodyA and bodyB are both dynamic.
tmpB = relB.cross(ax1); if(!hasStaticBody)
info->m_J1angularAxis[srow+0] = tmpA[0]; {
info->m_J1angularAxis[srow+1] = tmpA[1]; tmpA = relA.cross(ax1);
info->m_J1angularAxis[srow+2] = tmpA[2]; tmpB = relB.cross(ax1);
info->m_J2angularAxis[srow+0] = -tmpB[0]; info->m_J1angularAxis[srow+0] = tmpA[0];
info->m_J2angularAxis[srow+1] = -tmpB[1]; info->m_J1angularAxis[srow+1] = tmpA[1];
info->m_J2angularAxis[srow+2] = -tmpB[2]; info->m_J1angularAxis[srow+2] = tmpA[2];
info->m_J2angularAxis[srow+0] = -tmpB[0];
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 // right-hand part
btScalar lostop = getLowerLinLimit(); btScalar lostop = getLowerLinLimit();
@@ -1113,7 +1156,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
{ {
if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN) if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
{ {
info->cfm[nrow] = m_cfmDirLin; info->cfm[srow] = m_cfmDirLin;
} }
btScalar tag_vel = getTargetLinMotorVelocity(); btScalar tag_vel = getTargetLinMotorVelocity();
btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP); 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; info->m_constraintError[srow] += k * limit_err;
if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG) if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
{ {
info->cfm[nrow] = m_cfmLimAng; info->cfm[srow] = m_cfmLimAng;
} }
if(lostop == histop) if(lostop == histop)
{ {