From 644d01d23155d55b48c460839e850797c2fa9a69 Mon Sep 17 00:00:00 2001
From: "erwin.coumans@gmail.com"
 <erwin.coumans@gmail.com@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Thu, 31 Oct 2013 06:17:08 +0000
Subject: [PATCH] added the btNNCGConstraintSolver, based on the paper
 "Nonsmooth Nonlinear Conjugate Gradient Method for interactive contact force
 problems". The solver needs a lot of iterations, before the quality goes up
 (~ 1000) Thanks to Gabor PUHR for the contribution! Improved the
 btLemkeSolver. Remove the sparse optimizations from the btMatrixX.h, replace
 it with explicit call to rowComputeNonZeroElements (only used in the
 btSolveProjectedGaussSeidel), it was likely slowing things down, without
 being useful. Re-enable SIMD in the solver (was accidently disabled in Bullet
 2.82 release)

---
 src/BulletDynamics/CMakeLists.txt             |   2 +
 .../ConstraintSolver/btConstraintSolver.h     |   3 +-
 .../btNNCGConstraintSolver.cpp                | 463 ++++++++++++++++++
 .../ConstraintSolver/btNNCGConstraintSolver.h |  64 +++
 .../btSequentialImpulseConstraintSolver.cpp   |  27 +-
 .../btSequentialImpulseConstraintSolver.h     |  11 +-
 .../MLCPSolvers/btLemkeAlgorithm.h            |   8 +-
 .../MLCPSolvers/btLemkeSolver.h               | 319 +++++++-----
 .../MLCPSolvers/btMLCPSolver.cpp              |  25 +-
 .../MLCPSolvers/btSolveProjectedGaussSeidel.h |   6 +
 src/LinearMath/btMatrixX.h                    | 164 +------
 11 files changed, 794 insertions(+), 298 deletions(-)
 create mode 100644 src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
 create mode 100644 src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h

diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index cc4727639..b98cca6f8 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -14,6 +14,7 @@ SET(BulletDynamics_SRCS
 	ConstraintSolver/btHingeConstraint.cpp
 	ConstraintSolver/btPoint2PointConstraint.cpp
 	ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+	ConstraintSolver/btNNCGConstraintSolver.cpp
 	ConstraintSolver/btSliderConstraint.cpp
 	ConstraintSolver/btSolve2LinearConstraint.cpp
 	ConstraintSolver/btTypedConstraint.cpp
@@ -53,6 +54,7 @@ SET(ConstraintSolver_HDRS
 	ConstraintSolver/btJacobianEntry.h
 	ConstraintSolver/btPoint2PointConstraint.h
 	ConstraintSolver/btSequentialImpulseConstraintSolver.h
+	ConstraintSolver/btNNCGConstraintSolver.h
 	ConstraintSolver/btSliderConstraint.h
 	ConstraintSolver/btSolve2LinearConstraint.h
 	ConstraintSolver/btSolverBody.h
diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 1ba1cd1e8..890afe6da 100644
--- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -33,7 +33,8 @@ class	btDispatcher;
 enum btConstraintSolverType
 {
 	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2
+	BT_MLCP_SOLVER=2,
+	BT_NNCG_SOLVER=4
 };
 
 class btConstraintSolver
diff --git a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
new file mode 100644
index 000000000..11fbadef7
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp
@@ -0,0 +1,463 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btNNCGConstraintSolver.h"
+
+
+
+
+
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+
+	m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
+	m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
+	m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
+	m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
+
+	return val;
+}
+
+btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
+{
+
+	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+
+	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+	{
+		if (1)			// uncomment this for a bit less random ((iteration & 7) == 0)
+		{
+
+			for (int j=0; j<numNonContactPool; ++j) {
+				int tmp = m_orderNonContactConstraintPool[j];
+				int swapi = btRandInt2(j+1);
+				m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
+				m_orderNonContactConstraintPool[swapi] = tmp;
+			}
+
+			//contact/friction constraints are not solved more than 
+			if (iteration< infoGlobal.m_numIterations)
+			{
+				for (int j=0; j<numConstraintPool; ++j) {
+					int tmp = m_orderTmpConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+					m_orderTmpConstraintPool[swapi] = tmp;
+				}
+
+				for (int j=0; j<numFrictionPool; ++j) {
+					int tmp = m_orderFrictionConstraintPool[j];
+					int swapi = btRandInt2(j+1);
+					m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+					m_orderFrictionConstraintPool[swapi] = tmp;
+				}
+			}
+		}
+	}
+
+
+	btScalar deltaflengthsqr = 0;
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	} else 
+	{
+		for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+		{
+			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+			if (iteration < constraint.m_overrideNumSolverIterations) 
+			{
+				btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				m_deltafNC[j] = deltaf;
+				deltaflengthsqr += deltaf * deltaf;
+			}
+		}
+	}
+
+
+	if (m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+		} else {
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+			} else 
+			{
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) 
+					{
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+
+
+	if (infoGlobal.m_solverMode & SOLVER_SIMD)
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+
+			///solve all contact constraints using SIMD, if available
+			if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
+			{
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
+
+				for (int c=0;c<numPoolConstraints;c++)
+				{
+					btScalar totalImpulse =0;
+
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
+						btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);						
+						m_deltafC[c] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+						totalImpulse = solveManifold.m_appliedImpulse;
+					}
+					bool applyFriction = true;
+					if (applyFriction)
+					{
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier] = 0;
+							}
+						}
+
+						if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
+						{
+
+							btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
+
+							if (totalImpulse>btScalar(0))
+							{
+								solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+								solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+								btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								m_deltafCF[c*multiplier+1] = deltaf;
+								deltaflengthsqr += deltaf*deltaf;
+							} else {
+								m_deltafCF[c*multiplier+1] = 0;
+							}
+						}
+					}
+				}
+
+			}
+			else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
+			{
+				//solve the friction constraints after all contact constraints, don't interleave them
+				int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+				int j;
+
+				for (j=0;j<numPoolConstraints;j++)
+				{
+					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafC[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				}
+
+
+
+				///solve all friction constraints, using SIMD, if available
+
+				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+				for (j=0;j<numFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+					if (totalImpulse>btScalar(0))
+					{
+						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						m_deltafCF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCF[j] = 0;
+					}
+				}
+
+
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+						if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+							rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+						btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						m_deltafCRF[j] = deltaf;
+						deltaflengthsqr += deltaf*deltaf;
+					} else {
+						m_deltafCRF[j] = 0;
+					}
+				}
+
+
+			}
+		}
+
+
+
+	} else
+	{
+
+		if (iteration< infoGlobal.m_numIterations)
+		{
+			for (int j=0;j<numConstraints;j++)
+			{
+				if (constraints[j]->isEnabled())
+				{
+					int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
+					int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
+					btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
+					btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
+					constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
+				}
+			}
+			///solve all contact constraints
+			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			for (int j=0;j<numPoolConstraints;j++)
+			{
+				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+				btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				m_deltafC[j] = deltaf;
+				deltaflengthsqr += deltaf*deltaf;
+			}
+			///solve all friction constraints
+			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			for (int j=0;j<numFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+
+				if (totalImpulse>btScalar(0))
+				{
+					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
+					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					m_deltafCF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCF[j] = 0;
+				}
+			}
+
+			int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+			for (int j=0;j<numRollingFrictionPoolConstraints;j++)
+			{
+				btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+				btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+				if (totalImpulse>btScalar(0))
+				{
+					btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
+					if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
+						rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
+
+					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
+					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
+
+					btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					m_deltafCRF[j] = deltaf;
+					deltaflengthsqr += deltaf*deltaf;
+				} else {
+					m_deltafCRF[j] = 0;
+				}
+			}
+		}
+	}
+
+
+
+
+	if (!m_onlyForNoneContact) 
+	{
+		if (iteration==0) 
+		{
+			for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
+			for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
+			for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
+			if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 
+			{
+				for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
+			}
+		} else 
+		{
+			// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
+			btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
+			if (beta>1) {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
+				if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
+				}
+			} else {
+				for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+					if (iteration < constraint.m_overrideNumSolverIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pNC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pC[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pC[j] = beta * m_pC[j] + m_deltafC[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
+				{
+					btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					if (iteration< infoGlobal.m_numIterations) {
+						btScalar additionaldeltaimpulse = beta * m_pCF[j];
+						constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+						m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
+						btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+						btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+						const btSolverConstraint& c = constraint;
+						body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+						body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+					}
+				}
+				if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
+					for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
+					{
+						btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+						if (iteration< infoGlobal.m_numIterations) {
+							btScalar additionaldeltaimpulse = beta * m_pCRF[j];
+							constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
+							m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
+							btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
+							btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
+							const btSolverConstraint& c = constraint;
+							body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
+							body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
+						}
+					}
+				}
+			}
+		}
+		m_deltafLengthSqrPrev = deltaflengthsqr;
+	}
+
+	return deltaflengthsqr;
+}
+
+btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	m_pNC.resizeNoInitialize(0);
+	m_pC.resizeNoInitialize(0);
+	m_pCF.resizeNoInitialize(0);
+	m_pCRF.resizeNoInitialize(0);
+
+	m_deltafNC.resizeNoInitialize(0);
+	m_deltafC.resizeNoInitialize(0);
+	m_deltafCF.resizeNoInitialize(0);
+	m_deltafCRF.resizeNoInitialize(0);
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
+}
+
+
+
diff --git a/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
new file mode 100644
index 000000000..6a196416a
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h
@@ -0,0 +1,64 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
+#define BT_NNCG_CONSTRAINT_SOLVER_H
+
+#include "btSequentialImpulseConstraintSolver.h"
+
+ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
+{
+protected:
+
+	btScalar m_deltafLengthSqrPrev;
+
+	btAlignedObjectArray<btScalar> m_pNC;  // p for None Contact constraints
+	btAlignedObjectArray<btScalar> m_pC;   // p for Contact constraints
+	btAlignedObjectArray<btScalar> m_pCF;  // p for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
+
+	//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
+	btAlignedObjectArray<btScalar> m_deltafNC;  // deltaf for NoneContact constraints
+	btAlignedObjectArray<btScalar> m_deltafC;   // deltaf for Contact constraints
+	btAlignedObjectArray<btScalar> m_deltafCF;  // deltaf for ContactFriction constraints
+	btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
+
+		
+protected:
+
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_NNCG_SOLVER;
+	}
+
+	bool m_onlyForNoneContact;
+};
+
+
+
+
+#endif //BT_NNCG_CONSTRAINT_SOLVER_H
+
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 4b1155940..aabfa208d 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -58,7 +58,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
 #endif//USE_SIMD
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -86,13 +86,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	return deltaImpulse.m128_f32[0];
 #else
