Allow the ForkLiftDemo to toggle between MLCP and SI solver, using F6 key.

Apply patch for CMake config, see Issue 754 (Issue 753)
Fix a few issue with the MLCP solver: allow split impulse, and fix offset in friction dependencies

BulletConfig.cmake.in

@@ -0,0 +1,25 @@
+#                                               -*- cmake -*-
+#
+#  BulletConfig.cmake(.in)
+#
+
+# Use the following variables to compile and link against Bullet:
+#  BULLET_FOUND              - True if Bullet was found on your system
+#  BULLET_USE_FILE           - The file making Bullet usable
+#  BULLET_DEFINITIONS        - Definitions needed to build with Bullet
+#  BULLET_INCLUDE_DIR        - Directory where Bullet-C-Api.h can be found
+#  BULLET_INCLUDE_DIRS       - List of directories of Bullet and it's dependencies
+#  BULLET_LIBRARIES          - List of libraries to link against Bullet library
+#  BULLET_LIBRARY_DIRS       - List of directories containing Bullet' libraries
+#  BULLET_ROOT_DIR           - The base directory of Bullet
+#  BULLET_VERSION_STRING     - A human-readable string containing the version
+
+set ( BULLET_FOUND 1 )
+set ( BULLET_USE_FILE     "@BULLET_USE_FILE@" )
+set ( BULLET_DEFINITIONS  "@BULLET_DEFINITIONS@" )
+set ( BULLET_INCLUDE_DIR  "@INCLUDE_INSTALL_DIR@" )
+set ( BULLET_INCLUDE_DIRS "@INCLUDE_INSTALL_DIR@" )
+set ( BULLET_LIBRARIES    "@BULLET_LIBRARIES@" )
+set ( BULLET_LIBRARY_DIRS "@LIB_DESTINATION@" )
+set ( BULLET_ROOT_DIR     "@CMAKE_INSTALL_PREFIX@" )
+set ( BULLET_VERSION_STRING "@BULLET_VERSION@" )

CMakeLists.txt

@@ -422,3 +422,18 @@ OPTION(BUILD_UNIT_TESTS "Build Unit Tests"	OFF)
 IF (BUILD_UNIT_TESTS)
 	SUBDIRS(UnitTests)
 ENDIF()
+
+set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet )
+list (APPEND BULLET_LIBRARIES LinearMath)
+list (APPEND BULLET_LIBRARIES BulletCollisions)
+list (APPEND BULLET_LIBRARIES BulletDynamics)
+list (APPEND BULLET_LIBRARIES BulletSoftBody)
+set (BULLET_USE_FILE ${CMAKE_INSTALL_PREFIX}/${BULLET_CONFIG_CMAKE_PATH}/UseBullet.cmake)
+configure_file ( ${CMAKE_SOURCE_DIR}/BulletConfig.cmake.in
+                 ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake
+                 @ONLY ESCAPE_QUOTES
+               )
+install ( FILES ${CMAKE_SOURCE_DIR}/UseBullet.cmake
+                ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake
+          DESTINATION ${BULLET_CONFIG_CMAKE_PATH}
+        )

ChangeLog

@@ -4,6 +4,10 @@ Primary author and maintainer: Erwin Coumans
 This ChangeLog is incomplete, for an up-to-date list of all fixed issues see http://bullet.googlecode.com
 using http://tinyurl.com/yabmjjj
 
+2013 October 23
+	- Bullet 2.82 release
+	- See docs/BulletQuickstart.pdf or issue tracked for details.
+
 2012 September 10
 	- Bullet 2.81 release preparation
 	

Demos/ForkLiftDemo/ForkLiftDemo.cpp

@@ -22,6 +22,11 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
 #include "GLDebugFont.h"
 
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+
+
 #ifndef M_PI
 #define M_PI       3.14159265358979323846
 #endif
@@ -55,6 +60,8 @@ subject to the following restrictions:
 		btVector3 wheelAxleCS(-1,0,0);
 #endif
 
+bool useMCLPSolver = true;
+
 #include "GLDebugDrawer.h"
 #include <stdio.h> //printf debugging
 
@@ -123,7 +130,7 @@ m_maxCameraDistance(10.f)
 }
 
 
-void ForkLiftDemo::termPhysics()
+void ForkLiftDemo::exitPhysics()
 {
 		//cleanup in the reverse order of creation/initialization
 
@@ -157,6 +164,7 @@ void ForkLiftDemo::termPhysics()
 		btCollisionShape* shape = m_collisionShapes[j];
 		delete shape;
 	}
+	m_collisionShapes.clear();
 
 	delete m_indexVertexArrays;
 	delete m_vertices;
