Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / MLCPSolvers / btMLCPSolver.cpp
index 0635b958bed9a93f7ab74a9994e8b419e0f3e560..6688694a92828ea8c11bc7f2f747f0f6d9a3f912 100644 (file)
@@ -44,8 +44,8 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
                int numFrictionPerContact = m_tmpSolverContactConstraintPool.size()==m_tmpSolverContactFrictionConstraintPool.size()? 1 : 2;
 
 
-               int numBodies = m_tmpSolverBodyPool.size();
-               m_allConstraintArray.resize(0);
+       //      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());
@@ -53,7 +53,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
                int dindex = 0;
                for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
                {
-                       m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
+                       m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
                        m_limitDependencies[dindex++] = -1;
                }
  
@@ -65,14 +65,14 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
                {
                        for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
                        {
-                               m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+                               m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
                                m_limitDependencies[dindex++] = -1;
-                               m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+                               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_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+                                       m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
                                        m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
                                }
                        }
@@ -80,19 +80,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
                {
                        for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
                        {
-                               m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+                               m_allConstraintPtrArray.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_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
                                m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
                        }
                        
                }
 
 
-               if (!m_allConstraintArray.size())
+               if (!m_allConstraintPtrArray.size())
                {
                        m_A.resize(0,0);
                        m_b.resize(0);
@@ -156,7 +156,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 {
        int numContactRows = interleaveContactAndFriction ? 3 : 1;
 
-       int numConstraintRows = m_allConstraintArray.size();
+       int numConstraintRows = m_allConstraintPtrArray.size();
        int n = numConstraintRows;
        {
                BT_PROFILE("init b (rhs)");
@@ -166,11 +166,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                m_bSplit.setZero();
                for (int i=0;i<numConstraintRows ;i++)
                {
-                       btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
+                       btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
                        if (!btFuzzyZero(jacDiag))
                        {
-                               btScalar rhs = m_allConstraintArray[i].m_rhs;
-                               btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
+                               btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
+                               btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
                                m_b[i]=rhs/jacDiag;
                                m_bSplit[i] = rhsPenetration/jacDiag;
                        }
@@ -178,8 +178,8 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                }
        }
 
-       btScalar* w = 0;
-       int nub = 0;
+//     btScalar* w = 0;
+//     int nub = 0;
 
        m_lo.resize(numConstraintRows);
        m_hi.resize(numConstraintRows);
@@ -195,14 +195,14 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                                m_hi[i] = BT_INFINITY;
                        } else
                        {
-                               m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-                               m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+                               m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+                               m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
                        }
                }
        }
 
        //
-       int m=m_allConstraintArray.size();
+       int m=m_allConstraintPtrArray.size();
 
        int numBodies = m_tmpSolverBodyPool.size();
        btAlignedObjectArray<int> bodyJointNodeArray;
@@ -213,7 +213,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
        btAlignedObjectArray<btJointNode> jointNodeArray;
        {
                BT_PROFILE("jointNodeArray.reserve");
-               jointNodeArray.reserve(2*m_allConstraintArray.size());
+               jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
        }
 
        static btMatrixXu J3;
@@ -235,7 +235,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
        {
                BT_PROFILE("ofs resize");
                ofs.resize(0);
-               ofs.resizeNoInitialize(m_allConstraintArray.size());
+               ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
        }                               
        {
                BT_PROFILE("Compute J and JinvM");
@@ -243,11 +243,11 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
                int numRows = 0;
 
-               for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
+               for (int i=0;i<m_allConstraintPtrArray.size();i+=numRows,c++)
                {
                        ofs[c] = rowOffset;
-                       int sbA = m_allConstraintArray[i].m_solverBodyIdA;
-                       int sbB = m_allConstraintArray[i].m_solverBodyIdB;
+                       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;
 
@@ -268,13 +268,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                                }
                                for (int row=0;row<numRows;row++,cur++)
                                {
-                                       btVector3 normalInvMass =                               m_allConstraintArray[i+row].m_contactNormal1 *          orgBodyA->getInvMass();
-                                       btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[i+row].m_relpos1CrossNormal *      orgBodyA->getInvInertiaTensorWorld();
+                                       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_allConstraintArray[i+row].m_contactNormal1[r]);
-                                               J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos1CrossNormal[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]);
                                        }
@@ -305,13 +305,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
                                for (int row=0;row<numRows;row++,cur++)
                                {
-                                       btVector3 normalInvMassB = m_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
-                                       btVector3 relPosInvInertiaB = m_allConstraintArray[i+row].m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
+                                       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_allConstraintArray[i+row].m_contactNormal2[r]);
-                                               J3.setElem(cur,r+4,m_allConstraintArray[i+row].m_relpos2CrossNormal[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]);
                                        }