-	resolveSingleConstraintRowGeneric(body1,body2,c);
+	return resolveSingleConstraintRowGeneric(body1,body2,c);
 #endif
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -120,9 +121,11 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 
 	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+
+	return deltaImpulse;
 }
 
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -147,13 +150,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	return deltaImpulse.m128_f32[0];
 #else
-	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
 #endif
 }
 
-// Projected Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+
+btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -173,6 +177,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	}
 	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+
+	return deltaImpulse;
 }
 
 
@@ -430,6 +436,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btSimdScalar velocityError =  desiredVelocity - rel_vel;
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -1452,8 +1459,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
 		
@@ -1472,8 +1478,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 180d2a385..c7fe2ccb5 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -88,13 +88,10 @@ protected:
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
-	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 		
 protected:
 	
diff --git a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
index caeff6f41..7555cd9d2 100644
--- a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
+++ b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
@@ -15,14 +15,14 @@ subject to the following restrictions:
 
 //The original version is here
 //https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
-//This file is re-distributed under the ZLib license, with permission of the original author
+//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
 //STL/std::vector replaced by btAlignedObjectArray
 
 
 
-#ifndef NUMERICS_LEMKE_ALGORITHM_H_
-#define NUMERICS_LEMKE_ALGORITHM_H_
+#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
+#define BT_NUMERICS_LEMKE_ALGORITHM_H_
 
 #include "LinearMath/btMatrixX.h"
 
@@ -105,4 +105,4 @@ protected:
 };
 
 
