bullet: Update to version 2.82 (bullet revision 2705)

    
Remove patch that has been applied upstream.

Fixes several bugs.

1 files changed, 626 insertions, 0 deletions

diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
new file mode 100644
index 00000000000..8888d3961f5
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -0,0 +1,626 @@
+/*
+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_allConstraintArray.resize(0);
+		m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
+		btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size()+m_tmpSolverContactConstraintPool.size()+m_tmpSolverContactFrictionConstraintPool.size());
+	//	printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
+
+		int dindex = 0;
+		for (int i=0;i<m_tmpSolverNonContactConstraintPool.size();i++)
+		{
+			m_allConstraintArray.push_back(m_tmpSolverNonContactConstraintPool[i]);
+			m_limitDependencies[dindex++] = -1;
+		}
+ 
+		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
+		
+		int firstContactConstraintOffset=dindex;
+
+		if (interleaveContactAndFriction)
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
+				if (numFrictionPerContact==2)
+				{
+					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
+				}
+			}
+		} else
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+			}
+			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
+			{
+				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
+			}
+			
+		}
+
+
+		if (!m_allConstraintArray.size())
+		{
+			m_A.resize(0,0);
+			m_b.resize(0);
+			m_x.resize(0);
+			m_lo.resize(0);
+			m_hi.resize(0);
+			return 0.f;
+		}
+	}
+
+	
+	if (gUseMatrixMultiply)
+	{
+		BT_PROFILE("createMLCP");
+		createMLCP(infoGlobal);
+	}
+	else
+	{
+		BT_PROFILE("createMLCPFast");
+		createMLCPFast(infoGlobal);
+	}
+
+	return 0.f;
+}
+
+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_allConstraintArray.size();
+	int n = numConstraintRows;
+	{
+		BT_PROFILE("init b (rhs)");
+		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
+		//m_b.setZero();
+		for (int i=0;i<numConstraintRows ;i++)
+		{
+			if (m_allConstraintArray[i].m_jacDiagABInv)
+			{
+				m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+			}
+
+		}
+	}
+
+	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_allConstraintArray[i].m_lowerLimit;
+				m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+			}
+		}
+	}
+
+	//
+	int m=m_allConstraintArray.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_allConstraintArray.size());
+	}
+
+	static btMatrixXu J3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2*m,8);
+	}
+	static btMatrixXu JinvM3;
+	{
+		BT_PROFILE("JinvM3.resize/setZero");
+
+		JinvM3.resize(2*m,8);
+		JinvM3.setZero();
+		J3.setZero();
+	}
+	int cur=0;
+	int rowOffset = 0;
+	static btAlignedObjectArray<int> ofs;
+	{
+		BT_PROFILE("ofs resize");
+		ofs.resize(0);
+		ofs.resizeNoInitialize(m_allConstraintArray.size());
+	}				
+	{
+		BT_PROFILE("Compute J and JinvM");
+		int c=0;
+
+		int numRows = 0;
+
+		for (int i=0;i<m_allConstraintArray.size();i+=numRows,c++)
+		{
+			ofs[c] = rowOffset;
+			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;
+
+			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_allConstraintArray[i+row].m_contactNormal1 *		orgBodyA->getInvMass();
+					btVector3 relPosCrossNormalInvInertia = m_allConstraintArray[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]);
+						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_allConstraintArray[i+row].m_contactNormal2*orgBodyB->getInvMass();
+					btVector3 relPosInvInertiaB = m_allConstraintArray[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]);
+						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_allConstraintArray.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;
+
+			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_allConstraintArray[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_allConstraintArray[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_allConstraintArray.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;
+				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++;
+			}
+		}
+	}
+
+	///todo: use proper cfm values from the constraints (getInfo2)
+	if (1)
+	{
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			float cfm = 0.00001f;
+			m_A.setElem(i,i,m_A(i,i)+ cfm / 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_allConstraintArray.size();i++)
+			{
+				const btSolverConstraint& c = m_allConstraintArray[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_allConstraintArray.size();
+
+	m_b.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_bSplit.resize(numConstraintRows);
+ 
+	for (int i=0;i<numConstraintRows ;i++)
+	{
+		if (m_allConstraintArray[i].m_jacDiagABInv)
+		{
+			m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+			if (infoGlobal.m_splitImpulse)
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+		}
+	}
+ 
+	static btMatrixXu Minv;
+	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);
+	}
+ 
+	static btMatrixXu J;
+	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_allConstraintArray[i].m_lowerLimit;
+		m_hi[i] = m_allConstraintArray[i].m_upperLimit;
+ 
+		int bodyIndex0 = m_allConstraintArray[i].m_solverBodyIdA;
+		int bodyIndex1 = m_allConstraintArray[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]);
+		}
+		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]);
+		}
+	}
+ 
+	static btMatrixXu J_transpose;
+	J_transpose= J.transpose();
+
+	static btMatrixXu tmp;
+
+	{
+		{
+			BT_PROFILE("J*Minv");
+			tmp = J*Minv;
+
+		}
+		{
+			BT_PROFILE("J*tmp");
+			m_A = tmp*J_transpose;
+		}
+	}
+
+	if (1)
+	{
+		///todo: use proper cfm values from the constraints (getInfo2)
+		// add cfm to the diagonal of m_A
+		for ( int i=0; i<m_A.rows(); ++i) 
+		{
+			float cfm = 0.0001f;
+			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+		}
+	}
+
+	m_x.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_xSplit.resize(numConstraintRows);
+//	m_x.setZero();
+
+	for (int i=0;i<m_allConstraintArray.size();i++)
+	{
+		const btSolverConstraint& c = m_allConstraintArray[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_allConstraintArray.size();i++)
+		{
+			{
+				btSolverConstraint& c = m_allConstraintArray[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];
+ 
+				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]);
+				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]);
+					c.m_appliedPushImpulse = m_xSplit[i];
+				}
+				c.m_appliedImpulse = m_x[i];
+			}
+		}
+	}
+	else
+	{
+		m_fallback++;
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	}
+
+	return 0.f;
+}
\ No newline at end of file