btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
btCollisionObject** bodies,
int numBodies,
btPersistentManifold** manifoldPtr,
int numManifolds,
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer)
{
// 1. Setup for rigid-bodies
btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
// 2. Setup for multi-bodies
// a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
// b. Set the index array for frictional contact constraints, m_limitDependencies
{
BT_PROFILE("gather constraint data");
int dindex = 0;
const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
m_allConstraintPtrArray.resize(0);
m_multiBodyAllConstraintPtrArray.resize(0);
// i. Setup for rigid bodies
m_limitDependencies.resize(numRigidBodyConstraints);
for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
{
m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
int firstContactConstraintOffset = dindex;
// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
if (interleaveContactAndFriction)
{
for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
{
const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
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);
}
// ii. Setup for multibodies
dindex = 0;
m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
}
firstContactConstraintOffset = dindex;
// The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
if (interleaveContactAndFriction)
{
for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
{
const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
m_multiBodyLimitDependencies[dindex++] = findex;
if (numtiBodyNumFrictionPerContact == 2)
{
btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
m_multiBodyLimitDependencies[dindex++] = findex;
}
}
}
else
{
for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = -1;
}
for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
{
m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
}
}
if (!m_multiBodyAllConstraintPtrArray.size())
{
m_multiBodyA.resize(0, 0);
m_multiBodyB.resize(0);
m_multiBodyX.resize(0);
m_multiBodyLo.resize(0);
m_multiBodyHi.resize(0);
}
}
// Construct MLCP terms
{
BT_PROFILE("createMLCPFast");
createMLCPFast(infoGlobal);
}
return btScalar(0);
}
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_allConstraintArray.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_allConstraintArray.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_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
m_allConstraintArray.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_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
}
}
} else
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
}
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
}
}
if (!m_allConstraintArray.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;
}