-#endif /* NUMERICS_LEMKE_ALGORITHM_H_ */
+#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */
diff --git a/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h b/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
index 177559569..41dc1e5d1 100644
--- a/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
+++ b/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
@@ -22,103 +22,134 @@ subject to the following restrictions:
 #include "btLemkeAlgorithm.h"
 
 
-
-#ifdef BT_DEBUG_OSTREAM
-using namespace std;
-#endif //BT_DEBUG_OSTREAM
 
+
+///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
+///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
+///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
 class btLemkeSolver : public btMLCPSolverInterface
 {
+protected:
+	
 public:
+
+	btScalar	m_maxValue;
+	int			m_debugLevel;
+	int			m_maxLoops;
+	bool		m_useLoHighBounds;
+	
+	
+
+	btLemkeSolver()
+		:m_maxValue(100000),
+		m_debugLevel(0),
+		m_maxLoops(1000),
+		m_useLoHighBounds(true)
+	{
+	}
 	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
+		
+		if (m_useLoHighBounds)
+		{
+
+		BT_PROFILE("btLemkeSolver::solveMLCP");
 		int n = A.rows();
 		if (0==n)
 			return true;
 		
-//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+		bool fail = false;
 
+		btVectorXu solution(n);
 		btVectorXu q1;
 		q1.resize(n);
 		for (int row=0;row<n;row++)
 		{
 			q1[row] = -b[row];
 		}
-		
-//		cout << "A" << endl;
-//		cout << A << endl;
 
-		/////////////////////////////////////
+	//		cout << "A" << endl;
+	//		cout << A << endl;
 
-		//slow matrix inversion, replace with LU decomposition
-		btMatrixXu A1;
-		A1.resize(A.rows(),A.cols());
-		for (int row=0;row<A.rows();row++)
-		{
-			for (int col=0;col<A.cols();col++)
+			/////////////////////////////////////
+
+			//slow matrix inversion, replace with LU decomposition
+			btMatrixXu A1;
+			btMatrixXu B(n,n);
 			{
-				A1.setElem(row,col,A(row,col));
+				BT_PROFILE("inverse(slow)");
+				A1.resize(A.rows(),A.cols());
+				for (int row=0;row<A.rows();row++)
+				{
+					for (int col=0;col<A.cols();col++)
+					{
+						A1.setElem(row,col,A(row,col));
+					}
+				}
+
+				btMatrixXu matrix;
+				matrix.resize(n,2*n);
+				for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						matrix.setElem(row,col,A1(row,col));
+					}
+				}
+
+
+				btScalar ratio,a;
+				int i,j,k;
+				for(i = 0; i < n; i++){
+				for(j = n; j < 2*n; j++){
+					if(i==(j-n))
+						matrix.setElem(i,j,1.0);
+					else
+						matrix.setElem(i,j,0.0);
+				}
 			}
-		}
-
-		btMatrixXu matrix;
-		matrix.resize(n,2*n);
-		for (int row=0;row<n;row++)
-		{
-			for (int col=0;col<n;col++)
-			{
-				matrix.setElem(row,col,A1(row,col));
+			for(i = 0; i < n; i++){
+				for(j = 0; j < n; j++){
+					if(i!=j)
+					{
+						btScalar v = matrix(i,i);
+						if (btFuzzyZero(v))
+						{
+							a = 0.000001f;
+						}
+						ratio = matrix(j,i)/matrix(i,i);
+						for(k = 0; k < 2*n; k++){
+							matrix.addElem(j,k,- ratio * matrix(i,k));
+						}
+					}
+				}
+			}
+			for(i = 0; i < n; i++){
+				a = matrix(i,i);
+				if (btFuzzyZero(a))
+				{
+					a = 0.000001f;
+				}
+				btScalar invA = 1.f/a;
+				for(j = 0; j < 2*n; j++){
+					matrix.mulElem(i,j,invA);
+				}
 			}
-		}
-
-
-		btScalar ratio,a;
-		int i,j,k;
-		for(i = 0; i < n; i++){
-        for(j = n; j < 2*n; j++){
-            if(i==(j-n))
-                matrix.setElem(i,j,1.0);
-            else
-                matrix.setElem(i,j,0.0);
-        }
-    }
-    for(i = 0; i < n; i++){
-        for(j = 0; j < n; j++){
-            if(i!=j){
-                ratio = matrix(j,i)/matrix(i,i);
-                for(k = 0; k < 2*n; k++){
-					matrix.addElem(j,k,- ratio * matrix(i,k));
-                }
-            }
-        }
-    }
-    for(i = 0; i < n; i++){
-        a = matrix(i,i);
-		btScalar invA = 1.f/a;
-        for(j = 0; j < 2*n; j++){
-			matrix.mulElem(i,j,invA);
-        }
-    }
 
 	
 
-	btMatrixXu B(n,n);
+	
 
-	for (int row=0;row<n;row++)
-		{
-			for (int col=0;col<n;col++)
-			{
-				B.setElem(row,col,matrix(row,n+col));
+			for (int row=0;row<n;row++)
+				{
+					for (int col=0;col<n;col++)
+					{
+						B.setElem(row,col,matrix(row,n+col));
+					}
+				}
 			}
-		}
-
-//	cout << "B" << endl;
-//	cout << B << endl;
-
-	btMatrixXu b1(n,1);
-
-
 
+		btMatrixXu b1(n,1);
 
 		btMatrixXu M(n*2,n*2);
 		for (int row=0;row<n;row++)
@@ -135,9 +166,6 @@ public:
 			}
 		}
 
