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];
73 const btMultiBody* multiBodyA = constraint.m_multiBodyA;
74 const btMultiBody* multiBodyB = constraint.m_multiBodyB;
78 const btScalar* jacA = &
data.m_jacobians[constraint.m_jacAindex];
79 const btScalar* deltaA = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
80 const int ndofA = multiBodyA->getNumDofs() + 6;
85 const int solverBodyIdA = constraint.m_solverBodyIdA;
87 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
88 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
90 constraint.m_relpos1CrossNormal,
92 constraint.m_angularComponentA);
97 const btScalar* jacB = &
data.m_jacobians[constraint.m_jacBindex];
98 const btScalar* deltaB = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
99 const int ndofB = multiBodyB->getNumDofs() + 6;
104 const int solverBodyIdB = constraint.m_solverBodyIdB;
106 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
107 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
109 constraint.m_relpos2CrossNormal,
111 constraint.m_angularComponentB);
125 const btMultiBody* multiBodyA = constraint.m_multiBodyA;
126 const btMultiBody* multiBodyB = constraint.m_multiBodyB;
127 const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
128 const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
132 btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
134 if (offDiagMultiBodyA)
136 const btScalar* offDiagJacA = &
data.m_jacobians[offDiagConstraint.m_jacAindex];
138 if (offDiagMultiBodyA == multiBodyA)
140 const int ndofA = multiBodyA->getNumDofs() + 6;
141 const btScalar* deltaA = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
144 else if (offDiagMultiBodyA == multiBodyB)
146 const int ndofB = multiBodyB->getNumDofs() + 6;
147 const btScalar* deltaB = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
153 const int solverBodyIdA = constraint.m_solverBodyIdA;
154 const int solverBodyIdB = constraint.m_solverBodyIdB;
156 const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
157 btAssert(offDiagSolverBodyIdA != -1);
159 if (offDiagSolverBodyIdA == solverBodyIdA)
162 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
163 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
165 offDiagConstraint.m_relpos1CrossNormal,
166 offDiagConstraint.m_contactNormal1,
167 invMassA, constraint.m_angularComponentA,
168 constraint.m_contactNormal1);
170 else if (offDiagSolverBodyIdA == solverBodyIdB)
173 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
174 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
176 offDiagConstraint.m_relpos1CrossNormal,
177 offDiagConstraint.m_contactNormal1,
179 constraint.m_angularComponentB,
180 constraint.m_contactNormal2);
184 if (offDiagMultiBodyB)
186 const btScalar* offDiagJacB = &
data.m_jacobians[offDiagConstraint.m_jacBindex];
188 if (offDiagMultiBodyB == multiBodyA)
190 const int ndofA = multiBodyA->getNumDofs() + 6;
191 const btScalar* deltaA = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
194 else if (offDiagMultiBodyB == multiBodyB)
196 const int ndofB = multiBodyB->getNumDofs() + 6;
197 const btScalar* deltaB = &
data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
203 const int solverBodyIdA = constraint.m_solverBodyIdA;
204 const int solverBodyIdB = constraint.m_solverBodyIdB;
206 const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
207 btAssert(offDiagSolverBodyIdB != -1);
209 if (offDiagSolverBodyIdB == solverBodyIdA)
212 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
213 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
215 offDiagConstraint.m_relpos2CrossNormal,
216 offDiagConstraint.m_contactNormal2,
217 invMassA, constraint.m_angularComponentA,
218 constraint.m_contactNormal1);
220 else if (offDiagSolverBodyIdB == solverBodyIdB)
223 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
224 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
226 offDiagConstraint.m_relpos2CrossNormal,
227 offDiagConstraint.m_contactNormal2,
228 invMassB, constraint.m_angularComponentB,
229 constraint.m_contactNormal2);
248 if (numConstraintRows == 0)
251 int n = numConstraintRows;
254 m_b.resize(numConstraintRows);
258 for (
int i = 0; i < numConstraintRows; i++)
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++)
298 int numBodies = m_tmpSolverBodyPool.size();
319 JinvM3.resize(2 * m, 8);
342 btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
343 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
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++)
365 for (
int r = 0;
r < 3;
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++)
402 for (
int r = 0;
r < 3;
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();
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;
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;
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;
497 for (; row__ < numJointRows;)
502 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
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);
544 m_x[i] =
c.m_appliedImpulse;
560 if (multiBodyNumConstraints == 0)
570 for (
int i = 0; i < multiBodyNumConstraints; ++i)
573 const btScalar jacDiag = constraint.m_jacDiagABInv;
578 const btScalar rhs = constraint.m_rhs;
591 for (
int i = 0; i < multiBodyNumConstraints; ++i)
605 m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
608 for (
int i = 0; i < multiBodyNumConstraints; ++i)
618 for (
int j = i + 1; j < multiBodyNumConstraints; ++j)
644 for (
int i = 0; i < multiBodyNumConstraints; ++i)
729 int firstContactConstraintOffset = dindex;
743 if (numFrictionPerContact == 2)
779 for (
int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
785 firstContactConstraintOffset = dindex;
800 const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
804 if (numtiBodyNumFrictionPerContact == 2)
868 const btScalar deltaImpulse =
m_x[i] -
c.m_appliedImpulse;
869 c.m_appliedImpulse =
m_x[i];
871 int sbA =
c.m_solverBodyIdA;
872 int sbB =
c.m_solverBodyIdB;
877 solverBodyA.internalApplyImpulse(
c.m_contactNormal1 * solverBodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
878 solverBodyB.internalApplyImpulse(
c.m_contactNormal2 * solverBodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
883 solverBodyA.internalApplyPushImpulse(
c.m_contactNormal1 * solverBodyA.internalGetInvMass(),
c.m_angularComponentA, deltaPushImpulse);
884 solverBodyB.internalApplyPushImpulse(
c.m_contactNormal2 * solverBodyB.internalGetInvMass(),
c.m_angularComponentB, deltaPushImpulse);
899 const int ndofA = multiBodyA->getNumDofs() + 6;
900 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacAindex], deltaImpulse,
c.m_deltaVelAindex, ndofA);
901 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
904 multiBodyA->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacAindex], deltaImpulse);
909 const int sbA =
c.m_solverBodyIdA;
911 solverBodyA.internalApplyImpulse(
c.m_contactNormal1 * solverBodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
917 const int ndofB = multiBodyB->getNumDofs() + 6;
918 applyDeltaVee(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacBindex], deltaImpulse,
c.m_deltaVelBindex, ndofB);
919 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
922 multiBodyB->applyDeltaVeeMultiDof2(&
m_data.m_deltaVelocitiesUnitImpulse[
c.m_jacBindex], deltaImpulse);
927 const int sbB =
c.m_solverBodyIdB;
929 solverBodyB.internalApplyImpulse(
c.m_contactNormal2 * solverBodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
938 : m_solver(solver), m_fallback(0)
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
btConstraintSolverType
btConstraintSolver provides solver interface
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
static btScalar computeConstraintMatrixDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint)
static btScalar computeConstraintMatrixOffDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint, const btMultiBodySolverConstraint &offDiagConstraint)
static btScalar computeDeltaVelocityInConstraintSpace(const btVector3 &angularDeltaVelocity, const btVector3 &contactNormal, btScalar invMass, const btVector3 &angularJacobian, const btVector3 &linearJacobian)
static bool interleaveContactAndFriction1
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btVector3 m_relpos2CrossNormal
btVector3 m_contactNormal1
btVector3 m_relpos1CrossNormal
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE void reserve(int _Count)
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE void push_back(const T &_Val)
SIMD_FORCE_INLINE T & expand(const T &fillValue=T())
original version written by Erwin Coumans, October 2013
virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray< int > &limitDependency, int numIterations, bool useSparsity=true)=0
btMatrixXu m_scratchJ3
Cache variable for constraint Jacobian matrix.
btAlignedObjectArray< int > m_multiBodyLimitDependencies
btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
btVectorXu m_xSplit
Split impulse cache vector corresponding to m_x.
btVectorXu m_b
b vector in the MLCP formulation.
btAlignedObjectArray< btMultiBodySolverConstraint * > m_multiBodyAllConstraintPtrArray
Array of all the multibody constraints.
btMLCPSolverInterface * m_solver
MLCP solver.
int getNumFallbacks() const
void setNumFallbacks(int num)
Sets the number of fallbacks. This function may be used to reset the number to zero.
btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btVectorXu m_lo
Lower bound of constraint impulse, m_x.
virtual ~btMultiBodyMLCPConstraintSolver()
Destructor.
btMatrixXu m_A
A matrix in the MLCP formulation.
btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface *solver)
int m_fallback
Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver f...
btMatrixXu m_multiBodyA
A matrix in the MLCP formulation.
btAlignedObjectArray< int > m_limitDependencies
btVectorXu m_bSplit
Split impulse Cache vector corresponding to m_b.
void setMLCPSolver(btMLCPSolverInterface *solver)
Sets MLCP solver. Assumed it's not null.
btAlignedObjectArray< int > m_scratchOfs
Cache variable for offsets.
void createMLCPFastRigidBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two rigid bodies.
btVectorXu m_hi
Upper bound of constraint impulse, m_x.
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Solves MLCP and returns the success.
btVectorXu m_x
Constraint impulse, which is an output of MLCP solving.
btVectorXu m_multiBodyLo
Lower bound of constraint impulse, m_x.
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms, which are m_A, m_b, m_lo, and m_hi.
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Array of all the rigid body constraints.
btMatrixXu m_scratchJInvM3
Cache variable for constraint Jacobian times inverse mass matrix.
btVectorXu m_multiBodyX
Constraint impulse, which is an output of MLCP solving.
void createMLCPFastMultiBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody.
btVectorXu m_multiBodyHi
Upper bound of constraint impulse, m_x.
virtual btConstraintSolverType getSolverType() const
Returns the constraint solver type.
btVectorXu m_multiBodyB
b vector in the MLCP formulation.
btScalar getInvMass() const
const btMatrix3x3 & getInvInertiaTensorWorld() const