@@ -185,7 +193,7 @@ void ForkLiftDemo::termPhysics()
 
 ForkLiftDemo::~ForkLiftDemo()
 {
-	termPhysics();
+	exitPhysics();
 }
 
 void ForkLiftDemo::initPhysics()
@@ -203,8 +211,24 @@ void ForkLiftDemo::initPhysics()
 	btVector3 worldMin(-1000,-1000,-1000);
 	btVector3 worldMax(1000,1000,1000);
 	m_overlappingPairCache = new btAxisSweep3(worldMin,worldMax);
+	if (useMCLPSolver)
+	{
+		btDantzigSolver* mlcp = new btDantzigSolver();
+		//btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
+		btMLCPSolver* sol = new btMLCPSolver(mlcp);
+		m_constraintSolver = sol;
+	} else
+	{
 		m_constraintSolver = new btSequentialImpulseConstraintSolver();
+	}
 	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_constraintSolver,m_collisionConfiguration);
+	if (useMCLPSolver)
+	{
+		m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 1;//for direct solver it is better to have a small A matrix
+	} else
+	{
+		m_dynamicsWorld ->getSolverInfo().m_minimumSolverBatchSize = 128;//for direct solver, it is better to solve multiple objects together, small batches have high overhead
+	}
 #ifdef FORCE_ZAXIS_UP
 	m_dynamicsWorld->setGravity(btVector3(0,0,-10));
 #endif 
@@ -212,7 +236,7 @@ void ForkLiftDemo::initPhysics()
 	//m_dynamicsWorld->setGravity(btVector3(0,0,0));
 btTransform tr;
 tr.setIdentity();
-tr.setOrigin(btVector3(0,-10,0));
+tr.setOrigin(btVector3(0,-3,0));
 
 //either use heightfield or triangle mesh
 
@@ -341,14 +365,14 @@ tr.setOrigin(btVector3(0,-10,0));
 		loadTrans.setOrigin(btVector3(-2.1f, 0.0f, 0.0f));
 		loadCompound->addChildShape(loadTrans, loadShapeC);
 		loadTrans.setIdentity();
-		m_loadStartPos = btVector3(0.0f, -3.5f, 7.0f);
+		m_loadStartPos = btVector3(0.0f, 3.5f, 7.0f);
 		loadTrans.setOrigin(m_loadStartPos);
 		m_loadBody  = localCreateRigidBody(4, loadTrans, loadCompound);
 	}
 
 
 
-	clientResetScene();
+	
 	
 	/// create vehicle
 	{
@@ -408,6 +432,7 @@ tr.setOrigin(btVector3(0,-10,0));
 		}
 	}
 
+	resetForklift();
 	
 	setCameraDistance(26.f);
 
@@ -459,6 +484,25 @@ void ForkLiftDemo::renderme()
 		sprintf(buf,"SHIFT+Cursor UP/Down - move fork up/down");
 		yStart+=20;
 		GLDebugDrawString(xStart,yStart,buf);
+		
+		yStart+=20;
+		glRasterPos3f(xStart, yStart, 0);
+		sprintf(buf,"F6 - toggle solver");
+		GLDebugDrawString(xStart,yStart,buf);
+
+		yStart+=20;
+		glRasterPos3f(xStart, yStart, 0);
+		if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
+		{
+			sprintf(buf,"Using direct MLCP solver");
+		} else
+		{
+			sprintf(buf,"Using sequential impulse solver");
+		}
+		GLDebugDrawString(xStart,yStart,buf);
+
+		
+
 		glRasterPos3f(xStart, yStart, 0);
 		sprintf(buf,"F5 - toggle camera mode");
 		yStart+=20;
@@ -510,6 +554,19 @@ void ForkLiftDemo::clientMoveAndDisplay()
 		int numSimSteps;
         numSimSteps = m_dynamicsWorld->stepSimulation(dt,maxSimSubSteps);
 
+		if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
+		{
+			btMLCPSolver* sol = (btMLCPSolver*) m_dynamicsWorld->getConstraintSolver();
+			int numFallbacks = sol->getNumFallbacks();
+			if (numFallbacks)
+			{
+				static int totalFailures = 0;
+				totalFailures+=numFallbacks;
+				printf("MLCP solver failed %d times, falling back to btSequentialImpulseSolver (SI)\n",totalFailures);
+			}
+			sol->setNumFallbacks(0);
+		}
+
 
 //#define VERBOSE_FEEDBACK
 #ifdef VERBOSE_FEEDBACK
@@ -574,8 +631,13 @@ void ForkLiftDemo::displayCallback(void)
 }
 
 