-//		cout << "M:" << endl;
-//		cout << M << endl;
-
 		btMatrixXu Bb1 = B*b1;
 //		q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
 
@@ -149,29 +177,16 @@ public:
 			qq[n+row] = Bb1(row,0)+hi[row];
 		}
 
-	//	cout << "qq:" << endl;
-	//	cout << qq << endl;
-
-		int debugLevel=0;
-//		btLemkeAlgorithm lemke(A,q1,debugLevel);
-//		lemke.setSystem(A,q1);
-
-		btLemkeAlgorithm lemke(M,qq,debugLevel);
-		
-
-		int maxloops = 100;
-		btVectorXu z1  = lemke.solve(maxloops);
-		
-	//	cout << "x" << endl;
-	//	cout << z1 << endl;
-		
-		//y_plus  = z1(1:8);
-		//y_minus = z1(9:16);
-		//y1      = y_plus - y_minus;
-		//x1      = B*(y1-b1);
+		btVectorXu z1;
 
 		btMatrixXu y1;
 		y1.resize(n,1);
+		btLemkeAlgorithm lemke(M,qq,m_debugLevel);
+		{
+			BT_PROFILE("lemke.solve");
+			lemke.setSystem(M,qq);
+			z1  = lemke.solve(m_maxLoops);
+		}
 		for (int row=0;row<n;row++)
 		{
 			y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
@@ -182,20 +197,15 @@ public:
 			y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
 		}
 
-		btMatrixXu x1 = B*(y1_b1);
-		
-//		cout << "x1" << endl;
-//		cout << x1 << endl;
-		btVectorXu solution(n);
+		btMatrixXu x1;
+
+		x1 = B*(y1_b1);
+			
 		for (int row=0;row<n;row++)
 		{
 			solution[row] = x1(row,0);//n];
 		}
-		//check solution
-//		cout << "solution" << endl;
-//		cout << solution << endl;
-		
-		bool fail = false;
+
 		int errorIndexMax = -1;
 		int errorIndexMin = -1;
 		float errorValueMax = -1e30;
@@ -207,13 +217,14 @@ public:
 			volatile btScalar check = x[i];
 			if (x[i] != check)
 			{
+				//printf("Lemke result is #NAN\n");
 				x.setZero();
 				return false;
 			}
 			
 			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
 			//we need to figure out why it happens, and fix it, or detect it properly)
-			if (x[i]>100000)
+			if (x[i]>m_maxValue)
 			{
 				if (x[i]> errorValueMax)
 				{
@@ -223,7 +234,86 @@ public:
 				}
 				////printf("x[i] = %f,",x[i]);
 			}
