diff --git a/Demos/OpenGL/GlutStuff.cpp b/Demos/OpenGL/GlutStuff.cpp
index d8de3e726..95658f1f2 100644
--- a/Demos/OpenGL/GlutStuff.cpp
+++ b/Demos/OpenGL/GlutStuff.cpp
@@ -78,7 +78,7 @@ int glutmain(int argc, char **argv,int width,int height,const char* title,DemoAp
 
 	glutInit(&argc, argv);
     glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_STENCIL);
-    glutInitWindowPosition(0, 0);
+    glutInitWindowPosition(width/2, height/2);
     glutInitWindowSize(width, height);
     glutCreateWindow(title);
 #ifdef BT_USE_FREEGLUT
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index aa8b87062..590e172b2 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -152,7 +152,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 #endif
 }
 
-// Project Gauss Seidel or the equivalent Sequential Impulse
+// Projected Gauss Seidel or the equivalent Sequential Impulse
  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
@@ -355,8 +355,6 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 {
 
 	
-	solverConstraint.m_contactNormal1 = normalAxis;
-	solverConstraint.m_contactNormal2 = -normalAxis;
 	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 
@@ -372,15 +370,30 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
+	if (body0)
 	{
+		solverConstraint.m_contactNormal1 = normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
-	}
+		solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor();
+	}else
 	{
+		solverConstraint.m_contactNormal1.setZero();
+		solverConstraint.m_relpos1CrossNormal.setZero();
+		solverConstraint.m_angularComponentA .setZero();
+	}
+
+	if (body1)
+	{
+		solverConstraint.m_contactNormal2 = -normalAxis;
 		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2);
 		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_angularComponentB = body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor();
+	} else
+	{
+		solverConstraint.m_contactNormal2.setZero();
+		solverConstraint.m_relpos2CrossNormal.setZero();
+		solverConstraint.m_angularComponentB.setZero();
 	}
 
 	{
@@ -418,8 +431,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		
 	}
 }
@@ -498,8 +511,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = cfmSlip;
-		solverConstraint.m_lowerLimit = 0;
-		solverConstraint.m_upperLimit = 1e10f;
+		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
+		solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		
 	}
 }
@@ -543,7 +556,15 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 			body.setCompanionId(solverBodyIdA);
 		} else
 		{
-			return 0;//assume first one is a fixed solver body
+			
+			if (m_fixedBodyId<0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody,0);
+			}
+			return m_fixedBodyId;
+//			return 0;//assume first one is a fixed solver body
 		}
 	}
 
@@ -605,10 +626,24 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					solverConstraint.m_jacDiagABInv = denom;
 				}
 
-				solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
-				solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
-				solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-				solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+				if (rb0)
+				{
+					solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
+					solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+				} else
+				{
+					solverConstraint.m_contactNormal1.setZero();
+					solverConstraint.m_relpos1CrossNormal.setZero();
+				}
+				if (rb1)
+				{
+					solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB;
+					solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+				}else
+				{
+					solverConstraint.m_contactNormal2.setZero();
+					solverConstraint.m_relpos2CrossNormal.setZero();
+				}
 
 				btScalar restitution = 0.f;
 				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
@@ -870,6 +905,10 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 				if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 				{
 					cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
 					if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
@@ -877,17 +916,16 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
 					}
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
 				} else
 				{
 					btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 
+					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
+					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -895,9 +933,6 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					}
 
-					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
@@ -938,6 +973,7 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold**
 
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
+	m_fixedBodyId = -1;
 	BT_PROFILE("solveGroupCacheFriendlySetup");
 	(void)debugDrawer;
 
@@ -1022,8 +1058,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	m_tmpSolverBodyPool.reserve(numBodies+1);
 	m_tmpSolverBodyPool.resize(0);
 
-	btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
-    initSolverBody(&fixedBody,0);
+	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+    //initSolverBody(&fixedBody,0);
 
 	//convert all bodies
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index eb6a3313c..2da4dbca4 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -42,7 +42,7 @@ protected:
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
 	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
 	int							m_maxOverrideNumSolverIterations;
-
+	int m_fixedBodyId;
 	void setupFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
 									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
