Add the option for the btSimulationIslandManager to avoid splitting islands (for constraint solving)

Move the convertContact inside constraint solver to its own method

Demos/BasicDemo/BasicDemo.cpp

@@ -13,17 +13,27 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+#define LARGE_DEMO 0
+
+#if LARGE_DEMO
+	///create 512 (8x8x8) dynamic object
+	#define ARRAY_SIZE_X 8
+	#define ARRAY_SIZE_Y 8
+	#define ARRAY_SIZE_Z 8
+#else
+	///create 125 (5x5x5) dynamic object
+	#define ARRAY_SIZE_X 5
+	#define ARRAY_SIZE_Y 5
+	#define ARRAY_SIZE_Z 5
+#endif
 
-///create 125 (5x5x5) dynamic object
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_Z 5
 
 //maximum number of objects (and allow user to shoot additional boxes)
 #define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)
 
 ///scaling of the objects (0.1 = 20 centimeter boxes )
-#define SCALING 0.1
+//#define SCALING 0.1
+#define SCALING 1
 #define START_POS_X -5
 #define START_POS_Y -5
 #define START_POS_Z -3
@@ -34,6 +44,9 @@ subject to the following restrictions:
 #include "btBulletDynamicsCommon.h"
 #include <stdio.h> //printf debugging
 
+#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btParallelBatchConstraintSolver2.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 
 void BasicDemo::clientMoveAndDisplay()
 {
@@ -46,6 +59,10 @@ void BasicDemo::clientMoveAndDisplay()
 	if (m_dynamicsWorld)
 	{
 		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
+		if (m_idle)
+		{
+			CProfileManager::dumpAll();
+		}
 		//optional but useful: debug drawing
 		m_dynamicsWorld->debugDrawWorld();
 	}
@@ -75,6 +92,10 @@ void BasicDemo::displayCallback(void) {
 }
 
 
+static btSequentialImpulseConstraintSolver* sDefSeqImpSolver = 0;
+static btParallelBatchConstraintSolver* sParallelBatchSolver = 0;
+static btParallelBatchConstraintSolver2* sParallelBatchSolver2 = 0;
+
 
 
 
@@ -94,22 +115,31 @@ void	BasicDemo::initPhysics()
 	m_broadphase = new btDbvtBroadphase();
 
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
-	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+	sDefSeqImpSolver = new btSequentialImpulseConstraintSolver;
-	m_solver = sol;
+	// create parallel batch solver for tests 
+	sParallelBatchSolver = new btParallelBatchConstraintSolver();
+	sParallelBatchSolver2 = new btParallelBatchConstraintSolver2();
+	
+	// start with parallel batch solver
+	m_solver = sParallelBatchSolver;
 
-	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
+	btDiscreteDynamicsWorld* ddw = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
+	m_dynamicsWorld = ddw;
+	ddw->getSimulationIslandManager()->setSplitIslands(false);
 
 	m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 
 	///create a few basic rigid bodies
-	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(30.),btScalar(1.),btScalar(30.)));
+//	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
 //	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
 	
 	m_collisionShapes.push_back(groundShape);
 
 	btTransform groundTransform;
 	groundTransform.setIdentity();
