61 int firstContactConstraintOffset=dindex;
72 if (numFrictionPerContact==2)
159 int n = numConstraintRows;
162 m_b.resize(numConstraintRows);
166 for (
int i=0;i<numConstraintRows ;i++)
174 m_bSplit[i] = rhsPenetration/jacDiag;
183 m_lo.resize(numConstraintRows);
184 m_hi.resize(numConstraintRows);
189 for (
int i=0;i<numConstraintRows;i++)
210 bodyJointNodeArray.
resize(numBodies,-1);
227 JinvM3.resize(2*m,8);
259 slotA =jointNodeArray.
size();
261 int prevSlot = bodyJointNodeArray[sbA];
262 bodyJointNodeArray[sbA] = slotA;
263 jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
264 jointNodeArray[slotA].jointIndex = c;
265 jointNodeArray[slotA].constraintRowIndex = i;
266 jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
268 for (
int row=0;row<numRows;row++,cur++)
273 for (
int r=0;r<3;r++)
277 JinvM3.setElem(cur,r,normalInvMass[r]);
278 JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]);
281 JinvM3.setElem(cur,3,0);
283 JinvM3.setElem(cur,7,0);
295 slotB =jointNodeArray.
size();
297 int prevSlot = bodyJointNodeArray[sbB];
298 bodyJointNodeArray[sbB] = slotB;
299 jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
300 jointNodeArray[slotB].jointIndex = c;
301 jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
302 jointNodeArray[slotB].constraintRowIndex = i;
305 for (
int row=0;row<numRows;row++,cur++)
310 for (
int r=0;r<3;r++)
314 JinvM3.setElem(cur,r,normalInvMassB[r]);
315 JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]);
318 JinvM3.setElem(cur,3,0);
320 JinvM3.setElem(cur,7,0);
335 const btScalar* JinvM = JinvM3.getBufferPointer();
337 const btScalar* Jptr = J3.getBufferPointer();
361 const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
364 int startJointNodeA = bodyJointNodeArray[sbA];
365 while (startJointNodeA>=0)
367 int j0 = jointNodeArray[startJointNodeA].jointIndex;
368 int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
375 m_A.multiplyAdd2_p8r ( JinvMrow,
376 Jptr + 2*8*(
size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]);
378 startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
383 int startJointNodeB = bodyJointNodeArray[sbB];
384 while (startJointNodeB>=0)
386 int j1 = jointNodeArray[startJointNodeB].jointIndex;
387 int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
393 m_A.multiplyAdd2_p8r ( JinvMrow + 8*(
size_t)numRows,
394 Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
396 startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
409 for (;row__<numJointRows;)
420 const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__;
421 const btScalar *Jrow = Jptr + 2*8*(size_t)row__;
422 m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__);
425 m_A.multiplyAdd2_p8r (JinvMrow + 8*(
size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__);
436 for (
int i=0; i<
m_A.rows(); ++i)
445 m_A.copyLowerToUpperTriangle();
450 m_x.resize(numConstraintRows);
475 m_b.resize(numConstraintRows);
482 for (
int i=0;i<numConstraintRows ;i++)
493 Minv.resize(6*numBodies,6*numBodies);
495 for (
int i=0;i<numBodies;i++)
499 setElem(Minv,i*6+0,i*6+0,invMass[0]);
500 setElem(Minv,i*6+1,i*6+1,invMass[1]);
501 setElem(Minv,i*6+2,i*6+2,invMass[2]);
504 for (
int r=0;r<3;r++)
505 for (
int c=0;c<3;c++)
510 J.resize(numConstraintRows,6*numBodies);
513 m_lo.resize(numConstraintRows);
514 m_hi.resize(numConstraintRows);
516 for (
int i=0;i<numConstraintRows;i++)
545 J_transpose= J.transpose();
557 m_A = tmp*J_transpose;
564 for (
int i=0; i<
m_A.rows(); ++i)
570 m_x.resize(numConstraintRows);
bool interleaveContactAndFriction
void setElem(btMatrixXd &mat, int row, int col, double val)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
bool btFuzzyZero(btScalar x)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
T & expand(const T &fillValue=T())
btCollisionObject can be used to manage collision detection objects.
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
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_scratchJTranspose
btAlignedObjectArray< int > m_limitDependencies
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
btMatrixXu m_scratchJ3
The following scratch variables are not stateful – contents are cleared prior to each use.
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
btVectorXu m_bSplit
when using 'split impulse' we solve two separate (M)LCPs
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< int > m_scratchOfs
btMatrixXu m_scratchJInvM3
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btMLCPSolver(btMLCPSolverInterface *solver)
original version written by Erwin Coumans, October 2013
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
btMLCPSolverInterface * m_solver
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
The btRigidBody is the main class for rigid body objects.
btScalar getInvMass() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
btConstraintArray m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btConstraintArray m_tmpSolverNonContactConstraintPool
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btVector3 can be used to represent 3D points and vectors.
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
const btVector3 & internalGetInvMass() const
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
btVector3 m_angularComponentB
btSimdScalar m_appliedImpulse
btVector3 m_angularComponentA
btSimdScalar m_appliedPushImpulse
btVector3 m_contactNormal1