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];
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++)
302 bodyJointNodeArray.
resize(numBodies, -1);
319 JinvM3.resize(2 * m, 8);
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;)
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);
560 if (multiBodyNumConstraints == 0)
570 for (
int i = 0; i < multiBodyNumConstraints; ++i)
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)
703 bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
729 int firstContactConstraintOffset = dindex;
743 if (numFrictionPerContact == 2)
785 firstContactConstraintOffset = dindex;
800 const int findex = (frictionContactConstraint1.
m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
804 if (numtiBodyNumFrictionPerContact == 2)
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)
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Array of all the rigid body constraints.
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.
btVectorXu m_xSplit
Split impulse cache vector corresponding to m_x.
void push_back(const T &_Val)
static btScalar computeDeltaVelocityInConstraintSpace(const btVector3 &angularDeltaVelocity, const btVector3 &contactNormal, btScalar invMass, const btVector3 &angularJacobian, const btVector3 &linearJacobian)
btVector3 m_relpos1CrossNormal
btConstraintArray m_tmpSolverContactFrictionConstraintPool
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)
btVectorXu m_hi
Upper bound of constraint impulse, m_x.
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.
btVectorXu m_x
Constraint impulse, which is an output of MLCP solving.
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
const btVector3 & internalGetInvMass() const
btVectorXu m_multiBodyLo
Lower bound of constraint impulse, m_x.
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)
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
btMatrixXu m_scratchJInvM3
Cache variable for constraint Jacobian times inverse mass matrix.
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btMatrixXu m_multiBodyA
A matrix in the MLCP formulation.
btVector3 m_angularComponentA
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface *solver)
Constructor.
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
btMultiBody * m_multiBodyA
btScalar getInvMass() const
original version written by Erwin Coumans, October 2013
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
btAlignedObjectArray< int > m_multiBodyLimitDependencies
Indices of normal contact constraint associated with frictional contact constraint for multibodies...
btConstraintArray m_tmpSolverContactConstraintPool
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
btMatrixXu m_A
A matrix in the MLCP formulation.
The btRigidBody is the main class for rigid body objects.
btVector3 m_angularComponentB
btAlignedObjectArray< int > m_scratchOfs
Cache variable for offsets.
btAlignedObjectArray< int > m_limitDependencies
Indices of normal contact constraint associated with frictional contact constraint for rigid bodies...
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btAlignedObjectArray< btScalar > m_jacobians
btVectorXu m_b
b vector in the MLCP formulation.
btVector3 can be used to represent 3D points and vectors.
int size() const
return the number of elements in the array
int getNumFallbacks() const
Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the ...
btSimdScalar m_appliedImpulse
btAlignedObjectArray< btMultiBodySolverConstraint * > m_multiBodyAllConstraintPtrArray
Array of all the multibody constraints.
btSimdScalar m_appliedPushImpulse
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
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)
btVectorXu m_multiBodyHi
Upper bound of constraint impulse, m_x.
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)
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btMLCPSolverInterface * m_solver
MLCP solver.
T & expand(const T &fillValue=T())
btVector3 m_relpos2CrossNormal
btVector3 m_contactNormal1
const btMatrix3x3 & getInvInertiaTensorWorld() const
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
btVectorXu m_bSplit
Split impulse Cache vector corresponding to m_b.
static btScalar computeConstraintMatrixOffDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint, const btMultiBodySolverConstraint &offDiagConstraint)
btVectorXu m_multiBodyB
b vector in the MLCP formulation.
btVectorXu m_multiBodyX
Constraint impulse, which is an output of MLCP solving.
btConstraintArray m_tmpSolverNonContactConstraintPool
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btVectorXu m_lo
Lower bound of constraint impulse, m_x.
btVector3 m_contactNormal2
btMultiBodyJacobianData m_data
btMatrixXu m_scratchJ3
Cache variable for constraint Jacobian matrix.
virtual btConstraintSolverType getSolverType() const
Returns the constraint solver type.