-	groundTransform.setOrigin(btVector3(0,-50,0));
+	groundTransform.setOrigin(btVector3(0,-5,0));
+//	groundTransform.setOrigin(btVector3(0,-50,0));
 
 	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
 	{
@@ -215,7 +245,9 @@ void	BasicDemo::exitPhysics()
 
 	delete m_dynamicsWorld;
 	
-	delete m_solver;
+	m_solver = 0;
+	delete sDefSeqImpSolver;
+	delete sParallelBatchSolver;
 	
 	delete m_broadphase;
 	
@@ -228,4 +260,48 @@ void	BasicDemo::exitPhysics()
 
 
 
+void BasicDemo::keyboardCallback(unsigned char key, int x, int y)
+{
+	(void)x;
+	(void)y;
+	switch (key) 
+	{
+		case 'q' : 
+			exitPhysics();
+			break;
+		case 'S' :
+			{
+				btConstraintSolver* curr_solver = m_dynamicsWorld->getConstraintSolver();
+				btDiscreteDynamicsWorld* pDdw = (btDiscreteDynamicsWorld*)m_dynamicsWorld;
+				if(curr_solver == sDefSeqImpSolver)
+				{
+					static bool toggle = true;
+					toggle=!toggle;
+					if (toggle)
+					{
+						pDdw->setConstraintSolver(sParallelBatchSolver);
+						pDdw->getSimulationIslandManager()->setSplitIslands(false);
+						printf("\nUsing ParallelBatch constraint solver\n");
+					} else
+					{
+						pDdw->setConstraintSolver(sParallelBatchSolver2);
+						pDdw->getSimulationIslandManager()->setSplitIslands(false);
+						printf("\nUsing ParallelBatch constraint solver2\n");
+
+					}
+				}
+				else
+				{
+					m_dynamicsWorld->setConstraintSolver(sDefSeqImpSolver);
+					pDdw->getSimulationIslandManager()->setSplitIslands(true);
+					printf("\nUsing default SequentialImpulse constraint solver\n");
+				}
+			}
+			break;
+		default : 
+			break;
+	}
+	DemoApplication::keyboardCallback(key, x, y);
+}
+
 

Demos/BasicDemo/BasicDemo.h

@@ -57,6 +57,8 @@ class BasicDemo : public DemoApplication
 	virtual void clientMoveAndDisplay();
 
 	virtual void displayCallback();
+
+	virtual void keyboardCallback(unsigned char key, int x, int y);
 	
 	static DemoApplication* Create()
 	{

Demos/OpenGL/DemoApplication.cpp

@@ -1269,6 +1269,7 @@ void	DemoApplication::clientResetScene()
 		numObjects = m_dynamicsWorld->getNumCollisionObjects();
 	}
 
+	///create a copy of the array, not a reference!
 	btCollisionObjectArray copyArray = m_dynamicsWorld->getCollisionObjectArray();
 
 	for (i=0;i<copyArray.size();i++)
@@ -1297,8 +1298,8 @@ void	DemoApplication::clientResetScene()
 				colObj->setDeactivationTime(0);
 				//colObj->setActivationState(WANTS_DEACTIVATION);
 			}
-			//removed cached contact points
+			//removed cached contact points (this is not necessary if all objects have been removed from the dynamics world)
-			//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
+			m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 
 			btRigidBody* body = btRigidBody::upcast(colObj);
 			if (body && !body->isStaticObject())
@@ -1310,6 +1311,7 @@ void	DemoApplication::clientResetScene()
 
 	}
 
+	///reset some internal cached data in the broadphase
 	m_dynamicsWorld->getBroadphase()->resetPool(getDynamicsWorld()->getDispatcher());
 	m_dynamicsWorld->getConstraintSolver()->reset();
 	

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -24,7 +24,8 @@ subject to the following restrictions:
 //#include <stdio.h>
 #include "LinearMath/btQuickprof.h"
 
-btSimulationIslandManager::btSimulationIslandManager()
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
 {
 }
 
@@ -251,11 +252,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 	int i;
 	int maxNumManifolds = dispatcher->getNumManifolds();
 
-#define SPLIT_ISLANDS 1
+//#define SPLIT_ISLANDS 1
-#ifdef SPLIT_ISLANDS
+//#ifdef SPLIT_ISLANDS
 
 	
-#endif //SPLIT_ISLANDS
+//#endif //SPLIT_ISLANDS
 
 	
 	for (i=0;i<maxNumManifolds ;i++)
@@ -279,11 +280,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 			{
 				colObj0->activate();
 			}
-#ifdef SPLIT_ISLANDS
+			if(m_splitIslands)
-	//		//filtering for response
+			{ 
-			if (dispatcher->needsResponse(colObj0,colObj1))
+				//filtering for response
-				m_islandmanifold.push_back(manifold);
+				if (dispatcher->needsResponse(colObj0,colObj1))
-#endif //SPLIT_ISLANDS
+					m_islandmanifold.push_back(manifold);
+			}
 		}
 	}
 }
