Merge pull request #890 from erwincoumans/master

(finally!) add option to terminate PGS constraint solvers based on a least square residual threshold

examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp

@@ -80,7 +80,18 @@ m_window(0)
 	m_window = helper->getAppInterface()->m_window;
 
 	m_data = new GpuRigidBodyDemoInternalData;
+	m_data->m_guiHelper = helper;
 }
+
+void GpuRigidBodyDemo::resetCamera()
+{
+	float dist = 114;
+	float pitch = 52;
+	float yaw = 35;
+	float targetPos[3]={0,0,0};
+	m_data->m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+}
+
 GpuRigidBodyDemo::~GpuRigidBodyDemo()
 {
 

examples/OpenCL/rigidbody/GpuRigidBodyDemo.h

@@ -31,7 +31,8 @@ public:
 	
 	virtual void renderScene();
 	
-	
+	void resetCamera();
+
 	virtual void stepSimulation(float deltaTime);
 
 	//for picking

examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h

@@ -32,6 +32,7 @@ struct	GpuRigidBodyDemoInternalData
 	int m_pickGraphicsShapeIndex;
 	int m_pickGraphicsShapeInstance;
 	b3Config m_config;
+	GUIHelperInterface* m_guiHelper;
 
 	GpuRigidBodyDemoInternalData()
 		:m_instancePosOrnColor(0),
@@ -45,7 +46,8 @@ struct	GpuRigidBodyDemoInternalData
 		m_pickGraphicsShapeInstance(-1),
 		m_pickBody(-1),
 		m_altPressed(0),
-		m_controlPressed(0)
+		m_controlPressed(0),
+		m_guiHelper(0)
 
 	{
 	}

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -717,7 +717,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
-
+	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
+	
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()

src/Bullet3Collision/NarrowPhaseCollision/b3Config.h

@@ -20,7 +20,7 @@ struct	b3Config
 	int m_maxTriConvexPairCapacity;
 
 	b3Config()
-		:m_maxConvexBodies(32*1024),
+		:m_maxConvexBodies(128*1024),
 		m_maxVerticesPerFace(64),
 		m_maxFacesPerShape(12),
 		m_maxConvexVertices(8192),

src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp

@@ -12,6 +12,7 @@ subject to the following restrictions:
 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.
 */
+#define CLEAR_MANIFOLD 1
 
 #include "btSphereSphereCollisionAlgorithm.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -58,7 +58,7 @@ struct btContactSolverInfoData
 	int			m_minimumSolverBatchSize;
 	btScalar	m_maxGyroscopicForce;
 	btScalar	m_singleAxisRollingFrictionThreshold;
-
+	btScalar	m_leastSquaresResidualThreshold;
 
 };
 
@@ -91,6 +91,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
 		m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
 		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+		m_leastSquaresResidualThreshold = 0.f;
 	}
 };
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -39,7 +39,7 @@ int		gNumSplitImpulseRecoveries = 0;
 
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
-
+//#define VERBOSE_RESIDUAL_PRINTF 1
 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
 ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
 static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
@@ -298,15 +298,17 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe
 }
 
 
-void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
+btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
         btSolverBody& body1,
         btSolverBody& body2,
         const btSolverConstraint& c)
 {
+	btScalar deltaImpulse = 0.f;
+
 		if (c.m_rhsPenetration)
         {
 			gNumSplitImpulseRecoveries++;
-			btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
+			deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
 			const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetPushVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
 			const btScalar deltaVel2Dotn	=	c.m_contactNormal2.dot(body2.internalGetPushVelocity())		+ c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
 
@@ -325,13 +327,14 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 			body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 			body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
         }
+		return deltaImpulse;
 }
 
- void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	if (!c.m_rhsPenetration)
-		return;
+		return 0.f;
 
 	gNumSplitImpulseRecoveries++;
 
@@ -357,8 +360,9 @@ void	btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
 	body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	return deltaImpulse;
 #else
-	resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
+	return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
 #endif
 }
 
@@ -1601,6 +1605,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
 {
+	btScalar leastSquaresResidual = 0.f;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@@ -1645,7 +1650,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			{
+				btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				leastSquaresResidual += residual*residual;
+			}
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
@@ -1674,7 +1682,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-						resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
+
 						totalImpulse = solveManifold.m_appliedImpulse;
 					}
 					bool applyFriction = true;
@@ -1689,7 +1699,8 @@ 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);
+								btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
 							}
 						}
 
@@ -1703,7 +1714,8 @@ 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);
+								btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+								leastSquaresResidual += residual*residual;
 							}
 						}
 					}
@@ -1719,8 +1731,8 @@ 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);
+					btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-
+					leastSquaresResidual += residual*residual;
 				}
 
 
@@ -1738,7 +1750,8 @@ 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);
+						btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
 					}
 				}
 
@@ -1758,7 +1771,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 						rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+						leastSquaresResidual += residual*residual;
 					}
 				}
 
@@ -1773,7 +1787,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
 			if (iteration < constraint.m_overrideNumSolverIterations)
-				resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+			{
+				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
+				leastSquaresResidual += residual*residual;
+			}
 		}
 
 		if (iteration< infoGlobal.m_numIterations)
@@ -1794,7 +1811,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			for (int j=0;j<numPoolConstraints;j++)
 			{
 				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+				leastSquaresResidual += residual*residual;
 			}
 			///solve all friction constraints
 			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