diff --git a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
index 2ade61b2f..5515e6b31 100644
--- a/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -55,6 +55,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	{
 		void*		m_originalContactPoint;
 		btScalar	m_unusedPadding4;
+		int			m_numRowsForNonContactConstraint;
 	};
 
 	int	m_overrideNumSolverIterations;
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
new file mode 100644
index 000000000..f3f8ac433
--- /dev/null
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -0,0 +1,578 @@
+/*
+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)
+{
+}
+
+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
+		
+
+
+		if (interleaveContactAndFriction)
+		{
+			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
+			{
+				int findex = dindex;
+				m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
+				m_limitDependencies[dindex++] = -1;
+				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex;//findex;
+				if (numFrictionPerContact==2)
+				{
+					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
+					m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1].m_frictionIndex;//findex;
+				}
+			}
+		} 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;
+			}
+			
+		}
+
+
+		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;
+}
+
+void btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
+{
+	 m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+
+}
+
+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_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;
+		}
+	}
+
+	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 btMatrixXf J3;
+	{
+		BT_PROFILE("J3.resize");
+		J3.resize(2*m,8);
+	}
+	static btMatrixXf 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++;
+			}
+		}
+	}
+				
+	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);
+		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;
+			}
+		} else
+		{
+			m_x.setZero();
+		}
+	}
+
+}
+
+void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
+{
+	int numBodies = this->m_tmpSolverBodyPool.size();
+	int numConstraintRows = m_allConstraintArray.size();
+
+	m_b.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;
+	}
+ 
+	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)
+	{
+		// 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);
+//	m_x.setZero();
+
+	for (int i=0;i<m_allConstraintArray.size();i++)
+	{
+		const btSolverConstraint& c = m_allConstraintArray[i];
+		m_x[i]=c.m_appliedImpulse;
+//		c.m_numRowsForNonContactConstraint
+	}
+
+}
+
+
+btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	{
+		BT_PROFILE("solveMLCP");
+//		printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
+		solveMLCP(infoGlobal);
+	}
+
+	//check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
+	{
+		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]);
+				c.m_appliedImpulse = m_x[i];
+			}
+		}
+	}
+
+
+	//btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+
+	return 0.f;
+}
\ No newline at end of file
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
new file mode 100644
index 000000000..47e2d3a7f
--- /dev/null
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -0,0 +1,61 @@
+/*
+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
+
+#ifndef BT_MLCP_SOLVER_H
+#define BT_MLCP_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btMatrixX.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
+
+class btMLCPSolver : public btSequentialImpulseConstraintSolver
+{
+
+protected:
+
+
+	btMatrixXu m_A;
+	btVectorXu m_b;
+	btVectorXu m_x;
+	btVectorXu m_lo;
+	btVectorXu m_hi;
+	
+	btAlignedObjectArray<int> m_limitDependencies;
+	btConstraintArray m_allConstraintArray;
+
+	btMLCPSolverInterface* m_solver;
+
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual void createMLCP(const btContactSolverInfo& infoGlobal);
+	virtual void createMLCPFast(const btContactSolverInfo& infoGlobal);
+
+	virtual void solveMLCP(const btContactSolverInfo& infoGlobal);
+
+public:
+
+	btMLCPSolver(	 btMLCPSolverInterface* solver);
+	virtual ~btMLCPSolver();
+
+	void setMLCPSolver(btMLCPSolverInterface* solver)
+	{
+		m_solver = solver;
+	}
+
+};
+
+
+#endif //BT_MLCP_SOLVER_H
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
new file mode 100644
index 000000000..0ceeac816
--- /dev/null
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h
@@ -0,0 +1,32 @@
+/*
+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
+
+#ifndef BT_MLCP_SOLVER_INTERFACE_H
+#define BT_MLCP_SOLVER_INTERFACE_H
+
+#include "LinearMath/btMatrixX.h"
+
+class btMLCPSolverInterface
+{
+public:
+	virtual ~btMLCPSolverInterface()
+	{
+	}
+
+	virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)=0;
+};
+
+#endif //BT_MLCP_SOLVER_INTERFACE_H
diff --git a/src/BulletDynamics/MLCPSolvers/btPATHSolver.h b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
new file mode 100644
index 000000000..bdc9c0023
--- /dev/null
+++ b/src/BulletDynamics/MLCPSolvers/btPATHSolver.h
@@ -0,0 +1,145 @@
+/*
+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
+
+
+#ifndef BT_PATH_SOLVER_H
+#define BT_PATH_SOLVER_H
+
+//#define BT_USE_PATH
+#ifdef BT_USE_PATH
+
+extern "C" {
+#include "PATH/SimpleLCP.h"
+#include "PATH/License.h"
+#include "PATH/Error_Interface.h"
+};
+  void __stdcall MyError(Void *data, Char *msg)
+{
+	printf("Path Error: %s\n",msg);
+}
+  void __stdcall MyWarning(Void *data, Char *msg)
+{
+	printf("Path Warning: %s\n",msg);
+}
+
+Error_Interface e;
+
+
+
+#include "btMLCPSolverInterface.h"
+#include "Dantzig/lcp.h"
+
+class btPathSolver : public btMLCPSolverInterface
+{
+public:
+
+	btPathSolver()
+	{
+		License_SetString("2069810742&Courtesy_License&&&USR&2013&14_12_2011&1000&PATH&GEN&31_12_2013&0_0_0&0&0_0");
+		e.error_data = 0;
+		e.warning = MyWarning;
+		e.error = MyError;
+		Error_SetInterface(&e);
+	}
+
+
+	virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		MCP_Termination status;
+		
+
+		int numVariables = b.rows();
+		if (0==numVariables)
+			return;
+
+			/*	 - variables - the number of variables in the problem
+			- m_nnz - the number of nonzeros in the M matrix
+			- m_i - a vector of size m_nnz containing the row indices for M
+			- m_j - a vector of size m_nnz containing the column indices for M
+			- m_ij - a vector of size m_nnz containing the data for M
+			- q - a vector of size variables
+			- lb - a vector of size variables containing the lower bounds on x
+			- ub - a vector of size variables containing the upper bounds on x
+			*/
+		btAlignedObjectArray<double> values;
+		btAlignedObjectArray<int> rowIndices;
+		btAlignedObjectArray<int> colIndices;
+
+		for (int i=0;i<A.rows();i++)
+		{
+			for (int j=0;j<A.cols();j++)
+			{
+				if (A(i,j)!=0.f)
+				{
+					//add 1, because Path starts at 1, instead of 0
+					rowIndices.push_back(i+1);
+					colIndices.push_back(j+1);
+					values.push_back(A(i,j));
+				}
+			}
+		}
+		int numNonZero = rowIndices.size();
+		btAlignedObjectArray<double> zResult;
+		zResult.resize(numVariables);
+		btAlignedObjectArray<double> rhs;
+		btAlignedObjectArray<double> upperBounds;
+		btAlignedObjectArray<double> lowerBounds;
+		for (int i=0;i<numVariables;i++)
+		{
+			upperBounds.push_back(hi[i]);
+			lowerBounds.push_back(lo[i]);
+			rhs.push_back(-b[i]);
+		}
+
+
+		SimpleLCP(numVariables,numNonZero,&rowIndices[0],&colIndices[0],&values[0],&rhs[0],&lowerBounds[0],&upperBounds[0], &status, &zResult[0]);
+
+		if (status != MCP_Solved)
+		{
+			static const char* gReturnMsgs[] = {
+				"Invalid return",
+				"MCP_Solved: The problem was solved",
+				"MCP_NoProgress: A stationary point was found",
+				"MCP_MajorIterationLimit: Major iteration limit met",
+				"MCP_MinorIterationLimit: Cumulative minor iteration limit met",
+				"MCP_TimeLimit: Ran out of time",
+				"MCP_UserInterrupt: Control-C, typically",
+				"MCP_BoundError: Problem has a bound error",
+				"MCP_DomainError: Could not find starting point",
+				"MCP_Infeasible: Problem has no solution",
+				"MCP_Error: An error occurred within the code",
+				"MCP_LicenseError: License could not be found",
+				"MCP_OK"
+			};
+
+			printf("ERROR: The PATH MCP solver failed: %s\n", gReturnMsgs[(unsigned int)status]);// << std::endl;
+			printf("using Projected Gauss Seidel instead\n");
+			//x = Solve_GaussSeidel(A,b,lo,hi,limitDependencies,infoGlobal.m_numIterations);
+		} else
+		{
+			for (int i=0;i<numVariables;i++)
+			{
+				x[i] = zResult[i];
+			}
+		}
+
+	}
+};
+
+#endif //BT_USE_PATH
+
+
+#endif //BT_PATH_SOLVER_H
diff --git a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
new file mode 100644
index 000000000..e374c72e9
--- /dev/null
+++ b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
@@ -0,0 +1,79 @@
+/*
+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
+
+#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
+
+
+#include "btMLCPSolverInterface.h"
+
+class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
+{
+public:
+	virtual void solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
+	{
+		//A is a m-n matrix, m rows, n columns
+		btAssert(A.rows() == b.rows());
+
+		int i, j, numRows = A.rows();
+	
+		float delta;
+
+		for (int k = 0; k <numIterations; k++)
+		{
+			for (i = 0; i <numRows; i++)
+			{
+				delta = 0.0f;
+				if (useSparsity)
+				{
+					for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
+					{
+						int j = A.m_rowNonZeroElements1[i][h];
+						if (j != i)//skip main diagonal
+						{
+							delta += A(i,j) * x[j];
+						}
+					}
+				} else
+				{
+					for (j = 0; j <i; j++) 
+						delta += A(i,j) * x[j];
+					for (j = i+1; j<numRows; j++) 
+						delta += A(i,j) * x[j];
+				}
+
+				float aDiag = A(i,i);
+				x [i] = (b [i] - delta) / A(i,i);
+				float s = 1.f;
+
+				if (limitDependency[i]>=0)
+				{
+					s = x[limitDependency[i]];
+					if (s<0)
+						s=1;
+				}
+			
+				if (x[i]<lo[i]*s)
+					x[i]=lo[i]*s;
+				if (x[i]>hi[i]*s)
+					x[i]=hi[i]*s;
+			}
+		}
+	}
+
+};
+
+#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h
new file mode 100644
index 000000000..bfe881957
--- /dev/null
+++ b/src/LinearMath/btMatrixX.h
@@ -0,0 +1,540 @@
+/*
+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
+
+#ifndef BT_MATRIX_X_H
+#define BT_MATRIX_X_H
+
+#include "LinearMath/btQuickprof.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+class btIntSortPredicate
+{
+	public:
+		bool operator() ( const int& a, const int& b ) const
+		{
+			 return a < b;
+		}
+};
+
+
+template <typename T> 
+struct btMatrixX
+{
+	int m_rows;
+	int m_cols;
+	int m_operations;
+	int m_resizeOperations;
+	int m_setElemOperations;
+
+	btAlignedObjectArray<T>	m_storage;
+	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
+	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+
+	T* getBufferPointerWritable() 
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	btMatrixX()
+		:m_rows(0),
+		m_cols(0),
+		m_operations(0),
+		m_resizeOperations(0),
+		m_setElemOperations(0)
+	{
+	}
+	btMatrixX(int rows,int cols)
+		:m_rows(rows),
+		m_cols(cols),
+		m_operations(0),
+		m_resizeOperations(0),
+		m_setElemOperations(0)
+	{
+		resize(rows,cols);
+	}
+	void resize(int rows, int cols)
+	{
+		m_resizeOperations++;
+		m_rows = rows;
+		m_cols = cols;
+		{
+			BT_PROFILE("m_storage.resize");
+			m_storage.resize(rows*cols);
+		}
+		clearSparseInfo();
+	}
+	int cols() const
+	{
+		return m_cols;
+	}
+	int rows() const
+	{
+		return m_rows;
+	}
+	///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead
+	/*T& operator() (int row,int col)
+	{
+		return m_storage[col*m_rows+row];
+	}
+	*/
+
+	void addElem(int row,int col, T val)
+	{
+		if (val)
+		{
+			if (m_storage[col+row*m_cols]==0.f)
+			{
+				setElem(row,col,val);
+			} else
+			{
+				m_storage[row*m_cols+col] += val;
+			}
+		}
+	}
+	
+	void copyLowerToUpperTriangle()
+	{
+		int count=0;
+		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		{
+			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			{
+				int col = m_rowNonZeroElements1[row][j];
+				setElem(col,row, (*this)(row,col));
+				count++;
+
+			}
+		}
+		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
+	}
+	void setElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		if (val)
+		{
+			if (m_storage[col+row*m_cols]==0.f)
+			{
+				
+
+				m_rowNonZeroElements1[row].push_back(col);
+				m_colNonZeroElements[col].push_back(row);
+				/*
+//we need to keep the m_rowNonZeroElements1/m_colNonZeroElements arrays sorted (it is too slow, so commented out)
+				int f=0;
+				int l=0;
+				m_rowNonZeroElements1[row].findBinarySearch(col,&f,&l);
+//				btAssert(f==l);
+				if (f<m_rowNonZeroElements1[row].size()-1)
+				{
+					m_rowNonZeroElements1[row].expandNonInitializing();
+					for (int j=m_rowNonZeroElements1[row].size()-1;j>f;j--)
+						m_rowNonZeroElements1[row][j] = m_rowNonZeroElements1[row][j-1];
+					m_rowNonZeroElements1[row][f] = col;
+				} else
+				{
+					m_rowNonZeroElements1[row].push_back(col);
+				}
+				m_colNonZeroElements[col].findBinarySearch(row,&f,&l);
+	//			btAssert(f==l);
+				if (f<m_colNonZeroElements[col].size()-1)
+				{
+					m_colNonZeroElements[col].expandNonInitializing();
+					for (int j=m_colNonZeroElements[col].size()-1;j>f;j++)
+						m_colNonZeroElements[col][j-1] = m_colNonZeroElements[col][j];
+					m_colNonZeroElements[col][f] = row;
+				} else
+				{
+					m_colNonZeroElements[col].push_back(row);
+				}
+				
+				*/
+
+				m_storage[row*m_cols+col] = val;
+			} else
+			{
+
+			}
+		}
+	}
+	const T& operator() (int row,int col) const
+	{
+		return m_storage[col+row*m_cols];
+	}
+
+	void clearSparseInfo()
+	{
+		BT_PROFILE("clearSparseInfo=0");
+		m_rowNonZeroElements1.resize(m_rows);
+		m_colNonZeroElements.resize(m_cols);
+		for (int i=0;i<m_rows;i++)
+			m_rowNonZeroElements1[i].resize(0);
+		for (int j=0;j<m_cols;j++)
+			m_colNonZeroElements[j].resize(0);
+	}
+
+	void setZero()
+	{
+		{
+			BT_PROFILE("storage=0");
+			btSetZero(&m_storage[0],m_storage.size());
+			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+			//for (int i=0;i<m_storage.size();i++)
+	//			m_storage[i]=0;
+		}
+		{
+			BT_PROFILE("clearSparseInfo=0");
+			clearSparseInfo();
+		}
+	}
+
+	void	printMatrix(const char* msg)
+	{
+		printf("%s ---------------------\n",msg);
+		for (int i=0;i<rows();i++)
+		{
+			printf("\n");
+			for (int j=0;j<cols();j++)
+			{
+				printf("%2.1f\t",(*this)(i,j));
+			}
+		}
+		printf("\n---------------------\n");
+
+	}
+	void	printNumZeros(const char* msg)
+	{
+		printf("%s: ",msg);
+		int numZeros = 0;
+		for (int i=0;i<m_storage.size();i++)
+			if (m_storage[i]==0)
+				numZeros++;
+		int total = m_cols*m_rows;
+		int computedNonZero = total-numZeros;
+		int nonZero = 0;
+		for (int i=0;i<m_colNonZeroElements.size();i++)
+			nonZero += m_colNonZeroElements[i].size();
+		btAssert(computedNonZero==nonZero);
+		if(computedNonZero!=nonZero)
+		{
+			printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
+		}
+		//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
+		printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
+	}
+	/*
+	void rowComputeNonZeroElements()
+	{
+		m_rowNonZeroElements1.resize(rows());
+		for (int i=0;i<rows();i++)
+		{
+			m_rowNonZeroElements1[i].resize(0);
+			for (int j=0;j<cols();j++)
+			{
+				if ((*this)(i,j)!=0.f)
+				{
+					m_rowNonZeroElements1[i].push_back(j);
+				}
+			}
+		}
+	}
+	*/
+	btMatrixX transpose() const
+	{
+		//transpose is optimized for sparse matrices
+		btMatrixX tr(m_cols,m_rows);
+		tr.setZero();
+#if 0
+		for (int i=0;i<m_cols;i++)
+			for (int j=0;j<m_rows;j++)
+			{
+				T v = (*this)(j,i);
+				if (v)
+				{
+					tr.setElem(i,j,v);
+				}
+			}
+#else		
+		for (int i=0;i<m_colNonZeroElements.size();i++)
+			for (int h=0;h<m_colNonZeroElements[i].size();h++)
+			{
+				int j = m_colNonZeroElements[i][h];
+				T v = (*this)(j,i);
+				tr.setElem(i,j,v);
+			}
+#endif
+		return tr;
+	}
+
+	void sortRowIndexArrays()
+	{
+		for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
+		{
+			m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
+		}
+	}
+
+	void sortColIndexArrays()
+	{
+		for (int i=0;i<m_colNonZeroElements[i].size();i++)
+		{
+			m_colNonZeroElements[i].quickSort(btIntSortPredicate());
+		}
+	}
+
+	btMatrixX operator*(const btMatrixX& other)
+	{
+		//btMatrixX*btMatrixX implementation, optimized for sparse matrices
+		btAssert(cols() == other.rows());
+
+		btMatrixX res(rows(),other.cols());
+		res.setZero();
+//		BT_PROFILE("btMatrixX mul");
+		for (int j=0; j < res.cols(); ++j)
+		{
+			//int numZero=other.m_colNonZeroElements[j].size();
+			//if (numZero)
+			{
+				for (int i=0; i < res.rows(); ++i)
+				//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
+				{
+					T dotProd=0;
+					T dotProd2=0;
+					int waste=0,waste2=0;
+
+					bool doubleWalk = false;
+					if (doubleWalk)
+					{
+						int numRows = m_rowNonZeroElements1[i].size();
+						int numOtherCols = other.m_colNonZeroElements[j].size();
+						for (int ii=0;ii<numRows;ii++)
+						{
+							int vThis=m_rowNonZeroElements1[i][ii];
+						}
+
+						for (int ii=0;ii<numOtherCols;ii++)
+						{
+							int vOther = other.m_colNonZeroElements[j][ii];
+						}
+
+
+						int indexRow = 0;
+						int indexOtherCol = 0;
+						while (indexRow < numRows && indexOtherCol < numOtherCols)
+						{
+							int vThis=m_rowNonZeroElements1[i][indexRow];
+							int vOther = other.m_colNonZeroElements[j][indexOtherCol];
+							if (vOther==vThis)
+							{
+								dotProd += (*this)(i,vThis) * other(vThis,j);
+							}
+							if (vThis<vOther)
+							{
+								indexRow++;
+							} else
+							{
+								indexOtherCol++;
+							}
+						}
+
+					} else
+					{
+						bool useOtherCol = true;
+						if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
+						{
+						useOtherCol=true;
+						}
+						if (!useOtherCol )
+						{
+							for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
+							{
+								int v = other.m_colNonZeroElements[j][q];
+								T w = (*this)(i,v);
+								if (w!=0.f)
+								{
+									dotProd+=w*other(v,j);
+								}
+						
+							}
+						}
+						else
+						{
+							for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
+							{
+								int v=m_rowNonZeroElements1[i][q];
+								T w = (*this)(i,v);
+								if (other(v,j)!=0.f)
+								{
+									dotProd+=w*other(v,j);	
+								}
+						
+							}
+						}
+					}
+					if (dotProd)
+						res.setElem(i,j,dotProd);
+				}
+			}
+		}
+		return res;
+	}
+
+	// this assumes the 4th and 8th rows of B and C are zero.
+	void multiplyAdd2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther ,int row, int col)
+	{
+		const btScalar *bb = B;
+		for ( int i = 0;i<numRows;i++)
+		{
+			const btScalar *cc = C;
+			for ( int j = 0;j<numRowsOther;j++)
+			{
+				btScalar sum;
+				sum  = bb[0]*cc[0];
+				sum += bb[1]*cc[1];
+				sum += bb[2]*cc[2];
+				sum += bb[4]*cc[4];
+				sum += bb[5]*cc[5];
+				sum += bb[6]*cc[6];
+				addElem(row+i,col+j,sum);
+				cc += 8;
+			}
+			bb += 8;
+		}
+	}
+
+	void multiply2_p8r (const btScalar *B, const btScalar *C,  int numRows,  int numRowsOther, int row, int col)
+	{
+		btAssert (numRows>0 && numRowsOther>0 && B && C);
+		const btScalar *bb = B;
+		for ( int i = 0;i<numRows;i++)
+		{
+			const btScalar *cc = C;
+			for ( int j = 0;j<numRowsOther;j++)
+			{
+				btScalar sum;
+				sum  = bb[0]*cc[0];
+				sum += bb[1]*cc[1];
+				sum += bb[2]*cc[2];
+				sum += bb[4]*cc[4];
+				sum += bb[5]*cc[5];
+				sum += bb[6]*cc[6];
+				setElem(row+i,col+j,sum);
+				cc += 8;
+			}
+			bb += 8;
+		}
+	}
+
+};
+
+template <typename T> 
+struct btVectorX
+{
+	btAlignedObjectArray<T>	m_storage;
+
+	btVectorX()
+	{
+	}
+	btVectorX(int numRows)
+	{
+		m_storage.resize(numRows);
+	}
+
+	void resize(int rows)
+	{
+		m_storage.resize(rows);
+	}
+	int cols() const
+	{
+		return 1;
+	}
+	int rows() const
+	{
+		return m_storage.size();
+	}
+	int size() const
+	{
+		return rows();
+	}
+	void	setZero()
+	{
+	//	for (int i=0;i<m_storage.size();i++)
+	//		m_storage[i]=0;
+		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+		btSetZero(&m_storage[0],m_storage.size());
+	}
+	const T& operator[] (int index) const
+	{
+		return m_storage[index];
+	}
+
+	T& operator[] (int index)
+	{
+		return m_storage[index];
+	}
+
+	T* getBufferPointerWritable() 
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+
+};
+/*
+template <typename T> 
+void setElem(btMatrixX<T>& mat, int row, int col, T val)
+{
+	mat.setElem(row,col,val);
+}
+*/
+
+
+typedef btMatrixX<float> btMatrixXf;
+typedef btVectorX<float> btVectorXf;
+
+typedef btMatrixX<double> btMatrixXd;
+typedef btVectorX<double> btVectorXd;
+
+
+
+inline void setElem(btMatrixXd& mat, int row, int col, double val)
+{
+	mat.setElem(row,col,val);
+}
+
+inline void setElem(btMatrixXf& mat, int row, int col, float val)
+{
+	mat.setElem(row,col,val);
+}
+
+#ifdef BT_USE_DOUBLE_PRECISION
+	#define btVectorXu btVectorXd
+	#define btMatrixXu btMatrixXd
+#else
+	#define btVectorXu btVectorXf
+	#define btMatrixXu btMatrixXf
+#endif //BT_USE_DOUBLE_PRECISION
+
+
+
+#endif//BT_MATRIX_H_H
diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h
index 992dc3cff..2244f600f 100644
--- a/src/LinearMath/btScalar.h
+++ b/src/LinearMath/btScalar.h
@@ -611,6 +611,17 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
 	return d;
 }
 
+template<typename T>
+SIMD_FORCE_INLINE void btSetZero(T* a, int n)
+{
+  T* acurr = a;
+  size_t ncurr = n;
+  while (ncurr > 0) 
+  {
+    *(acurr++) = 0;
+    --ncurr;
+  }
+}
 // returns normalized value in range [-SIMD_PI, SIMD_PI]
 SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians) 
 {
@@ -629,6 +640,8 @@ SIMD_FORCE_INLINE btScalar btNormalizeAngle(btScalar angleInRadians)
 	}
 }
 
+
+
 ///rudimentary class to provide type info
 struct btTypedObject
 {