@@ -303,84 +305,86 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 	BT_PROFILE("processIslands");
 
-#ifndef SPLIT_ISLANDS
+	if(!m_splitIslands)
-	btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
-	
-	callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
-#else
-	// Sort manifolds, based on islands
-	// Sort the vector using predicate and std::sort
-	//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
-
-	int numManifolds = int (m_islandmanifold.size());
-
-	//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
-	m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
-
-	//now process all active islands (sets of manifolds for now)
-
-	int startManifoldIndex = 0;
-	int endManifoldIndex = 1;
-
-	//int islandId;
-
-	
-
-//	printf("Start Islands\n");
-
-	//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
-	for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
 	{
-		int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+		btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+		int maxNumManifolds = dispatcher->getNumManifolds();
+		callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+	}
+	else
+	{
+		// Sort manifolds, based on islands
+		// Sort the vector using predicate and std::sort
+		//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
 
+		int numManifolds = int (m_islandmanifold.size());
 
-	       bool islandSleeping = false;
+		//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
-                
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
-                for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
-                {
-                        int i = getUnionFind().getElement(endIslandIndex).m_sz;
-                        btCollisionObject* colObj0 = collisionObjects[i];
-						m_islandBodies.push_back(colObj0);
-                        if (!colObj0->isActive())
-                                islandSleeping = true;
-                }
-                
 
-		//find the accompanying contact manifold for this islandId
+		//now process all active islands (sets of manifolds for now)
-		int numIslandManifolds = 0;
-		btPersistentManifold** startManifold = 0;
 
-		if (startManifoldIndex<numManifolds)
+		int startManifoldIndex = 0;
+		int endManifoldIndex = 1;
+
+		//int islandId;
+
+		
+
+	//	printf("Start Islands\n");
+
+		//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+		for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
 		{
-			int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+			int islandId = getUnionFind().getElement(startIslandIndex).m_id;
-			if (curIslandId == islandId)
-			{
-				startManifold = &m_islandmanifold[startManifoldIndex];
-			
-				for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
-				{
 
+
+			   bool islandSleeping = false;
+	                
+					for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+					{
+							int i = getUnionFind().getElement(endIslandIndex).m_sz;
+							btCollisionObject* colObj0 = collisionObjects[i];
+							m_islandBodies.push_back(colObj0);
+							if (!colObj0->isActive())
+									islandSleeping = true;
+					}
+	                
+
+			//find the accompanying contact manifold for this islandId
+			int numIslandManifolds = 0;
+			btPersistentManifold** startManifold = 0;
+
+			if (startManifoldIndex<numManifolds)
+			{
+				int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
+				if (curIslandId == islandId)
+				{
+					startManifold = &m_islandmanifold[startManifoldIndex];
+				
+					for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
+					{
+
+					}
+					/// Process the actual simulation, only if not sleeping/deactivated
+					numIslandManifolds = endManifoldIndex-startManifoldIndex;
 				}
-				/// Process the actual simulation, only if not sleeping/deactivated
+
-				numIslandManifolds = endManifoldIndex-startManifoldIndex;
 			}
 
-		}
+			if (!islandSleeping)
+			{
+				callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
+	//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+			}
+			
+			if (numIslandManifolds)
+			{
+				startManifoldIndex = endManifoldIndex;
+			}
 
-		if (!islandSleeping)
+			m_islandBodies.resize(0);
-		{
-			callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
-//			printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
 		}
-		
+	} // else if(!splitIslands) 
-		if (numIslandManifolds)
-		{
-			startManifoldIndex = endManifoldIndex;
-		}
-
-		m_islandBodies.resize(0);
-	}
-#endif //SPLIT_ISLANDS
-
 
 }

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h

@@ -35,6 +35,7 @@ class btSimulationIslandManager
 	btAlignedObjectArray<btPersistentManifold*>  m_islandmanifold;
 	btAlignedObjectArray<btCollisionObject* >  m_islandBodies;
 	
+	bool m_splitIslands;
 	
 public:
 	btSimulationIslandManager();
@@ -65,6 +66,15 @@ public:
 
 	void buildIslands(btDispatcher* dispatcher,btCollisionWorld* colWorld);
 
+	bool getSplitIslands()
+	{
+		return m_splitIslands;
+	}
+	void setSplitIslands(bool doSplitIslands)
+	{
+		m_splitIslands = doSplitIslands;
+	}
+
 };
 
 #endif //SIMULATION_ISLAND_MANAGER_H

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -120,7 +120,6 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra
 		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
 }
 
