diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp')
| -rw-r--r-- | thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 639 |
1 files changed, 639 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp new file mode 100644 index 0000000000..8f54c52626 --- /dev/null +++ b/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -0,0 +1,639 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#include "btMLCPSolver.h" +#include "LinearMath/btMatrixX.h" +#include "LinearMath/btQuickprof.h" +#include "btSolveProjectedGaussSeidel.h" + + +btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) +:m_solver(solver), +m_fallback(0) +{ +} + +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) +{ + 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 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()); + + int dindex = 0; + 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; + + if (interleaveContactAndFriction) + { + 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+1]); + m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; + } + } + } else + { + 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++) + { + m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]); + m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset; + } + + } + + + if (!m_allConstraintPtrArray.size()) + { + m_A.resize(0,0); + m_b.resize(0); + m_x.resize(0); + m_lo.resize(0); + m_hi.resize(0); + return 0.f; + } + } + + + if (gUseMatrixMultiply) + { + BT_PROFILE("createMLCP"); + createMLCP(infoGlobal); + } + else + { + BT_PROFILE("createMLCPFast"); + createMLCPFast(infoGlobal); + } + + return 0.f; +} + +bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) +{ + bool result = true; + + if (m_A.rows()==0) + return true; + + //if using split impulse, we solve 2 separate (M)LCPs + if (infoGlobal.m_splitImpulse) + { + 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 ); + if (result) + 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 ); + } + 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 constraintRowIndex; +}; + + + +void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) +{ + int numContactRows = interleaveContactAndFriction ? 3 : 1; + + int numConstraintRows = m_allConstraintPtrArray.size(); + int n = numConstraintRows; + { + BT_PROFILE("init b (rhs)"); + m_b.resize(numConstraintRows); + m_bSplit.resize(numConstraintRows); + m_b.setZero(); + m_bSplit.setZero(); + 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; + } + + } + } + +// 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++) + { + if (0)//m_limitDependencies[i]>=0) + { + m_lo[i] = -BT_INFINITY; + m_hi[i] = BT_INFINITY; + } else + { + m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; + m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; + } + } + } + + // + int m=m_allConstraintPtrArray.size(); + + int numBodies = m_tmpSolverBodyPool.size(); + btAlignedObjectArray<int> bodyJointNodeArray; + { + BT_PROFILE("bodyJointNodeArray.resize"); + bodyJointNodeArray.resize(numBodies,-1); + } + btAlignedObjectArray<btJointNode> jointNodeArray; + { + BT_PROFILE("jointNodeArray.reserve"); + jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); + } + + btMatrixXu& J3 = m_scratchJ3; + { + BT_PROFILE("J3.resize"); + J3.resize(2*m,8); + } + btMatrixXu& JinvM3 = m_scratchJInvM3; + { + BT_PROFILE("JinvM3.resize/setZero"); + + JinvM3.resize(2*m,8); + JinvM3.setZero(); + J3.setZero(); + } + int cur=0; + int rowOffset = 0; + 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 numRows = 0; + + for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++) + { + ofs[c] = rowOffset; + 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; + + numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + if (orgBodyA) + { + { + int slotA=-1; + //find free jointNode slot for sbA + slotA =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbA]; + bodyJointNodeArray[sbA] = slotA; + jointNodeArray[slotA].nextJointNodeIndex = prevSlot; + jointNodeArray[slotA].jointIndex = c; + jointNodeArray[slotA].constraintRowIndex = i; + jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; + } + 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(); + + 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,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } else + { + cur += numRows; + } + if (orgBodyB) + { + + { + int slotB=-1; + //find free jointNode slot for sbA + slotB =jointNodeArray.size(); + jointNodeArray.expand();//NonInitializing(); + int prevSlot = bodyJointNodeArray[sbB]; + bodyJointNodeArray[sbB] = slotB; + jointNodeArray[slotB].nextJointNodeIndex = prevSlot; + jointNodeArray[slotB].jointIndex = c; + jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; + jointNodeArray[slotB].constraintRowIndex = i; + } + + 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(); + + 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,3,0); + JinvM3.setElem(cur,3,0); + J3.setElem(cur,7,0); + JinvM3.setElem(cur,7,0); + } + } + else + { + cur += 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); + } + + { + BT_PROFILE("m_A.setZero"); + m_A.setZero(); + } + int c=0; + { + int numRows = 0; + BT_PROFILE("Compute A"); + 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; + + numRows = i<m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows ; + + const btScalar *JinvMrow = JinvM + 2*8*(size_t)row__; + + { + int startJointNodeA = bodyJointNodeArray[sbA]; + while (startJointNodeA>=0) + { + int j0 = jointNodeArray[startJointNodeA].jointIndex; + int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; + 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; + //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]); + } + startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; + } + } + + { + int startJointNodeB = bodyJointNodeArray[sbB]; + while (startJointNodeB>=0) + { + int j1 = jointNodeArray[startJointNodeB].jointIndex; + int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; + + 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]); + } + startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; + } + } + } + + { + BT_PROFILE("compute diagonal"); + // compute diagonal blocks of m_A + + int row__ = 0; + int numJointRows = m_allConstraintPtrArray.size(); + + 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* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody; + + + 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) + { + m_A.multiplyAdd2_p8r (JinvMrow + 8*(size_t)infom, Jrow + 8*(size_t)infom, infom, infom, row__,row__); + } + row__ += infom; + jj++; + } + } + } + + if (1) + { + // add cfm to the diagonal of m_A + for ( int i=0; i<m_A.rows(); ++i) + { + 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 "); + m_A.copyLowerToUpperTriangle(); + } + + { + BT_PROFILE("resize/init x"); + m_x.resize(numConstraintRows); + m_xSplit.resize(numConstraintRows); + + if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING) + { + for (int i=0;i<m_allConstraintPtrArray.size();i++) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + m_x[i]=c.m_appliedImpulse; + m_xSplit[i] = c.m_appliedPushImpulse; + } + } else + { + m_x.setZero(); + m_xSplit.setZero(); + } + } + +} + +void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) +{ + int numBodies = this->m_tmpSolverBodyPool.size(); + int numConstraintRows = m_allConstraintPtrArray.size(); + + 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++) + { + if (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; + } + } + + btMatrixXu& Minv = m_scratchMInv; + Minv.resize(6*numBodies,6*numBodies); + Minv.setZero(); + 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]); + 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); + } + + 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++) + { + + 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]); + } + 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]); + } + } + + btMatrixXu& J_transpose = m_scratchJTranspose; + J_transpose= J.transpose(); + + btMatrixXu& tmp = m_scratchTmp; + + { + { + BT_PROFILE("J*Minv"); + tmp = J*Minv; + + } + { + BT_PROFILE("J*tmp"); + m_A = tmp*J_transpose; + } + } + + if (1) + { + // add cfm to the diagonal of m_A + 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_x.resize(numConstraintRows); + if (infoGlobal.m_splitImpulse) + m_xSplit.resize(numConstraintRows); +// m_x.setZero(); + + for (int i=0;i<m_allConstraintPtrArray.size();i++) + { + const btSolverConstraint& c = *m_allConstraintPtrArray[i]; + 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) +{ + bool result = true; + { + BT_PROFILE("solveMLCP"); +// printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols()); + result = solveMLCP(infoGlobal); + } + + //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations + if (result) + { + BT_PROFILE("process MLCP results"); + 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; + + btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; + + { + 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); + } + + 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); + c.m_appliedPushImpulse = m_xSplit[i]; + } + + } + } + } + else + { + // printf("m_fallback = %d\n",m_fallback); + m_fallback++; + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + } + + return 0.f; +} + + |