b7901c773c
Stop include Bullet headers using `-isystem` for GCC/Clang as it misleads SCons into not properly rebuilding all files when headers change. This means we also need to make sure Bullet builds without warning, and current version fares fairly well, there were just a couple to fix (patch included). Increase minimum version for distro packages to 2.90 (this was never released as the "next" version after 2.89 was 3.05... but that covers it too).
620 lines
20 KiB
C++
620 lines
20 KiB
C++
/*
|
|
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;
|
|
//Minv.printMatrix("Minv=");
|
|
{
|
|
{
|
|
BT_PROFILE("J*Minv");
|
|
tmp = J * Minv;
|
|
}
|
|
{
|
|
BT_PROFILE("J*tmp");
|
|
m_A = tmp * J_transpose;
|
|
}
|
|
}
|
|
//J.printMatrix("J");
|
|
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;
|
|
}
|