59 int firstContactConstraintOffset = dindex;
70 if (numFrictionPerContact == 2)
153 int n = numConstraintRows;
156 m_b.resize(numConstraintRows);
160 for (
int i = 0; i < numConstraintRows; i++)
167 m_b[i] = rhs / jacDiag;
168 m_bSplit[i] = rhsPenetration / jacDiag;
176 m_lo.resize(numConstraintRows);
177 m_hi.resize(numConstraintRows);
182 for (
int i = 0; i < numConstraintRows; i++)
200 int numBodies = m_tmpSolverBodyPool.size();
221 JinvM3.resize(2 * m, 8);
244 btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
245 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
253 slotA = jointNodeArray.
size();
255 int prevSlot = bodyJointNodeArray[sbA];
256 bodyJointNodeArray[sbA] = slotA;
257 jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
258 jointNodeArray[slotA].jointIndex =
c;
259 jointNodeArray[slotA].constraintRowIndex = i;
260 jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
262 for (
int row = 0; row < numRows; row++, cur++)
267 for (
int r = 0;
r < 3;
r++)
271 JinvM3.setElem(cur,
r, normalInvMass[
r]);
272 JinvM3.setElem(cur,
r + 4, relPosCrossNormalInvInertia[
r]);
274 J3.setElem(cur, 3, 0);
275 JinvM3.setElem(cur, 3, 0);
276 J3.setElem(cur, 7, 0);
277 JinvM3.setElem(cur, 7, 0);
289 slotB = jointNodeArray.
size();
291 int prevSlot = bodyJointNodeArray[sbB];
292 bodyJointNodeArray[sbB] = slotB;
293 jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
294 jointNodeArray[slotB].jointIndex =
c;
295 jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
296 jointNodeArray[slotB].constraintRowIndex = i;
299 for (
int row = 0; row < numRows; row++, cur++)
304 for (
int r = 0;
r < 3;
r++)
308 JinvM3.setElem(cur,
r, normalInvMassB[
r]);
309 JinvM3.setElem(cur,
r + 4, relPosInvInertiaB[
r]);
311 J3.setElem(cur, 3, 0);
312 JinvM3.setElem(cur, 3, 0);
313 J3.setElem(cur, 7, 0);
314 JinvM3.setElem(cur, 7, 0);
321 rowOffset += numRows;
326 const btScalar* JinvM = JinvM3.getBufferPointer();
328 const btScalar* Jptr = J3.getBufferPointer();
352 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
355 int startJointNodeA = bodyJointNodeArray[sbA];
356 while (startJointNodeA >= 0)
358 int j0 = jointNodeArray[startJointNodeA].jointIndex;
359 int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
365 m_A.multiplyAdd2_p8r(JinvMrow,
366 Jptr + 2 * 8 * (
size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
368 startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
373 int startJointNodeB = bodyJointNodeArray[sbB];
374 while (startJointNodeB >= 0)
376 int j1 = jointNodeArray[startJointNodeB].jointIndex;
377 int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
383 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)numRows,
384 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
386 startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
399 for (; row__ < numJointRows;)
404 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
408 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
409 const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
410 m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
413 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
424 for (
int i = 0; i <
m_A.rows(); ++i)
433 m_A.copyLowerToUpperTriangle();
438 m_x.resize(numConstraintRows);
446 m_x[i] =
c.m_appliedImpulse;
460 int numBodies = this->m_tmpSolverBodyPool.size();
463 m_b.resize(numConstraintRows);
470 for (
int i = 0; i < numConstraintRows; i++)
487 setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]);
488 setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]);
489 setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]);
490 btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
492 for (
int r = 0;
r < 3;
r++)
493 for (
int c = 0;
c < 3;
c++)
498 J.resize(numConstraintRows, 6 *
numBodies);
501 m_lo.resize(numConstraintRows);
502 m_hi.resize(numConstraintRows);
504 for (
int i = 0; i < numConstraintRows; i++)
532 J_transpose = J.transpose();
543 m_A = tmp * J_transpose;
550 for (
int i = 0; i <
m_A.rows(); ++i)
556 m_x.resize(numConstraintRows);
564 m_x[i] =
c.m_appliedImpulse;
587 int sbA =
c.m_solverBodyIdA;
588 int sbB =
c.m_solverBodyIdB;
597 c.m_appliedImpulse =
m_x[i];
598 solverBodyA.internalApplyImpulse(
c.m_contactNormal1 * solverBodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
599 solverBodyB.internalApplyImpulse(
c.m_contactNormal2 * solverBodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
605 solverBodyA.internalApplyPushImpulse(
c.m_contactNormal1 * solverBodyA.internalGetInvMass(),
c.m_angularComponentA, deltaImpulse);
606 solverBodyB.internalApplyPushImpulse(
c.m_contactNormal2 * solverBodyB.internalGetInvMass(),
c.m_angularComponentB, deltaImpulse);
_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
bool interleaveContactAndFriction
void setElem(btMatrixXd &mat, int row, int col, double val)
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...
btRigidBody * m_originalBody
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_scratchJTranspose
btAlignedObjectArray< int > m_limitDependencies
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
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
btScalar getInvMass() const
const btMatrix3x3 & getInvInertiaTensorWorld() const