-
 SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
@@ -269,7 +268,7 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 
 	solverConstraint.m_appliedImpulse = 0.f;
 	//	solverConstraint.m_appliedPushImpulse = 0.f;
-	solverConstraint.m_penetration = 0.f;
+
 	{
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
@@ -324,10 +323,8 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 		rel_vel = vel1Dotn+vel2Dotn;
 
 		btScalar positionalError = 0.f;
-		positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
-		solverConstraint.m_restitution=0.f;
 
-		btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel;
+		btSimdScalar velocityError =  - rel_vel;
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
 		solverConstraint.m_cfm = 0.f;
@@ -364,6 +361,249 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 }
 #include <stdio.h>
 
+
+
+void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
+{
+	btCollisionObject* colObj0=0,*colObj1=0;
+
+	colObj0 = (btCollisionObject*)manifold->getBody0();
+	colObj1 = (btCollisionObject*)manifold->getBody1();
+
+	int solverBodyIdA=-1;
+	int solverBodyIdB=-1;
+
+	if (manifold->getNumContacts())
+	{
+		solverBodyIdA = getOrInitSolverBody(*colObj0);
+		solverBodyIdB = getOrInitSolverBody(*colObj1);
+	}
+
+	btVector3 rel_pos1;
+	btVector3 rel_pos2;
+	btScalar relaxation;
+
+	for (int j=0;j<manifold->getNumContacts();j++)
+	{
+
+		btManifoldPoint& cp = manifold->getContactPoint(j);
+
+		///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! 
+		///-> if (cp.getDistance() <= btScalar(0.))
+		//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
+		{
+
+			const btVector3& pos1 = cp.getPositionWorldOnA();
+			const btVector3& pos2 = cp.getPositionWorldOnB();
+
+			rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+			rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
+
+
+			relaxation = 1.f;
+			btScalar rel_vel;
+			btVector3 vel;
+
+			int frictionIndex = m_tmpSolverContactConstraintPool.size();
+
+			{
+				btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
+				btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+				btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+
+				solverConstraint.m_solverBodyIdA = solverBodyIdA;
+				solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+				solverConstraint.m_originalContactPoint = &cp;
+
+				btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+				solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
+				btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+				solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1 : btVector3(0,0,0);
+				{
+#ifdef COMPUTE_IMPULSE_DENOM
+					btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+					btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else							
+					btVector3 vec;
+					btScalar denom0 = 0.f;
+					btScalar denom1 = 0.f;
+					if (rb0)
+					{
+						vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+						denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+					}
+					if (rb1)
+					{
+						vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+						denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+					}
+#endif //COMPUTE_IMPULSE_DENOM		
+
+					btScalar denom = relaxation/(denom0+denom1);
+					solverConstraint.m_jacDiagABInv = denom;
+				}
+
+				solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
+				solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
+				solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
+
+
+				btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+				btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+
+				vel  = vel1 - vel2;
+
+				rel_vel = cp.m_normalWorldOnB.dot(vel);
+
+				btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
+
+
+				solverConstraint.m_friction = cp.m_combinedFriction;
+
+				btScalar restitution = 0.f;
+				
+				if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
+				{
+					restitution = 0.f;
+				} else
+				{
+					restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+					if (restitution <= btScalar(0.))
+					{
+						restitution = 0.f;
+					};
+				}
+
+
+				///warm starting (or zero if disabled)
+				if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+					if (rb0)
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+					if (rb1)
+						m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+				} else
+				{
+					solverConstraint.m_appliedImpulse = 0.f;
+				}
+
+				//							solverConstraint.m_appliedPushImpulse = 0.f;
+
+				{
+					btScalar rel_vel;
+					btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
+					btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
+						+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
+
+					rel_vel = vel1Dotn+vel2Dotn;
+
+					btScalar positionalError = 0.f;
+					positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
+					btScalar	velocityError = restitution - rel_vel;// * damping;
+					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+					btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+					solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+					solverConstraint.m_cfm = 0.f;
+					solverConstraint.m_lowerLimit = 0;
+					solverConstraint.m_upperLimit = 1e10f;
+				}
+
+
+				/////setup the friction constraints
+
+
+
+				if (1)
+				{
+					solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+					if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+					{
+						cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+						btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+						if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
+						{
+							cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							{
+								cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+								cp.m_lateralFrictionDir2.normalize();//??
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
+							cp.m_lateralFrictionInitialized = true;
+						} else
+						{
+							//re-calculate friction direction every frame, todo: check if this is really needed
+							btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+							addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							{
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
+							cp.m_lateralFrictionInitialized = true;
+						}
+
+					} else
+					{
+						addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+							addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					}
+
+					if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
+					{
+						{
+							btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+							} else
+							{
+								frictionConstraint1.m_appliedImpulse = 0.f;
+							}
+						}
+
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+						{
+							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+							} else
+							{
+								frictionConstraint2.m_appliedImpulse = 0.f;
+							}
+						}
+					} else
+					{
+						btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+						frictionConstraint1.m_appliedImpulse = 0.f;
+						if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+						{
+							btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+							frictionConstraint2.m_appliedImpulse = 0.f;
+						}
+					}
+				}
+			}
+
+
+		}
+	}
+}
+
+
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
 	BT_PROFILE("solveGroupCacheFriendlySetup");
