Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
| Show All 13 Lines | |||||
| */ | */ | ||||
| ///original version written by Erwin Coumans, October 2013 | ///original version written by Erwin Coumans, October 2013 | ||||
| #include "btMLCPSolver.h" | #include "btMLCPSolver.h" | ||||
| #include "LinearMath/btMatrixX.h" | #include "LinearMath/btMatrixX.h" | ||||
| #include "LinearMath/btQuickprof.h" | #include "LinearMath/btQuickprof.h" | ||||
| #include "btSolveProjectedGaussSeidel.h" | #include "btSolveProjectedGaussSeidel.h" | ||||
| btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) | btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver) | ||||
| :m_solver(solver), | : m_solver(solver), | ||||
| m_fallback(0), | m_fallback(0) | ||||
| m_cfm(0.000001)//0.0000001 | |||||
| { | { | ||||
| } | } | ||||
| btMLCPSolver::~btMLCPSolver() | btMLCPSolver::~btMLCPSolver() | ||||
| { | { | ||||
| } | } | ||||
| bool gUseMatrixMultiply = false; | bool gUseMatrixMultiply = false; | ||||
| bool interleaveContactAndFriction = 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"); | 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_allConstraintPtrArray.resize(0); | ||||
| m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.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()); | btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size()); | ||||
| // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); | // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size()); | ||||
| int dindex = 0; | 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_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]); | ||||
| m_limitDependencies[dindex++] = -1; | m_limitDependencies[dindex++] = -1; | ||||
| } | } | ||||
| ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead | ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead | ||||
| int firstContactConstraintOffset=dindex; | int firstContactConstraintOffset = dindex; | ||||
| if (interleaveContactAndFriction) | 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_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); | ||||
| m_limitDependencies[dindex++] = -1; | m_limitDependencies[dindex++] = -1; | ||||
| m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]); | m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]); | ||||
| int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact)); | int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact)); | ||||
| m_limitDependencies[dindex++] = findex +firstContactConstraintOffset; | m_limitDependencies[dindex++] = findex + firstContactConstraintOffset; | ||||
| if (numFrictionPerContact==2) | if (numFrictionPerContact == 2) | ||||
| { | { | ||||
| m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]); | m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]); | ||||
| m_limitDependencies[dindex++] = findex+firstContactConstraintOffset; | 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_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]); | ||||
| m_limitDependencies[dindex++] = -1; | 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_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()) | if (!m_allConstraintPtrArray.size()) | ||||
| { | { | ||||
| m_A.resize(0,0); | m_A.resize(0, 0); | ||||
| m_b.resize(0); | m_b.resize(0); | ||||
| m_x.resize(0); | m_x.resize(0); | ||||
| m_lo.resize(0); | m_lo.resize(0); | ||||
| m_hi.resize(0); | m_hi.resize(0); | ||||
| return 0.f; | return 0.f; | ||||
| } | } | ||||
| } | } | ||||
| if (gUseMatrixMultiply) | if (gUseMatrixMultiply) | ||||
| { | { | ||||
| BT_PROFILE("createMLCP"); | BT_PROFILE("createMLCP"); | ||||
| createMLCP(infoGlobal); | createMLCP(infoGlobal); | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| BT_PROFILE("createMLCPFast"); | BT_PROFILE("createMLCPFast"); | ||||
| createMLCPFast(infoGlobal); | createMLCPFast(infoGlobal); | ||||
| } | } | ||||
| return 0.f; | return 0.f; | ||||
| } | } | ||||
| bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) | bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal) | ||||
| { | { | ||||
| bool result = true; | bool result = true; | ||||
| if (m_A.rows()==0) | if (m_A.rows() == 0) | ||||
| return true; | return true; | ||||
| //if using split impulse, we solve 2 separate (M)LCPs | //if using split impulse, we solve 2 separate (M)LCPs | ||||
| if (infoGlobal.m_splitImpulse) | if (infoGlobal.m_splitImpulse) | ||||
| { | { | ||||
| btMatrixXu Acopy = m_A; | btMatrixXu Acopy = m_A; | ||||
| btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; | btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies; | ||||
| // printf("solve first LCP\n"); | // printf("solve first LCP\n"); | ||||
| 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); | ||||
| if (result) | if (result) | ||||
| result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations ); | result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations); | ||||
| } | |||||
| } else | 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; | return result; | ||||
| } | } | ||||
| struct btJointNode | struct btJointNode | ||||
| { | { | ||||
| int jointIndex; // pointer to enclosing dxJoint object | int jointIndex; // pointer to enclosing dxJoint object | ||||
| int otherBodyIndex; // *other* body this joint is connected to | int otherBodyIndex; // *other* body this joint is connected to | ||||
| int nextJointNodeIndex;//-1 for null | int nextJointNodeIndex; //-1 for null | ||||
| int constraintRowIndex; | int constraintRowIndex; | ||||
| }; | }; | ||||
| void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) | void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) | ||||
| { | { | ||||
| int numContactRows = interleaveContactAndFriction ? 3 : 1; | int numContactRows = interleaveContactAndFriction ? 3 : 1; | ||||
| int numConstraintRows = m_allConstraintPtrArray.size(); | int numConstraintRows = m_allConstraintPtrArray.size(); | ||||
| int n = numConstraintRows; | int n = numConstraintRows; | ||||
| { | { | ||||
| BT_PROFILE("init b (rhs)"); | BT_PROFILE("init b (rhs)"); | ||||
| m_b.resize(numConstraintRows); | m_b.resize(numConstraintRows); | ||||
| m_bSplit.resize(numConstraintRows); | m_bSplit.resize(numConstraintRows); | ||||
| m_b.setZero(); | m_b.setZero(); | ||||
| m_bSplit.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; | btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv; | ||||
| if (!btFuzzyZero(jacDiag)) | if (!btFuzzyZero(jacDiag)) | ||||
| { | { | ||||
| btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; | btScalar rhs = m_allConstraintPtrArray[i]->m_rhs; | ||||
| btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; | btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration; | ||||
| m_b[i]=rhs/jacDiag; | m_b[i] = rhs / jacDiag; | ||||
| m_bSplit[i] = rhsPenetration/jacDiag; | m_bSplit[i] = rhsPenetration / jacDiag; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| // btScalar* w = 0; | // btScalar* w = 0; | ||||
| // int nub = 0; | // int nub = 0; | ||||
| m_lo.resize(numConstraintRows); | m_lo.resize(numConstraintRows); | ||||
| m_hi.resize(numConstraintRows); | m_hi.resize(numConstraintRows); | ||||
| { | { | ||||
| BT_PROFILE("init lo/ho"); | 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_lo[i] = -BT_INFINITY; | ||||
| m_hi[i] = BT_INFINITY; | m_hi[i] = BT_INFINITY; | ||||
| } else | } | ||||
| else | |||||
| { | { | ||||
| m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; | m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; | ||||
| m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; | m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| // | // | ||||
| int m=m_allConstraintPtrArray.size(); | int m = m_allConstraintPtrArray.size(); | ||||
| int numBodies = m_tmpSolverBodyPool.size(); | int numBodies = m_tmpSolverBodyPool.size(); | ||||
| btAlignedObjectArray<int> bodyJointNodeArray; | btAlignedObjectArray<int> bodyJointNodeArray; | ||||
| { | { | ||||
| BT_PROFILE("bodyJointNodeArray.resize"); | BT_PROFILE("bodyJointNodeArray.resize"); | ||||
| bodyJointNodeArray.resize(numBodies,-1); | bodyJointNodeArray.resize(numBodies, -1); | ||||
| } | } | ||||
| btAlignedObjectArray<btJointNode> jointNodeArray; | btAlignedObjectArray<btJointNode> jointNodeArray; | ||||
| { | { | ||||
| BT_PROFILE("jointNodeArray.reserve"); | BT_PROFILE("jointNodeArray.reserve"); | ||||
| jointNodeArray.reserve(2*m_allConstraintPtrArray.size()); | jointNodeArray.reserve(2 * m_allConstraintPtrArray.size()); | ||||
| } | } | ||||
| static btMatrixXu J3; | btMatrixXu& J3 = m_scratchJ3; | ||||
| { | { | ||||
| BT_PROFILE("J3.resize"); | BT_PROFILE("J3.resize"); | ||||
| J3.resize(2*m,8); | J3.resize(2 * m, 8); | ||||
| } | } | ||||
| static btMatrixXu JinvM3; | btMatrixXu& JinvM3 = m_scratchJInvM3; | ||||
| { | { | ||||
| BT_PROFILE("JinvM3.resize/setZero"); | BT_PROFILE("JinvM3.resize/setZero"); | ||||
| JinvM3.resize(2*m,8); | JinvM3.resize(2 * m, 8); | ||||
| JinvM3.setZero(); | JinvM3.setZero(); | ||||
| J3.setZero(); | J3.setZero(); | ||||
| } | } | ||||
| int cur=0; | int cur = 0; | ||||
| int rowOffset = 0; | int rowOffset = 0; | ||||
| static btAlignedObjectArray<int> ofs; | btAlignedObjectArray<int>& ofs = m_scratchOfs; | ||||
| { | { | ||||
| BT_PROFILE("ofs resize"); | BT_PROFILE("ofs resize"); | ||||
| ofs.resize(0); | ofs.resize(0); | ||||
| ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); | ofs.resizeNoInitialize(m_allConstraintPtrArray.size()); | ||||
| } | } | ||||
| { | { | ||||
| BT_PROFILE("Compute J and JinvM"); | BT_PROFILE("Compute J and JinvM"); | ||||
| int c=0; | int c = 0; | ||||
| int numRows = 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; | ofs[c] = rowOffset; | ||||
| int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; | int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; | ||||
| int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; | int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; | ||||
| btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; | btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; | ||||
| btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].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) | if (orgBodyA) | ||||
| { | { | ||||
| { | { | ||||
| int slotA=-1; | int slotA = -1; | ||||
| //find free jointNode slot for sbA | //find free jointNode slot for sbA | ||||
| slotA =jointNodeArray.size(); | slotA = jointNodeArray.size(); | ||||
| jointNodeArray.expand();//NonInitializing(); | jointNodeArray.expand(); //NonInitializing(); | ||||
| int prevSlot = bodyJointNodeArray[sbA]; | int prevSlot = bodyJointNodeArray[sbA]; | ||||
| bodyJointNodeArray[sbA] = slotA; | bodyJointNodeArray[sbA] = slotA; | ||||
| jointNodeArray[slotA].nextJointNodeIndex = prevSlot; | jointNodeArray[slotA].nextJointNodeIndex = prevSlot; | ||||
| jointNodeArray[slotA].jointIndex = c; | jointNodeArray[slotA].jointIndex = c; | ||||
| jointNodeArray[slotA].constraintRowIndex = i; | jointNodeArray[slotA].constraintRowIndex = i; | ||||
| jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1; | 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 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass(); | ||||
| btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld(); | 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, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]); | ||||
| J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos1CrossNormal[r]); | J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]); | ||||
| JinvM3.setElem(cur,r,normalInvMass[r]); | JinvM3.setElem(cur, r, normalInvMass[r]); | ||||
| JinvM3.setElem(cur,r+4,relPosCrossNormalInvInertia[r]); | JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]); | ||||
| } | } | ||||
| J3.setElem(cur,3,0); | J3.setElem(cur, 3, 0); | ||||
| JinvM3.setElem(cur,3,0); | JinvM3.setElem(cur, 3, 0); | ||||
| J3.setElem(cur,7,0); | J3.setElem(cur, 7, 0); | ||||
| JinvM3.setElem(cur,7,0); | JinvM3.setElem(cur, 7, 0); | ||||
| } | } | ||||
| } else | } | ||||
| else | |||||
| { | { | ||||
| cur += numRows; | cur += numRows; | ||||
| } | } | ||||
| if (orgBodyB) | if (orgBodyB) | ||||
| { | { | ||||
| { | { | ||||
| int slotB=-1; | int slotB = -1; | ||||
| //find free jointNode slot for sbA | //find free jointNode slot for sbA | ||||
| slotB =jointNodeArray.size(); | slotB = jointNodeArray.size(); | ||||
| jointNodeArray.expand();//NonInitializing(); | jointNodeArray.expand(); //NonInitializing(); | ||||
| int prevSlot = bodyJointNodeArray[sbB]; | int prevSlot = bodyJointNodeArray[sbB]; | ||||
| bodyJointNodeArray[sbB] = slotB; | bodyJointNodeArray[sbB] = slotB; | ||||
| jointNodeArray[slotB].nextJointNodeIndex = prevSlot; | jointNodeArray[slotB].nextJointNodeIndex = prevSlot; | ||||
| jointNodeArray[slotB].jointIndex = c; | jointNodeArray[slotB].jointIndex = c; | ||||
| jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; | jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1; | ||||
| jointNodeArray[slotB].constraintRowIndex = i; | 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 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass(); | ||||
| btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld(); | 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, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]); | ||||
| J3.setElem(cur,r+4,m_allConstraintPtrArray[i+row]->m_relpos2CrossNormal[r]); | J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]); | ||||
| JinvM3.setElem(cur,r,normalInvMassB[r]); | JinvM3.setElem(cur, r, normalInvMassB[r]); | ||||
| JinvM3.setElem(cur,r+4,relPosInvInertiaB[r]); | JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]); | ||||
| } | } | ||||
| J3.setElem(cur,3,0); | J3.setElem(cur, 3, 0); | ||||
| JinvM3.setElem(cur,3,0); | JinvM3.setElem(cur, 3, 0); | ||||
| J3.setElem(cur,7,0); | J3.setElem(cur, 7, 0); | ||||
| JinvM3.setElem(cur,7,0); | JinvM3.setElem(cur, 7, 0); | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| cur += numRows; | cur += numRows; | ||||
| } | } | ||||
| rowOffset+=numRows; | rowOffset += numRows; | ||||
| } | } | ||||
| } | } | ||||
| //compute JinvM = J*invM. | //compute JinvM = J*invM. | ||||
| const btScalar* JinvM = JinvM3.getBufferPointer(); | const btScalar* JinvM = JinvM3.getBufferPointer(); | ||||
| const btScalar* Jptr = J3.getBufferPointer(); | const btScalar* Jptr = J3.getBufferPointer(); | ||||
| { | { | ||||
| BT_PROFILE("m_A.resize"); | BT_PROFILE("m_A.resize"); | ||||
| m_A.resize(n,n); | m_A.resize(n, n); | ||||
| } | } | ||||
| { | { | ||||
| BT_PROFILE("m_A.setZero"); | BT_PROFILE("m_A.setZero"); | ||||
| m_A.setZero(); | m_A.setZero(); | ||||
| } | } | ||||
| int c=0; | int c = 0; | ||||
| { | { | ||||
| int numRows = 0; | int numRows = 0; | ||||
| BT_PROFILE("Compute A"); | 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 row__ = ofs[c]; | ||||
| int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; | int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA; | ||||
| int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; | int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB; | ||||
| // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; | // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; | ||||
| // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].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]; | int startJointNodeA = bodyJointNodeArray[sbA]; | ||||
| while (startJointNodeA>=0) | while (startJointNodeA >= 0) | ||||
| { | { | ||||
| int j0 = jointNodeArray[startJointNodeA].jointIndex; | int j0 = jointNodeArray[startJointNodeA].jointIndex; | ||||
| int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; | int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex; | ||||
| if (j0<c) | if (j0 < c) | ||||
| { | { | ||||
| int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows; | 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); | //printf("%d joint i %d and j0: %d: ",count++,i,j0); | ||||
| m_A.multiplyAdd2_p8r ( JinvMrow, | m_A.multiplyAdd2_p8r(JinvMrow, | ||||
| Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__,ofs[j0]); | Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]); | ||||
| } | } | ||||
| startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; | startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex; | ||||
| } | } | ||||
| } | } | ||||
| { | { | ||||
| int startJointNodeB = bodyJointNodeArray[sbB]; | int startJointNodeB = bodyJointNodeArray[sbB]; | ||||
| while (startJointNodeB>=0) | while (startJointNodeB >= 0) | ||||
| { | { | ||||
| int j1 = jointNodeArray[startJointNodeB].jointIndex; | int j1 = jointNodeArray[startJointNodeB].jointIndex; | ||||
| int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; | int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex; | ||||
| if (j1<c) | if (j1 < c) | ||||
| { | { | ||||
| int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; | int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows; | ||||
| size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8*numRowsOther : 0; | size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0; | ||||
| m_A.multiplyAdd2_p8r ( JinvMrow + 8*(size_t)numRows, | m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows, | ||||
| Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]); | Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]); | ||||
| } | } | ||||
| startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; | startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| { | { | ||||
| BT_PROFILE("compute diagonal"); | BT_PROFILE("compute diagonal"); | ||||
| // compute diagonal blocks of m_A | // compute diagonal blocks of m_A | ||||
| int row__ = 0; | int row__ = 0; | ||||
| int numJointRows = m_allConstraintPtrArray.size(); | int numJointRows = m_allConstraintPtrArray.size(); | ||||
| int jj=0; | int jj = 0; | ||||
| for (;row__<numJointRows;) | for (; row__ < numJointRows;) | ||||
| { | { | ||||
| //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; | //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA; | ||||
| int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB; | 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; | 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* JinvMrow = JinvM + 2 * 8 * (size_t)row__; | ||||
| const btScalar *Jrow = Jptr + 2*8*(size_t)row__; | const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__; | ||||
| m_A.multiply2_p8r (JinvMrow, Jrow, infom, infom, row__,row__); | m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__); | ||||
| if (orgBodyB) | 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; | row__ += infom; | ||||
| jj++; | jj++; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| if (1) | if (1) | ||||
| { | { | ||||
| // add cfm to the diagonal of m_A | // 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)+ m_cfm / 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 | ///fill the upper triangle of the matrix, to make it symmetric | ||||
| { | { | ||||
| BT_PROFILE("fill the upper triangle "); | BT_PROFILE("fill the upper triangle "); | ||||
| m_A.copyLowerToUpperTriangle(); | m_A.copyLowerToUpperTriangle(); | ||||
| } | } | ||||
| { | { | ||||
| BT_PROFILE("resize/init x"); | BT_PROFILE("resize/init x"); | ||||
| m_x.resize(numConstraintRows); | m_x.resize(numConstraintRows); | ||||
| m_xSplit.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]; | const btSolverConstraint& c = *m_allConstraintPtrArray[i]; | ||||
| m_x[i]=c.m_appliedImpulse; | m_x[i] = c.m_appliedImpulse; | ||||
| m_xSplit[i] = c.m_appliedPushImpulse; | m_xSplit[i] = c.m_appliedPushImpulse; | ||||
| } | } | ||||
| } else | } | ||||
| else | |||||
| { | { | ||||
| m_x.setZero(); | m_x.setZero(); | ||||
| m_xSplit.setZero(); | m_xSplit.setZero(); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) | void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal) | ||||
| { | { | ||||
| int numBodies = this->m_tmpSolverBodyPool.size(); | int numBodies = this->m_tmpSolverBodyPool.size(); | ||||
| int numConstraintRows = m_allConstraintPtrArray.size(); | int numConstraintRows = m_allConstraintPtrArray.size(); | ||||
| m_b.resize(numConstraintRows); | m_b.resize(numConstraintRows); | ||||
| if (infoGlobal.m_splitImpulse) | if (infoGlobal.m_splitImpulse) | ||||
| m_bSplit.resize(numConstraintRows); | m_bSplit.resize(numConstraintRows); | ||||
| m_bSplit.setZero(); | m_bSplit.setZero(); | ||||
| m_b.setZero(); | m_b.setZero(); | ||||
| for (int i=0;i<numConstraintRows ;i++) | for (int i = 0; i < numConstraintRows; i++) | ||||
| { | { | ||||
| if (m_allConstraintPtrArray[i]->m_jacDiagABInv) | 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) | 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; | ||||
| } | } | ||||
| } | } | ||||
| static btMatrixXu Minv; | btMatrixXu& Minv = m_scratchMInv; | ||||
| Minv.resize(6*numBodies,6*numBodies); | Minv.resize(6 * numBodies, 6 * numBodies); | ||||
| Minv.setZero(); | Minv.setZero(); | ||||
| for (int i=0;i<numBodies;i++) | for (int i = 0; i < numBodies; i++) | ||||
| { | { | ||||
| const btSolverBody& rb = m_tmpSolverBodyPool[i]; | const btSolverBody& rb = m_tmpSolverBodyPool[i]; | ||||
| const btVector3& invMass = rb.m_invMass; | const btVector3& invMass = rb.m_invMass; | ||||
| setElem(Minv,i*6+0,i*6+0,invMass[0]); | setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]); | ||||
| setElem(Minv,i*6+1,i*6+1,invMass[1]); | setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]); | ||||
| setElem(Minv,i*6+2,i*6+2,invMass[2]); | setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]); | ||||
| btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody; | btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody; | ||||
| for (int r=0;r<3;r++) | for (int r = 0; r < 3; r++) | ||||
| for (int c=0;c<3;c++) | for (int c = 0; c < 3; c++) | ||||
| setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0); | setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0); | ||||
| } | } | ||||
| static btMatrixXu J; | btMatrixXu& J = m_scratchJ; | ||||
| J.resize(numConstraintRows,6*numBodies); | J.resize(numConstraintRows, 6 * numBodies); | ||||
| J.setZero(); | J.setZero(); | ||||
| m_lo.resize(numConstraintRows); | m_lo.resize(numConstraintRows); | ||||
| m_hi.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_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit; | ||||
| m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; | m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit; | ||||
| int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; | int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA; | ||||
| int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; | int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB; | ||||
| if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) | if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody) | ||||
| { | { | ||||
| setElem(J,i,6*bodyIndex0+0,m_allConstraintPtrArray[i]->m_contactNormal1[0]); | 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 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]); | ||||
| setElem(J,i,6*bodyIndex0+2,m_allConstraintPtrArray[i]->m_contactNormal1[2]); | 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 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]); | ||||
| setElem(J,i,6*bodyIndex0+4,m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]); | 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 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]); | ||||
| } | } | ||||
| if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) | if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody) | ||||
| { | { | ||||
| setElem(J,i,6*bodyIndex1+0,m_allConstraintPtrArray[i]->m_contactNormal2[0]); | 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 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]); | ||||
| setElem(J,i,6*bodyIndex1+2,m_allConstraintPtrArray[i]->m_contactNormal2[2]); | 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 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]); | ||||
| setElem(J,i,6*bodyIndex1+4,m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]); | 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 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]); | ||||
| } | } | ||||
| } | } | ||||
| static btMatrixXu J_transpose; | btMatrixXu& J_transpose = m_scratchJTranspose; | ||||
| J_transpose= J.transpose(); | J_transpose = J.transpose(); | ||||
| static btMatrixXu tmp; | btMatrixXu& tmp = m_scratchTmp; | ||||
| { | { | ||||
| { | { | ||||
| BT_PROFILE("J*Minv"); | BT_PROFILE("J*Minv"); | ||||
| tmp = J*Minv; | tmp = J * Minv; | ||||
| } | } | ||||
| { | { | ||||
| BT_PROFILE("J*tmp"); | BT_PROFILE("J*tmp"); | ||||
| m_A = tmp*J_transpose; | m_A = tmp * J_transpose; | ||||
| } | } | ||||
| } | } | ||||
| if (1) | if (1) | ||||
| { | { | ||||
| // add cfm to the diagonal of m_A | // 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)+ m_cfm / infoGlobal.m_timeStep); | m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep); | ||||
| } | } | ||||
| } | } | ||||
| m_x.resize(numConstraintRows); | m_x.resize(numConstraintRows); | ||||
| if (infoGlobal.m_splitImpulse) | if (infoGlobal.m_splitImpulse) | ||||
| m_xSplit.resize(numConstraintRows); | 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]; | const btSolverConstraint& c = *m_allConstraintPtrArray[i]; | ||||
| m_x[i]=c.m_appliedImpulse; | m_x[i] = c.m_appliedImpulse; | ||||
| if (infoGlobal.m_splitImpulse) | if (infoGlobal.m_splitImpulse) | ||||
| m_xSplit[i] = c.m_appliedPushImpulse; | 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; | bool result = true; | ||||
| { | { | ||||
| BT_PROFILE("solveMLCP"); | 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); | result = solveMLCP(infoGlobal); | ||||
| } | } | ||||
| //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations | //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations | ||||
| if (result) | if (result) | ||||
| { | { | ||||
| BT_PROFILE("process MLCP results"); | 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]; | btSolverConstraint& c = *m_allConstraintPtrArray[i]; | ||||
| int sbA = c.m_solverBodyIdA; | int sbA = c.m_solverBodyIdA; | ||||
| int sbB = c.m_solverBodyIdB; | int sbB = c.m_solverBodyIdB; | ||||
| //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody; | //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& solverBodyA = m_tmpSolverBodyPool[sbA]; | ||||
| btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB]; | 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]; | c.m_appliedImpulse = m_x[i]; | ||||
| solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); | solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); | ||||
| solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); | solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); | ||||
| } | } | ||||
| if (infoGlobal.m_splitImpulse) | if (infoGlobal.m_splitImpulse) | ||||
| { | { | ||||
| btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; | btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse; | ||||
| solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); | solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); | ||||
| solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); | solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); | ||||
| c.m_appliedPushImpulse = m_xSplit[i]; | c.m_appliedPushImpulse = m_xSplit[i]; | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| else | else | ||||
| { | { | ||||
| // printf("m_fallback = %d\n",m_fallback); | // printf("m_fallback = %d\n",m_fallback); | ||||
| 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; | return 0.f; | ||||
| } | } | ||||