@@ -1808,7 +1826,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 					solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					leastSquaresResidual += residual*residual;
 				}
 			}
 
@@ -1826,12 +1845,13 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 					rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 					rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-					resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					leastSquaresResidual += residual*residual;
 				}
 			}
 		}
 	}
-	return 0.f;
+	return leastSquaresResidual;
 }
 
 
@@ -1844,6 +1864,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 		{
 			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 			{
+				btScalar leastSquaresResidual =0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
@@ -1851,15 +1872,24 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
 					}
 				}
+				if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
+				{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+					printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
+#endif
+					break;
+				}
 			}
 		}
 		else
 		{
 			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 			{
+				btScalar leastSquaresResidual = 0.f;
 				{
 					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
 					int j;
@@ -1867,7 +1897,15 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 					{
 						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
 
-						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						btScalar residual = resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						leastSquaresResidual += residual*residual;
+					}
+					if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
+					{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+						printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
+#endif
+						break;
 					}
 				}
 			}
@@ -1888,7 +1926,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 		for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
 		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
 		{
-			solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+			m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+
+			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+						printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
+#endif
+				break;
+			}
 		}
 
 	}

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -57,6 +57,8 @@ protected:
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
 
+	btScalar	m_leastSquaresResidual;
+
 	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, 
@@ -90,11 +92,11 @@ protected:
 	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
 
 
-	void	resolveSplitPenetrationSIMD(
+	btSimdScalar	resolveSplitPenetrationSIMD(
      btSolverBody& bodyA,btSolverBody& bodyB,
         const btSolverConstraint& contactConstraint);
 
-	void	resolveSplitPenetrationImpulseCacheFriendly(
+	btScalar	resolveSplitPenetrationImpulseCacheFriendly(
        btSolverBody& bodyA,btSolverBody& bodyB,
         const btSolverConstraint& contactConstraint);
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -26,7 +26,7 @@ subject to the following restrictions:
 
 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
-	btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
+	btScalar leastSquaredResidual  = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	
 	//solve featherstone non-contact constraints
 
@@ -38,7 +38,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 
 		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 		
-		resolveSingleConstraintRowGeneric(constraint);
+		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
+		leastSquaredResidual += residual*residual;
+
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
 		if(constraint.m_multiBodyB) 
@@ -49,9 +51,15 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
 	{
 		btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
-		if (iteration < infoGlobal.m_numIterations)
+		btScalar residual = 0.f;
-			resolveSingleConstraintRowGeneric(constraint);
 
+		if (iteration < infoGlobal.m_numIterations)
+		{
+			residual = resolveSingleConstraintRowGeneric(constraint);
+		}
+
+		leastSquaredResidual += residual*residual;
+ 
 		if(constraint.m_multiBodyA) 
 			constraint.m_multiBodyA->setPosUpdated(false);
 		if(constraint.m_multiBodyB) 
@@ -71,7 +79,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			{
 				frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
 				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
-				resolveSingleConstraintRowGeneric(frictionConstraint);
+				btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
+				leastSquaredResidual += residual*residual;
 
 				if(frictionConstraint.m_multiBodyA) 
 					frictionConstraint.m_multiBodyA->setPosUpdated(false);
@@ -80,7 +89,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			}
 		}
 	}
-	return val;
+	return leastSquaredResidual;
 }
 
 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
@@ -112,7 +121,7 @@ void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
 		m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
 }
 
-void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
+btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
 
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
@@ -190,7 +199,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	{
 		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 	}
-
+	return deltaImpulse;
 }
 
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -43,7 +43,7 @@ protected:
 	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;
 	int										m_tmpNumMultiBodyConstraints;
 
-	void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
+	btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
 	
 
 	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);

src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -23,7 +23,18 @@ subject to the following restrictions:
 ///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:
+
+	btScalar m_leastSquaresResidualThreshold;
+	btScalar m_leastSquaresResidual;
+
+	btSolveProjectedGaussSeidel()
+		:m_leastSquaresResidualThreshold(0),
+		m_leastSquaresResidual(0)
+	{
+	}
+
 	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())
@@ -36,10 +47,11 @@ public:
 
 		int i, j, numRows = A.rows();
 	
-		float delta;
+		btScalar delta;
 
 		for (int k = 0; k <numIterations; k++)
 		{
+			m_leastSquaresResidual = 0.f;
 			for (i = 0; i <numRows; i++)
 			{
 				delta = 0.0f;
@@ -61,9 +73,10 @@ public:
 						delta += A(i,j) * x[j];
 				}
 
-				float aDiag = A(i,i);
+				btScalar aDiag = A(i,i);
+				btScalar xOld = x[i];
 				x [i] = (b [i] - delta) / aDiag;
-				float s = 1.f;
+				btScalar s = 1.f;
 
 				if (limitDependency[i]>=0)
 				{
@@ -76,6 +89,17 @@ public:
 					x[i]=lo[i]*s;
 				if (x[i]>hi[i]*s)
 					x[i]=hi[i]*s;
+				btScalar diff = x[i] - xOld;
+				m_leastSquaresResidual += diff*diff;
+			}
+
+			btScalar eps  = m_leastSquaresResidualThreshold;
+			if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
+			{
+#ifdef VERBOSE_PRINTF_RESIDUAL
+				printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
+#endif
+				break;
 			}
 		}
 		return true;