void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) { int numBodies = this->m_tmpSolverBodyPool.size(); int numConstraintRows = m_allConstraintArray.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_allConstraintArray[i].m_jacDiagABInv) { m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv; if (infoGlobal.m_splitImpulse) m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv; } } static btMatrixXu Minv; 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); } static btMatrixXu J; 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_allConstraintArray[i].m_lowerLimit; m_hi[i] = m_allConstraintArray[i].m_upperLimit; int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA; int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB; if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) { setElem(J,i,6*bodyIndex0+0,m_allConstraintArray[i].m_contactNormal1[0]); setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]); setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]); setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]); setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]); setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]); } if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) { setElem(J,i,6*bodyIndex1+0,m_allConstraintArray[i].m_contactNormal2[0]); setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]); setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]); setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]); setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]); setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]); } } static btMatrixXu J_transpose; J_transpose= J.transpose(); static btMatrixXu tmp; { { 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)+ m_cfm / infoGlobal.m_timeStep); } } m_x.resize(numConstraintRows); if (infoGlobal.m_splitImpulse) m_xSplit.resize(numConstraintRows); // m_x.setZero(); for (int i=0;i<m_allConstraintArray.size();i++) { const btSolverConstraint& c = m_allConstraintArray[i]; m_x[i]=c.m_appliedImpulse; if (infoGlobal.m_splitImpulse) m_xSplit[i] = c.m_appliedPushImpulse; } }