@@ -443,7 +683,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 						currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
 						currentConstraintRow[j].m_upperLimit = FLT_MAX;
 						currentConstraintRow[j].m_appliedImpulse = 0.f;
-						currentConstraintRow[j].m_penetration = 0.f;
 						currentConstraintRow[j].m_appliedPushImpulse = 0.f;
 						currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
 						currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
@@ -510,9 +749,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 							rel_vel = vel1Dotn+vel2Dotn;
 
+							btScalar restitution = 0.f;
 							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
-							solverConstraint.m_restitution  = 0.f;
+							btScalar	velocityError = restitution - rel_vel;// * damping;
-							btScalar	velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
 							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -533,258 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			for (i=0;i<numManifolds;i++)
 			{
 				manifold = manifoldPtr[i];
-				colObj0 = (btCollisionObject*)manifold->getBody0();
+				convertContact(manifold,infoGlobal);
-				colObj1 = (btCollisionObject*)manifold->getBody1();
-
-				int solverBodyIdA=-1;
-				int solverBodyIdB=-1;
-
-				if (manifold->getNumContacts())
-				{
-					solverBodyIdA = getOrInitSolverBody(*colObj0);
-					solverBodyIdB = getOrInitSolverBody(*colObj1);
-				}
-
-				if (solverBodyIdA == 0 && solverBodyIdB == 0)
-					continue;
-
-				btVector3 rel_pos1;
-				btVector3 rel_pos2;
-				btScalar relaxation;
-
-				for (int j=0;j<manifold->getNumContacts();j++)
-				{
-
-					btManifoldPoint& cp = manifold->getContactPoint(j);
-
-					///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! 
-					///-> if (cp.getDistance() <= btScalar(0.))
-					//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
-					{
-
-						const btVector3& pos1 = cp.getPositionWorldOnA();
-						const btVector3& pos2 = cp.getPositionWorldOnB();
-
-						rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
-						rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
-
-
-						relaxation = 1.f;
-						btScalar rel_vel;
-						btVector3 vel;
-
-						int frictionIndex = m_tmpSolverContactConstraintPool.size();
-
-						{
-							btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
-							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
-							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
-
-							solverConstraint.m_solverBodyIdA = solverBodyIdA;
-							solverConstraint.m_solverBodyIdB = solverBodyIdB;
-
-							solverConstraint.m_originalContactPoint = &cp;
-
-							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
-							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-							solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1 : btVector3(0,0,0);
-							{
-#ifdef COMPUTE_IMPULSE_DENOM
-								btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
-								btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
-#else							
-								btVector3 vec;
-								btScalar denom0 = 0.f;
-								btScalar denom1 = 0.f;
-								if (rb0)
-								{
-									vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
-									denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-								}
-								if (rb1)
-								{
-									vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
-									denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
-								}
-#endif //COMPUTE_IMPULSE_DENOM		
-
-								btScalar denom = relaxation/(denom0+denom1);
-								solverConstraint.m_jacDiagABInv = denom;
-							}
-
-							solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
-							solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
-
-
-							btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
-							btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
-
-							vel  = vel1 - vel2;
-
-							rel_vel = cp.m_normalWorldOnB.dot(vel);
-
-							solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop;
-							//solverConstraint.m_penetration = cp.getDistance();
-
-
-
-
-
-
-
-
-							solverConstraint.m_friction = cp.m_combinedFriction;
-
-
-							if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
-							{
-								solverConstraint.m_restitution = 0.f;
-							} else
-							{
-								solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-								if (solverConstraint.m_restitution <= btScalar(0.))
-								{
-									solverConstraint.m_restitution = 0.f;
-								};
-							}
-
-
-							///warm starting (or zero if disabled)
-							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-							{
-								solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-								if (rb0)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-								if (rb1)
-									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
-							} else
-							{
-								solverConstraint.m_appliedImpulse = 0.f;
-							}
-
-							//							solverConstraint.m_appliedPushImpulse = 0.f;
-
-							{
-								btScalar rel_vel;
-								btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
-									+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
-								btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
-									+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
-
-								rel_vel = vel1Dotn+vel2Dotn;
-
-								btScalar positionalError = 0.f;
-								positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
-								btScalar	velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
-								btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
-								btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
-								solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
-								solverConstraint.m_cfm = 0.f;
-								solverConstraint.m_lowerLimit = 0;
-								solverConstraint.m_upperLimit = 1e10f;
-							}
-
-
-#ifdef _USE_JACOBIAN
-							solverConstraint.m_jac =  btJacobianEntry (
-								rel_pos1,rel_pos2,cp.m_normalWorldOnB,
-								rb0->getInvInertiaDiagLocal(),
-								rb0->getInvMass(),
-								rb1->getInvInertiaDiagLocal(),
-								rb1->getInvMass());
-#endif //_USE_JACOBIAN
-
-							/////setup the friction constraints
-
-
-
-							if (1)
-							{
-								solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
-								if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
-								{
-									cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
-									btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
-									if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
-									{
-										cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
-										addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-										if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-										{
-											cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
-											cp.m_lateralFrictionDir2.normalize();//??
-											addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-										}
-										cp.m_lateralFrictionInitialized = true;
-									} else
-									{
-										//re-calculate friction direction every frame, todo: check if this is really needed
-										btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
-										addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-										if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-										{
-											addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-										}
-										cp.m_lateralFrictionInitialized = true;
-									}
-
-								} else
-								{
-									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-									if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-										addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-								}
-
-								if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
-								{
-									{
-										btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
-										if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-										{
-											frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
-											if (rb0)
-												m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
-											if (rb1)
-												m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
-										} else
-										{
-											frictionConstraint1.m_appliedImpulse = 0.f;
-										}
-									}
-
-									if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-									{
-										btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-										if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-										{
-											frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
-											if (rb0)
-												m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
-											if (rb1)
-												m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
-										} else
-										{
-											frictionConstraint2.m_appliedImpulse = 0.f;
-										}
-									}
-								} else
-								{
-									btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
-									frictionConstraint1.m_appliedImpulse = 0.f;
-									if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-									{
-										btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
-										frictionConstraint2.m_appliedImpulse = 0.f;
-									}
-								}
-							}
-						}
-
-
-					}
-				}
 			}
 		}
 	}

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -30,6 +30,7 @@ class btIDebugDraw;
 ///Applies impulses for combined restitution and penetration recovery and to simulate friction
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+protected:
 
 	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
 	btConstraintArray			m_tmpSolverContactConstraintPool;