-			if (x[i]<-10000)
+			if (x[i]<-m_maxValue)
+			{
+				if (x[i]<errorValueMin)
+				{
+					errorIndexMin = i;
+					errorValueMin = x[i];
+					fail = true;
+					//printf("x[i] = %f,",x[i]);
+				}
+			}
+		}
+		if (fail)
+		{
+			int m_errorCountTimes = 0;
+			if (errorIndexMin<0)
+				errorValueMin = 0.f;
+			if (errorIndexMax<0)
+				errorValueMax = 0.f;
+			m_errorCountTimes++;
+		//	printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
+			for (int i=0;i<n;i++)
+			{
+				x[i]=0.f;
+			}
+		}
+		return !fail;
+	} else
+		
+	{
+			int dimension = A.rows();
+		if (0==dimension)
+			return true;
+		
+//		printf("================ solving using Lemke/Newton/Fixpoint\n");
+
+		btVectorXu q;
+		q.resize(dimension);
+		for (int row=0;row<dimension;row++)
+		{
+			q[row] = -b[row];
+		}
+		
+		btLemkeAlgorithm lemke(A,q,m_debugLevel);
+		
+		
+		lemke.setSystem(A,q);
+		
+		btVectorXu solution = lemke.solve(m_maxLoops);
+		
+		//check solution
+		
+		bool fail = false;
+		int errorIndexMax = -1;
+		int errorIndexMin = -1;
+		float errorValueMax = -1e30;
+		float errorValueMin = 1e30;
+		
+		for (int i=0;i<dimension;i++)
+		{
+			x[i] = solution[i+dimension];
+			volatile btScalar check = x[i];
+			if (x[i] != check)
+			{
+				x.setZero();
+				return false;
+			}
+			
+			//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver 
+			//we need to figure out why it happens, and fix it, or detect it properly)
+			if (x[i]>m_maxValue)
+			{
+				if (x[i]> errorValueMax)
+				{
+					fail = true;
+					errorIndexMax = i;
+					errorValueMax = x[i];
+				}
+				////printf("x[i] = %f,",x[i]);
+			}
+			if (x[i]<-m_maxValue)
 			{
 				if (x[i]<errorValueMin)
 				{
@@ -242,28 +332,17 @@ public:
 			if (errorIndexMax<0)
 				errorValueMax = 0.f;
 			printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
-			for (int i=0;i<n;i++)
+			for (int i=0;i<dimension;i++)
 			{
 				x[i]=0.f;
 			}
 		}
-#if 0
-		if (lemke.getInfo()<0)
-		{
-			printf("Lemke found no solution, info = %d\n",lemke.getInfo());
-		} else
-		{
-			printf("Lemke info = %d, found a solution in %d steps\n",lemke.getInfo(),lemke.getSteps());
-			//printf("Lemke found a solution\n");
-			for (int i=0;i<dimension;i++)
-			{
-				x[i] = solution(i+dimension);
-			}
-		}
-#endif
+
 
 		return !fail;
-		
+	}
+	return true;
+
 	}
 
 };
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 72eed87d3..0243567ae 100644
--- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -19,6 +19,7 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
+
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
 :m_solver(solver),
 m_fallback(0)
@@ -160,13 +161,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		BT_PROFILE("init b (rhs)");
 		m_b.resize(numConstraintRows);
 		m_bSplit.resize(numConstraintRows);
-		//m_b.setZero();
+		m_b.setZero();
+		m_bSplit.setZero();
 		for (int i=0;i<numConstraintRows ;i++)
 		{
-			if (m_allConstraintArray[i].m_jacDiagABInv)
+			btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
 			{
-				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 rhs = m_allConstraintArray[i].m_rhs;
+				btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
+				m_b[i]=rhs/jacDiag;
+				m_bSplit[i] = rhsPenetration/jacDiag;
 			}
 
 		}
@@ -425,14 +430,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		}
 	}
 
-	///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.000001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
 				   
@@ -473,6 +476,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	if (infoGlobal.m_splitImpulse)
 		m_bSplit.resize(numConstraintRows);
  
+	m_bSplit.setZero();
+	m_b.setZero();
+
 	for (int i=0;i<numConstraintRows ;i++)
 	{
 		if (m_allConstraintArray[i].m_jacDiagABInv)
@@ -554,12 +560,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 	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.000001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
 
@@ -618,6 +622,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	}
 	else
 	{
+	//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
 		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	}
