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, 0 insertions, 639 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp deleted file mode 100644 index 8f54c52626..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ /dev/null @@ -1,639 +0,0 @@ -/* -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; -} - - |