@@ -38,7 +39,6 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
 	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
 
-protected:
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
 	
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
@@ -47,6 +47,8 @@ protected:
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
 	btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 
+	void	convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
+
 	void	resolveSplitPenetrationImpulseCacheFriendly(
         btSolverBody& body1,
         btSolverBody& body2,

src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -44,10 +44,12 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	
 	
 	btScalar	m_friction;
-	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
-	btScalar	m_penetration;
+	union
-
+	{
+		int	m_numConsecutiveRowsPerKernel;
+		btScalar	m_unusedPadding0;
+	};
 
 	union
 	{

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -665,8 +665,11 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		{
 			if (islandId<0)
 			{
-				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				if (numManifolds + m_numConstraints)
-				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				{
+					///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
 			} else
 			{
 					//also add all non-contact constraints/joints for this island

src/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.cpp

@@ -1238,7 +1238,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
 									vel = vel1 - vel2;
 									rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-									constraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
+									btScalar penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
 									constraint.m_friction = cp.m_combinedFriction;
 									float rest =  - rel_vel * cp.m_combinedRestitution;
 									if (rest <= btScalar(0.))
@@ -1251,7 +1251,7 @@ void processSolverTask(void* userPtr, void* lsMemory)
 									btScalar erp = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_erp;
 									btScalar timeStep = taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_timeStep;
 																	
-									constraint.m_restitution = rest;
+									btScalar restitution = rest;
 									constraint.m_appliedImpulse = cp.m_appliedImpulse*taskDesc.m_commandData.m_manifoldSetup.m_solverInfo.m_warmstartingFactor;
 									if (constraint.m_appliedImpulse!= 0.f)
 									{
@@ -1271,8 +1271,8 @@ void processSolverTask(void* userPtr, void* lsMemory)
 										rel_vel = vel1Dotn-vel2Dotn;
 
 										btScalar positionalError = 0.f;
-										positionalError = -constraint.m_penetration * erp/timeStep;
+										positionalError = -penetration * erp/timeStep;
-										btScalar	velocityError = constraint.m_restitution - rel_vel;// * damping;
+										btScalar	velocityError = restitution - rel_vel;// * damping;
 										btScalar  penetrationImpulse = positionalError*constraint.m_jacDiagABInv;
 										btScalar velocityImpulse = velocityError *constraint.m_jacDiagABInv;
 										constraint.m_rhs = penetrationImpulse+velocityImpulse;
@@ -1339,9 +1339,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
 
 										btScalar positionalError = 0.f;
 										positionalError = 0;
-										constraint.m_restitution=0.f;
+										btScalar restitution=0.f;
 
-										btSimdScalar velocityError = constraint.m_restitution - rel_vel;
+										btSimdScalar velocityError = restitution - rel_vel;
 										btSimdScalar	velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
 										constraint.m_rhs = velocityImpulse;
 										constraint.m_cfm = 0.f;
@@ -1386,9 +1386,9 @@ void processSolverTask(void* userPtr, void* lsMemory)
 
 										btScalar positionalError = 0.f;
 										positionalError = 0;
-										constraint.m_restitution=0.f;
+										btScalar restitution=0.f;
 
-										btSimdScalar velocityError = constraint.m_restitution - rel_vel;
+										btSimdScalar velocityError = restitution - rel_vel;
 										btSimdScalar	velocityImpulse = velocityError * btSimdScalar(constraint.m_jacDiagABInv);
 										constraint.m_rhs = velocityImpulse;
 										constraint.m_cfm = 0.f;