diff --git a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
index ce8a07e64..f681fa1cf 100644
--- a/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
+++ b/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
@@ -20,11 +20,17 @@ subject to the following restrictions:
 
 #include "btMLCPSolverInterface.h"
 
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
 {
 public:
 	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
+		if (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
 		//A is a m-n matrix, m rows, n columns
 		btAssert(A.rows() == b.rows());
 
diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h
index 52cc79b3a..b025da7a1 100644
--- a/src/LinearMath/btMatrixX.h
+++ b/src/LinearMath/btMatrixX.h
@@ -113,10 +113,13 @@ struct btVectorX
 	}
 	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());
+		if (m_storage.size())
+		{
+			//	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
 	{
@@ -158,8 +161,7 @@ struct btMatrixX
 	int m_setElemOperations;
 
 	btAlignedObjectArray<T>	m_storage;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+	mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
 
 	T* getBufferPointerWritable() 
 	{
@@ -196,7 +198,6 @@ struct btMatrixX
 			BT_PROFILE("m_storage.resize");
 			m_storage.resize(rows*cols);
 		}
-		clearSparseInfo();
 	}
 	int cols() const
 	{
@@ -231,14 +232,6 @@ struct btMatrixX
 	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);
-			}
-		}
 		m_storage[row*m_cols+col] = val;
 	}
 	
@@ -256,11 +249,10 @@ struct btMatrixX
 	void copyLowerToUpperTriangle()
 	{
 		int count=0;
-		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		for (int row=0;row<rows();row++)
 		{
-			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			for (int col=0;col<row;col++)
 			{
-				int col = m_rowNonZeroElements1[row][j];
 				setElem(col,row, (*this)(row,col));
 				count++;
 				
@@ -274,16 +266,6 @@ struct btMatrixX
 		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()
 	{
@@ -294,10 +276,6 @@ struct btMatrixX
 			//for (int i=0;i<m_storage.size();i++)
 	//			m_storage[i]=0;
 		}
-		{
-			BT_PROFILE("clearSparseInfo=0");
-			clearSparseInfo();
-		}
 	}
 	
 	void setIdentity()
@@ -327,28 +305,9 @@ struct btMatrixX
 		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()
+
+
+	void rowComputeNonZeroElements() const
 	{
 		m_rowNonZeroElements1.resize(rows());
 		for (int i=0;i<rows();i++)
@@ -363,13 +322,11 @@ struct btMatrixX
 			}
 		}
 	}
-	*/
 	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++)
 			{
@@ -379,37 +336,13 @@ struct btMatrixX
 					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
+		//btMatrixX*btMatrixX implementation, brute force
 		btAssert(cols() == other.rows());
 
 		btMatrixX res(rows(),other.cols());
@@ -417,76 +350,18 @@ struct btMatrixX
 //		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++)
+							for (int v=0;v<rows();v++)
 							{
-								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)
 								{
@@ -593,10 +468,9 @@ struct btMatrixX
 	btMatrixX negative()
 	{
 		btMatrixX neg(rows(),cols());
-		for (int i=0;i<m_colNonZeroElements.size();i++)
-			for (int h=0;h<m_colNonZeroElements[i].size();h++)
+		for (int i=0;i<rows();i++)
+			for (int j=0;j<cols();j++)
 			{
-				int j = m_colNonZeroElements[i][h];
 				T v = (*this)(i,j);
 				neg.setElem(i,j,-v);
 			}
@@ -625,7 +499,7 @@ std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
 		{
 			for (int j=0;j<mat.cols();j++)
 			{
-				os << std::setw(10) << mat(i,j);
+				os << std::setw(12) << mat(i,j);
 			}
 			if (i!=mat.rows()-1)
 				os << std::endl << "  ";
@@ -643,7 +517,7 @@ std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
 		//printf("%s ---------------------\n",msg);
 		for (int i=0;i<mat.rows();i++)
 		{
-				os << std::setw(10) << mat[i];
+				os << std::setw(12) << mat[i];
 			if (i!=mat.rows()-1)
 				os << std::endl << "  ";
 		}