diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 395 |
1 files changed, 188 insertions, 207 deletions
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 8f54c52626..5d864f2757 100644 --- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -19,10 +19,9 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolveProjectedGaussSeidel.h" - -btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) -:m_solver(solver), -m_fallback(0) +btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver) + : m_solver(solver), + m_fallback(0) { } @@ -33,67 +32,65 @@ btMLCPSolver::~btMLCPSolver() bool gUseMatrixMultiply = false; bool interleaveContactAndFriction = false; -btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, numBodiesUnUsed, manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); { BT_PROFILE("gather constraint data"); - int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2; - + int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2; - // int numBodies = m_tmpSolverBodyPool.size(); + // int numBodies = m_tmpSolverBodyPool.size(); m_allConstraintPtrArray.resize(0); - m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); - btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size()); - // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); + m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size()); + btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size()); + // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); int dindex = 0; - for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } - + ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead - - int firstContactConstraintOffset=dindex; + + int firstContactConstraintOffset = dindex; if (interleaveContactAndFriction) { - for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); - int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); - m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; - if (numFrictionPerContact==2) + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); + int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; + if (numFrictionPerContact == 2) { - m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); - m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); + m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; } } - } else + } + else { - for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); m_limitDependencies[dindex++] = -1; } - for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++) + for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++) { m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); - m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; + m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset; } - } - if (!m_allConstraintPtrArray.size()) { - m_A.resize(0,0); + m_A.resize(0, 0); m_b.resize(0); m_x.resize(0); m_lo.resize(0); @@ -102,7 +99,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, } } - if (gUseMatrixMultiply) { BT_PROFILE("createMLCP"); @@ -121,7 +117,7 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) { bool result = true; - if (m_A.rows()==0) + if (m_A.rows() == 0) return true; //if using split impulse, we solve 2 separate (M)LCPs @@ -129,28 +125,26 @@ bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) { btMatrixXu Acopy = m_A; btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; -// printf("solve first LCP\n"); - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + // printf("solve first LCP\n"); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); if (result) - result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); - - } else + result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); + } + else { - result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations ); + result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations); } return result; } struct btJointNode { - int jointIndex; // pointer to enclosing dxJoint object - int otherBodyIndex; // *other* body this joint is connected to - int nextJointNodeIndex;//-1 for null + int jointIndex; // pointer to enclosing dxJoint object + int otherBodyIndex; // *other* body this joint is connected to + int nextJointNodeIndex; //-1 for null int constraintRowIndex; }; - - void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int numContactRows = interleaveContactAndFriction ? 3 : 1; @@ -163,36 +157,36 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_bSplit.resize(numConstraintRows); m_b.setZero(); m_bSplit.setZero(); - for (int i=0;i<numConstraintRows ;i++) + for (int i = 0; i < numConstraintRows; i++) { btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; if (!btFuzzyZero(jacDiag)) { btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; - m_b[i]=rhs/jacDiag; - m_bSplit[i] = rhsPenetration/jacDiag; + m_b[i] = rhs / jacDiag; + m_bSplit[i] = rhsPenetration / jacDiag; } - } } -// btScalar* w = 0; -// int nub = 0; + // btScalar* w = 0; + // int nub = 0; m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - + { BT_PROFILE("init lo/ho"); - for (int i=0;i<numConstraintRows;i++) + for (int i = 0; i < numConstraintRows; i++) { - if (0)//m_limitDependencies[i]>=0) + if (0) //m_limitDependencies[i]>=0) { m_lo[i] = -BT_INFINITY; m_hi[i] = BT_INFINITY; - } else + } + else { m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; @@ -201,48 +195,48 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) } // - int m=m_allConstraintPtrArray.size(); + int m = m_allConstraintPtrArray.size(); int numBodies = m_tmpSolverBodyPool.size(); btAlignedObjectArray<int> bodyJointNodeArray; { BT_PROFILE("bodyJointNodeArray.resize"); - bodyJointNodeArray.resize(numBodies,-1); + bodyJointNodeArray.resize(numBodies, -1); } btAlignedObjectArray<btJointNode> jointNodeArray; { BT_PROFILE("jointNodeArray.reserve"); - jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); } - btMatrixXu& J3 = m_scratchJ3; + btMatrixXu& J3 = m_scratchJ3; { BT_PROFILE("J3.resize"); - J3.resize(2*m,8); + J3.resize(2 * m, 8); } - btMatrixXu& JinvM3 = m_scratchJInvM3; + btMatrixXu& JinvM3 = m_scratchJInvM3; { BT_PROFILE("JinvM3.resize/setZero"); - JinvM3.resize(2*m,8); + JinvM3.resize(2 * m, 8); JinvM3.setZero(); J3.setZero(); } - int cur=0; + int cur = 0; int rowOffset = 0; - btAlignedObjectArray<int>& ofs = m_scratchOfs; + btAlignedObjectArray<int>& ofs = m_scratchOfs; { BT_PROFILE("ofs resize"); ofs.resize(0); ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); - } + } { BT_PROFILE("Compute J and JinvM"); - int c=0; + int c = 0; int numRows = 0; - for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) { ofs[c] = rowOffset; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; @@ -250,14 +244,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; - numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; if (orgBodyA) { { - int slotA=-1; + int slotA = -1; //find free jointNode slot for sbA - slotA =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); + slotA = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); int prevSlot = bodyJointNodeArray[sbA]; bodyJointNodeArray[sbA] = slotA; jointNodeArray[slotA].nextJointNodeIndex = prevSlot; @@ -265,35 +259,35 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray[slotA].constraintRowIndex = i; jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; } - for (int row=0;row<numRows;row++,cur++) + for (int row = 0; row < numRows; row++, cur++) { - btVector3 normalInvMass = m_allConstraintPtrArray[i+row]->m_contactNormal1 * orgBodyA->getInvMass(); - btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); + btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); + btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal1[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMass[r]); - JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMass[r]); + JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } - } else + } + else { cur += numRows; } if (orgBodyB) { - { - int slotB=-1; + int slotB = -1; //find free jointNode slot for sbA - slotB =jointNodeArray.size(); - jointNodeArray.expand();//NonInitializing(); + slotB = jointNodeArray.size(); + jointNodeArray.expand(); //NonInitializing(); int prevSlot = bodyJointNodeArray[sbB]; bodyJointNodeArray[sbB] = slotB; jointNodeArray[slotB].nextJointNodeIndex = prevSlot; @@ -302,78 +296,74 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) jointNodeArray[slotB].constraintRowIndex = i; } - for (int row=0;row<numRows;row++,cur++) + for (int row = 0; row < numRows; row++, cur++) { - btVector3 normalInvMassB = m_allConstraintPtrArray[i+row]->m_contactNormal2*orgBodyB->getInvMass(); - btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); + btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); + btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); - for (int r=0;r<3;r++) + for (int r = 0; r < 3; r++) { - J3.setElem(cur,r,m_allConstraintPtrArray[i+row]->m_contactNormal2[r]); - J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); - JinvM3.setElem(cur,r,normalInvMassB[r]); - JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); + J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); + J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); + JinvM3.setElem(cur, r, normalInvMassB[r]); + JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); } - J3.setElem(cur,3,0); - JinvM3.setElem(cur,3,0); - J3.setElem(cur,7,0); - JinvM3.setElem(cur,7,0); + J3.setElem(cur, 3, 0); + JinvM3.setElem(cur, 3, 0); + J3.setElem(cur, 7, 0); + JinvM3.setElem(cur, 7, 0); } } else { cur += numRows; } - rowOffset+=numRows; - + rowOffset += numRows; } - } - //compute JinvM = J*invM. const btScalar* JinvM = JinvM3.getBufferPointer(); const btScalar* Jptr = J3.getBufferPointer(); { BT_PROFILE("m_A.resize"); - m_A.resize(n,n); + m_A.resize(n, n); } - + { BT_PROFILE("m_A.setZero"); m_A.setZero(); } - int c=0; + int c = 0; { int numRows = 0; BT_PROFILE("Compute A"); - for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++) { int row__ = ofs[c]; int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows; - numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; { int startJointNodeA = bodyJointNodeArray[sbA]; - while (startJointNodeA>=0) + while (startJointNodeA >= 0) { int j0 = jointNodeArray[startJointNodeA].jointIndex; int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; - if (j0<c) + if (j0 < c) { - int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8*numRowsOther : 0; + size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0; //printf("%d joint i %d and j0: %d: ",count++,i,j0); - m_A.multiplyAdd2_p8r ( JinvMrow, - Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); + m_A.multiplyAdd2_p8r(JinvMrow, + Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); } startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; } @@ -381,17 +371,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) { int startJointNodeB = bodyJointNodeArray[sbB]; - while (startJointNodeB>=0) + while (startJointNodeB >= 0) { int j1 = jointNodeArray[startJointNodeB].jointIndex; int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; - if (j1<c) + if (j1 < c) { - int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; - size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; - m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, - Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); + int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; + size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, + Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); } startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; } @@ -402,27 +392,25 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) BT_PROFILE("compute diagonal"); // compute diagonal blocks of m_A - int row__ = 0; + int row__ = 0; int numJointRows = m_allConstraintPtrArray.size(); - int jj=0; - for (;row__<numJointRows;) + int jj = 0; + for (; row__ < numJointRows;) { - //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; - // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; + // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows; - - const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; - const btScalar *Jrow = Jptr + 2*8*(size_t)row__; - m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); - if (orgBodyB) + const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__; + const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; + m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); + if (orgBodyB) { - m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__); } row__ += infom; jj++; @@ -433,12 +421,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) if (1) { // add cfm to the diagonal of m_A - for ( int i=0; i<m_A.rows(); ++i) + for (int i = 0; i < m_A.rows(); ++i) { - m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep); + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); } } - + ///fill the upper triangle of the matrix, to make it symmetric { BT_PROFILE("fill the upper triangle "); @@ -450,21 +438,21 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) m_x.resize(numConstraintRows); m_xSplit.resize(numConstraintRows); - if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - m_x[i]=c.m_appliedImpulse; + m_x[i] = c.m_appliedImpulse; m_xSplit[i] = c.m_appliedPushImpulse; } - } else + } + else { m_x.setZero(); m_xSplit.setZero(); } } - } void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) @@ -475,120 +463,116 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) m_b.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) m_bSplit.resize(numConstraintRows); - + m_bSplit.setZero(); m_b.setZero(); - for (int i=0;i<numConstraintRows ;i++) + for (int i = 0; i < numConstraintRows; i++) { if (m_allConstraintPtrArray[i]->m_jacDiagABInv) { - m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv; if (infoGlobal.m_splitImpulse) - m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv; + m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv; } } - - btMatrixXu& Minv = m_scratchMInv; - Minv.resize(6*numBodies,6*numBodies); + + btMatrixXu& Minv = m_scratchMInv; + Minv.resize(6 * numBodies, 6 * numBodies); Minv.setZero(); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { const btSolverBody& rb = m_tmpSolverBodyPool[i]; const btVector3& invMass = rb.m_invMass; - setElem(Minv,i*6+0,i*6+0,invMass[0]); - setElem(Minv,i*6+1,i*6+1,invMass[1]); - setElem(Minv,i*6+2,i*6+2,invMass[2]); + setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]); + setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]); + setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]); btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody; - - for (int r=0;r<3;r++) - for (int c=0;c<3;c++) - setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0); + + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0); } - - btMatrixXu& J = m_scratchJ; - J.resize(numConstraintRows,6*numBodies); + + btMatrixXu& J = m_scratchJ; + J.resize(numConstraintRows, 6 * numBodies); J.setZero(); - + m_lo.resize(numConstraintRows); m_hi.resize(numConstraintRows); - - for (int i=0;i<numConstraintRows;i++) - { + for (int i = 0; i < numConstraintRows; i++) + { m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; - + int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { - setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); - setElem(J,i,6*bodyIndex0+1,m_allConstraintPtrArray[i]->m_contactNormal1[1]); - setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); - setElem(J,i,6*bodyIndex0+3,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); - setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); - setElem(J,i,6*bodyIndex0+5,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); + setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]); + setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]); + setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]); + setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); + setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); + setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { - setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); - setElem(J,i,6*bodyIndex1+1,m_allConstraintPtrArray[i]->m_contactNormal2[1]); - setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); - setElem(J,i,6*bodyIndex1+3,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); - setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); - setElem(J,i,6*bodyIndex1+5,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); + setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]); + setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]); + setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]); + setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); + setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); + setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); } } - - btMatrixXu& J_transpose = m_scratchJTranspose; - J_transpose= J.transpose(); - btMatrixXu& tmp = m_scratchTmp; + btMatrixXu& J_transpose = m_scratchJTranspose; + J_transpose = J.transpose(); + + btMatrixXu& tmp = m_scratchTmp; { { BT_PROFILE("J*Minv"); - tmp = J*Minv; - + tmp = J * Minv; } { BT_PROFILE("J*tmp"); - m_A = tmp*J_transpose; + m_A = tmp * J_transpose; } } if (1) { // add cfm to the diagonal of m_A - for ( int i=0; i<m_A.rows(); ++i) + for (int i = 0; i < m_A.rows(); ++i) { - m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep); + m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); } } m_x.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) m_xSplit.resize(numConstraintRows); -// m_x.setZero(); + // m_x.setZero(); - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { const btSolverConstraint& c = *m_allConstraintPtrArray[i]; - m_x[i]=c.m_appliedImpulse; + m_x[i] = c.m_appliedImpulse; if (infoGlobal.m_splitImpulse) m_xSplit[i] = c.m_appliedPushImpulse; } - } - -btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { bool result = true; { BT_PROFILE("solveMLCP"); -// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); + // printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); result = solveMLCP(infoGlobal); } @@ -596,44 +580,41 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod if (result) { BT_PROFILE("process MLCP results"); - for (int i=0;i<m_allConstraintPtrArray.size();i++) + for (int i = 0; i < m_allConstraintPtrArray.size(); i++) { { btSolverConstraint& c = *m_allConstraintPtrArray[i]; int sbA = c.m_solverBodyIdA; int sbB = c.m_solverBodyIdB; //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; - // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; - + { - btScalar deltaImpulse = m_x[i]-c.m_appliedImpulse; + btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse; c.m_appliedImpulse = m_x[i]; - solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } if (infoGlobal.m_splitImpulse) { btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; - solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); c.m_appliedPushImpulse = m_xSplit[i]; } - } } } else { - // printf("m_fallback = %d\n",m_fallback); + // printf("m_fallback = %d\n",m_fallback); m_fallback++; - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); } return 0.f; } - - |