23 #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 43 return angularDeltaVelocity.
dot(angularJacobian) + contactNormal.
dot(linearJacobian) * invMass;
53 return angularDeltaVelocity.
dot(angularJacobian) + invMass;
60 for (
int i = 0; i <
size; ++i)
61 result += deltaVelocity[i] * jacobian[i];
80 const int ndofA = multiBodyA->
getNumDofs() + 6;
87 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
99 const int ndofB = multiBodyB->
getNumDofs() + 6;
106 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
132 btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
134 if (offDiagMultiBodyA)
138 if (offDiagMultiBodyA == multiBodyA)
140 const int ndofA = multiBodyA->
getNumDofs() + 6;
144 else if (offDiagMultiBodyA == multiBodyB)
146 const int ndofB = multiBodyB->
getNumDofs() + 6;
157 btAssert(offDiagSolverBodyIdA != -1);
159 if (offDiagSolverBodyIdA == solverBodyIdA)
162 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
170 else if (offDiagSolverBodyIdA == solverBodyIdB)
173 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
184 if (offDiagMultiBodyB)
188 if (offDiagMultiBodyB == multiBodyA)
190 const int ndofA = multiBodyA->
getNumDofs() + 6;
194 else if (offDiagMultiBodyB == multiBodyB)
196 const int ndofB = multiBodyB->
getNumDofs() + 6;
207 btAssert(offDiagSolverBodyIdB != -1);
209 if (offDiagSolverBodyIdB == solverBodyIdA)
212 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
220 else if (offDiagSolverBodyIdB == solverBodyIdB)
223 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
238 createMLCPFastRigidBody(infoGlobal);
239 createMLCPFastMultiBody(infoGlobal);
246 int numConstraintRows = m_allConstraintPtrArray.size();
248 if (numConstraintRows == 0)
251 int n = numConstraintRows;
254 m_b.resize(numConstraintRows);
255 m_bSplit.resize(numConstraintRows);
258 for (
int i = 0; i < numConstraintRows; i++)
260 btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
263 btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
264 btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
265 m_b[i] = rhs / jacDiag;
266 m_bSplit[i] = rhsPenetration / jacDiag;
274 m_lo.resize(numConstraintRows);
275 m_hi.resize(numConstraintRows);
280 for (
int i = 0; i < numConstraintRows; i++)
289 m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
290 m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
296 int m = m_allConstraintPtrArray.size();
298 int numBodies = m_tmpSolverBodyPool.size();
302 bodyJointNodeArray.
resize(numBodies, -1);
307 jointNodeArray.
reserve(2 * m_allConstraintPtrArray.size());
319 JinvM3.resize(2 * m, 8);
337 for (
int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
340 int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
341 int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
342 btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
343 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
345 numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
351 slotA = jointNodeArray.
size();
353 int prevSlot = bodyJointNodeArray[sbA];
354 bodyJointNodeArray[sbA] = slotA;
355 jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
356 jointNodeArray[slotA].jointIndex = c;
357 jointNodeArray[slotA].constraintRowIndex = i;
358 jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
360 for (
int row = 0; row < numRows; row++, cur++)
362 btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->
getInvMass();
365 for (
int r = 0; r < 3; r++)
367 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
368 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
369 JinvM3.setElem(cur, r, normalInvMass[r]);
370 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
372 J3.setElem(cur, 3, 0);
373 JinvM3.setElem(cur, 3, 0);
374 J3.setElem(cur, 7, 0);
375 JinvM3.setElem(cur, 7, 0);
387 slotB = jointNodeArray.
size();
389 int prevSlot = bodyJointNodeArray[sbB];
390 bodyJointNodeArray[sbB] = slotB;
391 jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
392 jointNodeArray[slotB].jointIndex = c;
393 jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
394 jointNodeArray[slotB].constraintRowIndex = i;
397 for (
int row = 0; row < numRows; row++, cur++)
399 btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->
getInvMass();
402 for (
int r = 0; r < 3; r++)
404 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
405 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
406 JinvM3.setElem(cur, r, normalInvMassB[r]);
407 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
409 J3.setElem(cur, 3, 0);
410 JinvM3.setElem(cur, 3, 0);
411 J3.setElem(cur, 7, 0);
412 JinvM3.setElem(cur, 7, 0);
419 rowOffset += numRows;
424 const btScalar* JinvM = JinvM3.getBufferPointer();
426 const btScalar* Jptr = J3.getBufferPointer();
440 for (
int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
443 int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
444 int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
448 numRows = i < m_tmpSolverNonContactConstraintPool.
size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
450 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
453 int startJointNodeA = bodyJointNodeArray[sbA];
454 while (startJointNodeA >= 0)
456 int j0 = jointNodeArray[startJointNodeA].jointIndex;
457 int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
460 int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.
size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
461 size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
463 m_A.multiplyAdd2_p8r(JinvMrow,
464 Jptr + 2 * 8 * (
size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
466 startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
471 int startJointNodeB = bodyJointNodeArray[sbB];
472 while (startJointNodeB >= 0)
474 int j1 = jointNodeArray[startJointNodeB].jointIndex;
475 int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
479 int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.
size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
480 size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
481 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)numRows,
482 Jptr + 2 * 8 * (
size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
484 startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
494 int numJointRows = m_allConstraintPtrArray.size();
497 for (; row__ < numJointRows;)
500 int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
502 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
504 const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
506 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
507 const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
508 m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
511 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)infom, Jrow + 8 * (
size_t)infom, infom, infom, row__, row__);
522 for (
int i = 0; i < m_A.rows(); ++i)
531 m_A.copyLowerToUpperTriangle();
536 m_x.resize(numConstraintRows);
537 m_xSplit.resize(numConstraintRows);
541 for (
int i = 0; i < m_allConstraintPtrArray.size(); i++)
558 const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
560 if (multiBodyNumConstraints == 0)
567 m_multiBodyB.resize(multiBodyNumConstraints);
568 m_multiBodyB.setZero();
570 for (
int i = 0; i < multiBodyNumConstraints; ++i)
579 m_multiBodyB[i] = rhs / jacDiag;
588 m_multiBodyLo.resize(multiBodyNumConstraints);
589 m_multiBodyHi.resize(multiBodyNumConstraints);
591 for (
int i = 0; i < multiBodyNumConstraints; ++i)
605 m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
608 for (
int i = 0; i < multiBodyNumConstraints; ++i)
613 m_multiBodyA.setElem(i, i, diagA);
618 for (
int j = i + 1; j < multiBodyNumConstraints; ++j)
624 m_multiBodyA.setElem(i, j, offDiagA);
625 m_multiBodyA.setElem(j, i, offDiagA);
631 for (
int i = 0; i < m_multiBodyA.rows(); ++i)
640 m_multiBodyX.resize(multiBodyNumConstraints);
644 for (
int i = 0; i < multiBodyNumConstraints; ++i)
652 m_multiBodyX.setZero();
670 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.
m_numIterations);
672 result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.
m_numIterations);
676 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.
m_numIterations);
683 if (m_multiBodyA.rows() != 0)
685 result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.
m_numIterations);
703 bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
713 const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
714 const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
716 m_allConstraintPtrArray.resize(0);
717 m_multiBodyAllConstraintPtrArray.resize(0);
721 m_limitDependencies.resize(numRigidBodyConstraints);
723 for (
int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
725 m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
726 m_limitDependencies[dindex++] = -1;
729 int firstContactConstraintOffset = dindex;
734 for (
int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
736 const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
738 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
739 m_limitDependencies[dindex++] = -1;
740 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
741 int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
742 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
743 if (numFrictionPerContact == 2)
745 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
746 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
752 for (
int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
754 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
755 m_limitDependencies[dindex++] = -1;
757 for (
int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
759 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
760 m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
764 if (!m_allConstraintPtrArray.size())
777 m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
779 for (
int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
781 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
782 m_multiBodyLimitDependencies[dindex++] = -1;
785 firstContactConstraintOffset = dindex;
790 for (
int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
792 const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
794 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
795 m_multiBodyLimitDependencies[dindex++] = -1;
797 btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
798 m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
800 const int findex = (frictionContactConstraint1.
m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
802 m_multiBodyLimitDependencies[dindex++] = findex;
804 if (numtiBodyNumFrictionPerContact == 2)
806 btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
807 m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
809 m_multiBodyLimitDependencies[dindex++] = findex;
815 for (
int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
817 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
818 m_multiBodyLimitDependencies[dindex++] = -1;
820 for (
int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
822 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
823 m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].
m_frictionIndex + firstContactConstraintOffset;
827 if (!m_multiBodyAllConstraintPtrArray.size())
829 m_multiBodyA.resize(0, 0);
830 m_multiBodyB.resize(0);
831 m_multiBodyX.resize(0);
832 m_multiBodyLo.resize(0);
833 m_multiBodyHi.resize(0);
840 createMLCPFast(infoGlobal);
851 result = solveMLCP(infoGlobal);
864 for (
int i = 0; i < m_allConstraintPtrArray.size(); ++i)
889 for (
int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
899 const int ndofA = multiBodyA->
getNumDofs() + 6;
901 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 905 #endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 917 const int ndofB = multiBodyB->
getNumDofs() + 6;
919 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 923 #endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 938 : m_solver(solver), m_fallback(0)
btScalar getInvMass() const
void createMLCPFastMultiBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody...
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Solves MLCP and returns the success.
static btScalar computeDeltaVelocityInConstraintSpace(const btVector3 &angularDeltaVelocity, const btVector3 &contactNormal, btScalar invMass, const btVector3 &angularJacobian, const btVector3 &linearJacobian)
btVector3 m_relpos1CrossNormal
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
btVector3 m_angularComponentA
btConstraintSolverType
btConstraintSolver provides solver interface
btVector3 m_angularComponentB
int m_fallback
Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver f...
virtual ~btMultiBodyMLCPConstraintSolver()
Destructor.
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
void createMLCPFastRigidBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two rigid bodies.
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
int getNumFallbacks() const
Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the ...
void setNumFallbacks(int num)
Sets the number of fallbacks. This function may be used to reset the number to zero.
void setMLCPSolver(btMLCPSolverInterface *solver)
Sets MLCP solver. Assumed it's not null.
static bool interleaveContactAndFriction
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btScalar dot(const btVector3 &v) const
Return the dot product.
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btVector3 m_angularComponentA
int size() const
return the number of elements in the array
btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface *solver)
Constructor.
btMultiBody * m_multiBodyA
original version written by Erwin Coumans, October 2013
btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
static btScalar computeConstraintMatrixDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint)
btCollisionObject can be used to manage collision detection objects.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
btMultiBody * m_multiBodyB
The btRigidBody is the main class for rigid body objects.
btVector3 m_angularComponentB
btAlignedObjectArray< btScalar > m_jacobians
const btVector3 & internalGetInvMass() const
btVector3 can be used to represent 3D points and vectors.
btSimdScalar m_appliedImpulse
btSimdScalar m_appliedPushImpulse
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
bool btFuzzyZero(btScalar x)
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btMLCPSolverInterface * m_solver
MLCP solver.
const btMatrix3x3 & getInvInertiaTensorWorld() const
T & expand(const T &fillValue=T())
btVector3 m_relpos2CrossNormal
btVector3 m_contactNormal1
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms, which are m_A, m_b, m_lo, and m_hi.
btSimdScalar m_appliedImpulse
btVector3 m_contactNormal1
static btScalar computeConstraintMatrixOffDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint, const btMultiBodySolverConstraint &offDiagConstraint)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btVector3 m_contactNormal2
virtual btConstraintSolverType getSolverType() const
Returns the constraint solver type.