btMLCPSolver.cpp

Go to the documentation of this file.

/*
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.
*/
 
#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;
                }
 
                
                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);
                }
        }
                                   
        {
                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;
}