Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

Changeset View

Standalone View

View Options

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;
} }