-
 void ForkLiftDemo::clientResetScene()
+{
+	exitPhysics();
+	initPhysics();
+}
+
+void ForkLiftDemo::resetForklift()
 {
 	gVehicleSteering = 0.f;
 	gBreakingForce = defaultBreakingForce;
@@ -747,6 +809,32 @@ void ForkLiftDemo::specialKeyboard(int key, int x, int y)
 					break;
 				}
 
+			case GLUT_KEY_F6:
+				{
+					//switch solver (needs demo restart)
+					useMCLPSolver = !useMCLPSolver;
+					printf("switching to useMLCPSolver = %d\n", useMCLPSolver);
+
+					delete m_constraintSolver;
+					if (useMCLPSolver)
+					{
+						btDantzigSolver* mlcp = new btDantzigSolver();
+						//btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel;
+						btMLCPSolver* sol = new btMLCPSolver(mlcp);
+						m_constraintSolver = sol;
+					} else
+					{
+						m_constraintSolver = new btSequentialImpulseConstraintSolver();
+					}
+
+					m_dynamicsWorld->setConstraintSolver(m_constraintSolver);
+
+
+					//exitPhysics();
+					//initPhysics();
+					break;
+				}
+
 			case GLUT_KEY_F5:
 				m_useDefaultCamera = !m_useDefaultCamera;
 				break;

Demos/ForkLiftDemo/ForkLiftDemo.h

@@ -83,6 +83,8 @@ class ForkLiftDemo : public GlutDemoApplication
 
 	virtual void clientMoveAndDisplay();
 
+	virtual void	resetForklift();
+		
 	virtual void clientResetScene();
 
 	virtual void displayCallback();
@@ -97,7 +99,7 @@ class ForkLiftDemo : public GlutDemoApplication
 	void renderme();
 
 	void initPhysics();
-	void termPhysics();
+	void exitPhysics();
 
 	static DemoApplication* Create()
 	{

Demos/OpenGL/DemoApplication.cpp

@@ -817,7 +817,7 @@ void DemoApplication::pickObject(const btVector3& pickPos, const btCollisionObje
 
 			btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
 
-			if ((m_modifierKeys& BT_ACTIVE_SHIFT)==0)
+			if ((m_modifierKeys& BT_ACTIVE_SHIFT)!=0)
 			{
 				btTransform tr;
 				tr.setIdentity();

UseBullet.cmake

@@ -0,0 +1,10 @@
+#                                               -*- cmake -*-
+#
+#  UseBullet.cmake
+#
+
+
+add_definitions     ( ${BULLET_DEFINITIONS} )
+include_directories ( ${BULLET_INCLUDE_DIRS} )
+link_directories    ( ${BULLET_LIBRARY_DIRS} )
+

src/BulletDynamics/CMakeLists.txt

@@ -31,6 +31,8 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyConstraint.cpp
 	Featherstone/btMultiBodyPoint2Point.cpp
 	Featherstone/btMultiBodyJointMotor.cpp
+	MLCPSolvers/btDantzigLCP.cpp
+	MLCPSolvers/btMLCPSolver.cpp
 )
 
 SET(Root_HDRS
@@ -84,6 +86,16 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBodyPoint2Point.h
 	Featherstone/btMultiBodyJointMotor.h
 )
+
+SET(MLCPSolvers_HDRS
+	MLCPSolvers/btDantzigLCP.h
+	MLCPSolvers/btDantzigSolver.h
+	MLCPSolvers/btMLCPSolver.h
+	MLCPSolvers/btMLCPSolverInterface.h
+	MLCPSolvers/btPATHSolver.h
+	MLCPSolvers/btSolveProjectedGaussSeidel.h	
+)
+
 SET(Character_HDRS
 	Character/btCharacterControllerInterface.h
 	Character/btKinematicCharacterController.h
@@ -98,6 +110,7 @@ SET(BulletDynamics_HDRS
 	${Vehicle_HDRS}
 	${Character_HDRS}
 	${Featherstone_HDRS}
+	${MLCPSolvers_HDRS}
 )
 
 
@@ -134,6 +147,7 @@ DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics)
 			SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle)
 			SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character)
 			SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone)
+			SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers)
 		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
 	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
 ENDIF (INSTALL_LIBS)

src/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -28,6 +28,14 @@ class btIDebugDraw;
 class btStackAlloc;
 class	btDispatcher;
 /// btConstraintSolver provides solver interface
