mirror of https://github.com/axmolengine/axmol.git
621 lines
20 KiB
C++
621 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;
|
|
|
|
{
|
|
{
|
|
BT_PROFILE("J*Minv");
|
|
tmp = J * Minv;
|
|
}
|
|
{
|
|
BT_PROFILE("J*tmp");
|
|
m_A = tmp * J_transpose;
|
|
}
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
// add cfm to the diagonal of m_A
|
|
for (int i = 0; i < m_A.rows(); ++i)
|
|
{
|
|
m_A.setElem(i, i, m_A(i, i) + 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;
|
|
}
|