@@ -349,13 +349,13 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
        {
                int numRows = 0;
                BT_PROFILE("Compute A");
-               for (int i=0;i<m_allConstraintArray.size();i+= numRows,c++)
+               for (int i=0;i<m_allConstraintPtrArray.size();i+= numRows,c++)
                {
                        int row__ = ofs[c];
-                       int sbA = m_allConstraintArray[i].m_solverBodyIdA;
-                       int sbB = m_allConstraintArray[i].m_solverBodyIdB;
-                       btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
-                       btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
+                       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 ;
                                        
@@ -371,7 +371,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                                        {
                                                                 
                                                int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
-                                               size_t ofsother = (m_allConstraintArray[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);
                                                m_A.multiplyAdd2_p8r ( JinvMrow, 
                                                Jptr + 2*8*(size_t)ofs[j0] + ofsother, numRows, numRowsOther,  row__,ofs[j0]);
@@ -390,7 +390,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                                        if (j1<c)
                                        {
                                                int numRowsOther =  cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
-                                               size_t ofsother = (m_allConstraintArray[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, 
                                                Jptr + 2*8*(size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__,ofs[j1]);
                                        }
@@ -404,15 +404,15 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
                        // compute diagonal blocks of m_A
 
                        int  row__ = 0;
-                       int numJointRows = m_allConstraintArray.size();
+                       int numJointRows = m_allConstraintPtrArray.size();
 
                        int jj=0;
                        for (;row__<numJointRows;)
                        {
 
-                               int sbA = m_allConstraintArray[row__].m_solverBodyIdA;
-                               int sbB = m_allConstraintArray[row__].m_solverBodyIdB;
-                               btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+                               //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;
 
 
@@ -453,9 +453,9 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 
                if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
                {
-                       for (int i=0;i<m_allConstraintArray.size();i++)
+                       for (int i=0;i<m_allConstraintPtrArray.size();i++)
                        {
-                               const btSolverConstraint& c = m_allConstraintArray[i];
+                               const btSolverConstraint& c = *m_allConstraintPtrArray[i];
                                m_x[i]=c.m_appliedImpulse;
                                m_xSplit[i] = c.m_appliedPushImpulse;
                        }
@@ -471,7 +471,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 {
        int numBodies = this->m_tmpSolverBodyPool.size();
-       int numConstraintRows = m_allConstraintArray.size();
+       int numConstraintRows = m_allConstraintPtrArray.size();
 
        m_b.resize(numConstraintRows);
        if (infoGlobal.m_splitImpulse)
@@ -482,11 +482,11 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
        for (int i=0;i<numConstraintRows ;i++)
        {
-               if (m_allConstraintArray[i].m_jacDiagABInv)
+               if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
                {
-                       m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+                       m_b[i]=m_allConstraintPtrArray[i]->m_rhs/m_allConstraintPtrArray[i]->m_jacDiagABInv;
                        if (infoGlobal.m_splitImpulse)
-                               m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+                               m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration/m_allConstraintPtrArray[i]->m_jacDiagABInv;
                }
        }
  
@@ -517,28 +517,28 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
        for (int i=0;i<numConstraintRows;i++)
        {
 
-               m_lo[i] = m_allConstraintArray[i].m_lowerLimit;
-               m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+               m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
+               m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
  
-               int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
-               int bodyIndex1 = m_allConstraintArray[i].m_solverBodyIdB;
+               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_allConstraintArray[i].m_contactNormal1[0]);
-                       setElem(J,i,6*bodyIndex0+1,m_allConstraintArray[i].m_contactNormal1[1]);
-                       setElem(J,i,6*bodyIndex0+2,m_allConstraintArray[i].m_contactNormal1[2]);
-                       setElem(J,i,6*bodyIndex0+3,m_allConstraintArray[i].m_relpos1CrossNormal[0]);
-                       setElem(J,i,6*bodyIndex0+4,m_allConstraintArray[i].m_relpos1CrossNormal[1]);
-                       setElem(J,i,6*bodyIndex0+5,m_allConstraintArray[i].m_relpos1CrossNormal[2]);
+                       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_allConstraintArray[i].m_contactNormal2[0]);
-                       setElem(J,i,6*bodyIndex1+1,m_allConstraintArray[i].m_contactNormal2[1]);
-                       setElem(J,i,6*bodyIndex1+2,m_allConstraintArray[i].m_contactNormal2[2]);
-                       setElem(J,i,6*bodyIndex1+3,m_allConstraintArray[i].m_relpos2CrossNormal[0]);
-                       setElem(J,i,6*bodyIndex1+4,m_allConstraintArray[i].m_relpos2CrossNormal[1]);
-                       setElem(J,i,6*bodyIndex1+5,m_allConstraintArray[i].m_relpos2CrossNormal[2]);
+                       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]);
                }
        }
  
@@ -573,9 +573,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
                m_xSplit.resize(numConstraintRows);
 //     m_x.setZero();
 
-       for (int i=0;i<m_allConstraintArray.size();i++)
+       for (int i=0;i<m_allConstraintPtrArray.size();i++)
        {
-               const btSolverConstraint& c = m_allConstraintArray[i];
+               const btSolverConstraint& c = *m_allConstraintPtrArray[i];
                m_x[i]=c.m_appliedImpulse;
                if (infoGlobal.m_splitImpulse)
                        m_xSplit[i] = c.m_appliedPushImpulse;
@@ -597,27 +597,33 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
        if (result)
        {
                BT_PROFILE("process MLCP results");
-               for (int i=0;i<m_allConstraintArray.size();i++)
+               for (int i=0;i<m_allConstraintPtrArray.size();i++)
                {
                        {
-                               btSolverConstraint& c = m_allConstraintArray[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;
+                               //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
+                       //      btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 
                                btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
                                btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
  
-                               solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
-                               solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
+                               {
+                                       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)
                                {
-                                       solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
-                                       solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
+                                       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];
                                }
-                               c.m_appliedImpulse = m_x[i];
+                               
                        }
                }
        }
@@ -629,4 +635,6 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
        }
 
        return 0.f;
-}
\ No newline at end of file
+}
+
+