+
+
+enum btConstraintSolverType
+{
+	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
+	BT_MLCP_SOLVER=2
+};
+
 class btConstraintSolver
 {
 
@@ -44,6 +52,10 @@ public:
 
 	///clear internal cached data and reset random seed
 	virtual	void	reset() = 0;
+
+	virtual btConstraintSolverType	getSolverType() const=0;
+
+
 };
 
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -134,6 +134,11 @@ public:
 		return m_btSeed2;
 	}
 
+	
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_SEQUENTIAL_IMPULSE_SOLVER;
+	}
 };
 
 

src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

@@ -1782,7 +1782,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
 {
 	s_error = false;
 
-	printf("btSolveDantzigLCP n=%d\n",n);
+//	printf("btSolveDantzigLCP n=%d\n",n);
   btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
 #ifdef BT_DEBUG
   {
@@ -2011,7 +2011,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
         // our fingers and exit with the current solution.
         if (s <= btScalar(0.0)) 
 		{
-          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
+//          printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
           if (i < n) {
             btSetZero (x+i,n-i);
             btSetZero (w+i,n-i);

src/BulletDynamics/MLCPSolvers/btDantzigSolver.h

@@ -28,6 +28,14 @@ protected:
 	btScalar m_acceptableUpperLimitSolution;
 
 	btAlignedObjectArray<char>	m_tempBuffer;
+
+	btAlignedObjectArray<btScalar> m_A;
+	btAlignedObjectArray<btScalar> m_b;
+	btAlignedObjectArray<btScalar> m_x;
+	btAlignedObjectArray<btScalar> m_lo;
+	btAlignedObjectArray<btScalar> m_hi;
+	btAlignedObjectArray<int>	m_dependencies;
+
 public:
 
 	btDantzigSolver()
@@ -41,47 +49,46 @@ public:
 		int n = b.rows();
 		if (n)
 		{
-			btScalar* AA = (btScalar*) A.getBufferPointer();
-			btScalar* bb = (btScalar* ) b.getBufferPointer();
-			btScalar* xx = (btScalar*) x.getBufferPointer();
-			btScalar* llo = (btScalar*) lo.getBufferPointer();
-			btScalar* hhi = (btScalar*) hi.getBufferPointer();
-			int* findex = (int*) &limitDependency[0];
 			int nub = 0;
 			btAlignedObjectArray<btScalar> ww;
 			ww.resize(n);
 	
 
-
 			const btScalar* Aptr = A.getBufferPointer();
-
+			m_A.resize(n*n);
 			for (int i=0;i<n*n;i++)
 			{
-				AA[i] = Aptr[i];
+				m_A[i] = Aptr[i];
+
 			}
+
+			m_b.resize(n);
+			m_x.resize(n);
+			m_lo.resize(n);
+			m_hi.resize(n);
+			m_dependencies.resize(n);
 			for (int i=0;i<n;i++)
 			{
-				llo[i] = lo[i];
+				m_lo[i] = lo[i];
-				hhi[i] = hi[i];
+				m_hi[i] = hi[i];
-				bb[i] = b[i];
+				m_b[i] = b[i];
-				xx[i] = x[i];
+				m_x[i] = x[i];
+				m_dependencies[i] = limitDependency[i];
 			}
 
 			extern int numAllocas;
 			numAllocas = 0;
 
-			result = btSolveDantzigLCP (n,AA,xx,bb,&ww[0],nub,llo,hhi,findex);
+			result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0]);
+			if (!result)
+				return result;
 
 //			printf("numAllocas = %d\n",numAllocas);
 			for (int i=0;i<n;i++)
 			{
-				x[i] = xx[i];
+				volatile btScalar xx = m_x[i];
-
+				if (xx != m_x[i])
-				//test for NAN
-				if (x[i] != xx[i])
-				{
 					return false;
-				}
 				if (x[i] >= m_acceptableUpperLimitSolution)
 				{
 					return false;
@@ -91,7 +98,11 @@ public:
 				{
 					return false;
 				}
+			}
 
+			for (int i=0;i<n;i++)
+			{
+				x[i] = m_x[i];
 			}
 			
 		}

src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -20,7 +20,8 @@ subject to the following restrictions:
 #include "btSolveProjectedGaussSeidel.h"
 
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
-:m_solver(solver)
+:m_solver(solver),
+m_fallback(0)
 {
 }
 
@@ -56,21 +57,21 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
  
 		///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
 		
-
+		int firstContactConstraintOffset=dindex;
 
 		if (interleaveContactAndFriction)
 		{
 			for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
 			{
-				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;
+				int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
+				m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
 				if (numFrictionPerContact==2)
 				{
 					m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
-					m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1].m_frictionIndex;//findex;
+					m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
 				}
 			}
 		} else
@@ -83,7 +84,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 			for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
 			{
 				m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
-				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex;
+				m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
 			}
 			
 		}
@@ -117,7 +118,26 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
 
 bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 {
-	 return m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+	bool result = true;
+
+	if (m_A.rows()==0)
+		return true;
+
+	//if using split impulse, we solve 2 separate (M)LCPs
+	if (infoGlobal.m_splitImpulse)
+	{
+		btMatrixXu Acopy = m_A;
+		btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
+//		printf("solve first LCP\n");
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+		if (result)
+			result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
+
+	} else
+	{
+		result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
+	}
+	return result;
 }
 
 struct btJointNode
@@ -139,11 +159,16 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		BT_PROFILE("init b (rhs)");
 		m_b.resize(numConstraintRows);
+		m_bSplit.resize(numConstraintRows);
 		//m_b.setZero();
 		for (int i=0;i<numConstraintRows ;i++)
 		{
 			if (m_allConstraintArray[i].m_jacDiagABInv)
+			{
 				m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+			}
+
 		}
 	}
 
@@ -420,16 +445,20 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	{
 		BT_PROFILE("resize/init x");
 		m_x.resize(numConstraintRows);
+		m_xSplit.resize(numConstraintRows);
+
 		if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
 		{
 			for (int i=0;i<m_allConstraintArray.size();i++)
 			{
 				const btSolverConstraint& c = m_allConstraintArray[i];
 				m_x[i]=c.m_appliedImpulse;
+				m_xSplit[i] = c.m_appliedPushImpulse;
 			}
 		} else
 		{
 			m_x.setZero();
+			m_xSplit.setZero();
 		}
 	}
 
@@ -441,12 +470,17 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	int numConstraintRows = m_allConstraintArray.size();
 
 	m_b.resize(numConstraintRows);
-//	m_b.setZero();
+	if (infoGlobal.m_splitImpulse)
+		m_bSplit.resize(numConstraintRows);
  
 	for (int i=0;i<numConstraintRows ;i++)
 	{
 		if (m_allConstraintArray[i].m_jacDiagABInv)
+		{
 			m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
+			if (infoGlobal.m_splitImpulse)
+				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+		}
 	}
  
 	static btMatrixXu Minv;
@@ -530,13 +564,16 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	}
 
 	m_x.resize(numConstraintRows);
+	if (infoGlobal.m_splitImpulse)
+		m_xSplit.resize(numConstraintRows);
 //	m_x.setZero();
 
 	for (int i=0;i<m_allConstraintArray.size();i++)
 	{
 		const btSolverConstraint& c = m_allConstraintArray[i];
 		m_x[i]=c.m_appliedImpulse;
-//		c.m_numRowsForNonContactConstraint
+		if (infoGlobal.m_splitImpulse)
+			m_xSplit[i] = c.m_appliedPushImpulse;
 	}
 
 }
@@ -569,13 +606,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
  
 				solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
 				solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
+				if (infoGlobal.m_splitImpulse)
+				{
+					solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
+					solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
+					c.m_appliedPushImpulse = m_xSplit[i];
+				}
 				c.m_appliedImpulse = m_x[i];
 			}
 		}
 	}
 	else
 	{
-		printf("fallback to btSequentialImpulseConstraintSolver\n");
+		m_fallback++;
 		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	}
 

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -26,17 +26,22 @@ class btMLCPSolver : public btSequentialImpulseConstraintSolver
 
 protected:
 	
-
 	btMatrixXu m_A;
 	btVectorXu m_b;
 	btVectorXu m_x;
 	btVectorXu m_lo;
 	btVectorXu m_hi;
 	
+	///when using 'split impulse' we solve two separate (M)LCPs
+	btVectorXu m_bSplit;
+	btVectorXu m_xSplit;
+	btVectorXu m_bSplit1;
+	btVectorXu m_xSplit2;
+
 	btAlignedObjectArray<int> m_limitDependencies;
 	btConstraintArray m_allConstraintArray;
-
 	btMLCPSolverInterface* m_solver;
+	int m_fallback;
 
 	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);
@@ -56,6 +61,20 @@ public:
 		m_solver = solver;
 	}
 
+	int getNumFallbacks() const
+	{
+		return m_fallback;
+	}
+	void setNumFallbacks(int num)
+	{
+		m_fallback = num;
+	}
+
+	virtual btConstraintSolverType	getSolverType() const
+	{
+		return BT_MLCP_SOLVER;
+	}
+
 };
 
 

src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h

@@ -72,6 +72,7 @@ public:
 					x[i]=hi[i]*s;
 			}
 		}
+		return true;
 	}
 
 };