Merge pull request #2141 from erwincoumans/blocksolver

solver experiment

12 files changed, 1264 insertions, 216 deletions

diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp
new file mode 100644
index 000000000..f3ab0cfed
--- /dev/null
+++ b/examples/BlockSolver/BlockSolverExample.cpp
@@ -0,0 +1,219 @@
+#include "BlockSolverExample.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "btBlockSolver.h"
+//for URDF import support
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+class BlockSolverExample : public CommonMultiBodyBase
+{
+	int m_option;
+public:
+	BlockSolverExample(GUIHelperInterface* helper, int option);
+	virtual ~BlockSolverExample();
+
+	virtual void initPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+
+	virtual void resetCamera()
+	{
+		float dist = 3;
+		float pitch = -35;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, .1};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+	void createMultiBodyStack();
+	btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
+	btMultiBody* loadRobot(std::string filepath);
+};
+
+
+BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
+	: CommonMultiBodyBase(helper),
+	m_option(option)
+{
+	m_guiHelper->setUpAxis(2);
+}
+
+BlockSolverExample::~BlockSolverExample()
+{
+	// Do nothing
+}
+
+void BlockSolverExample::stepSimulation(float deltaTime)
+{
+	//use a smaller internal timestep, there are stability issues
+	btScalar internalTimeStep = 1./240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+}
+
+void BlockSolverExample::initPhysics()
+{
+	
+	///collision configuration contains default setup for memory, collision setup
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+
+
+	btMLCPSolverInterface* mlcp;
+	if (m_option&BLOCK_SOLVER_SI)
+	{
+		btAssert(!m_solver);
+		m_solver = new btMultiBodyConstraintSolver;
+		b3Printf("Constraint Solver: Sequential Impulse");
+	}
+	if (m_option&BLOCK_SOLVER_MLCP_PGS)
+	{
+		btAssert(!m_solver);
+		mlcp = new btSolveProjectedGaussSeidel();
+		m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+		b3Printf("Constraint Solver: MLCP + PGS");
+	}
+	if (m_option&BLOCK_SOLVER_MLCP_DANTZIG)
+	{
+		btAssert(!m_solver);
+		mlcp = new btDantzigSolver();
+		m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+		b3Printf("Constraint Solver: MLCP + Dantzig");
+	}
+	if (m_option&BLOCK_SOLVER_BLOCK)
+	{
+		m_solver = new btBlockSolver();
+	}
+
+	btAssert(m_solver);
+
+	
+
+	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_dynamicsWorld = world;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+	m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);  //todo: what value is good?
+
+	if (m_option&BLOCK_SOLVER_SCENE_MB_STACK)
+	{
+		createMultiBodyStack();
+	}
+	
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+void BlockSolverExample::createMultiBodyStack()
+{
+	///create a few basic rigid bodies
+	bool loadPlaneFromURDF = false;
+	if (loadPlaneFromURDF)
+	{
+		btMultiBody* mb = loadRobot("plane.urdf");
+		printf("!\n");
+	}
+	else
+	{
+		btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
+		m_collisionShapes.push_back(groundShape);
+		btScalar mass = 0;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, -50));
+		btMultiBody* body = createMultiBody(mass, tr, groundShape);
+	}
+
+	for (int i=0;i<10;i++)
+	{
+		btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
+		m_collisionShapes.push_back(boxShape);
+		btScalar mass = 1;
+		if (i == 9)
+			mass = 100;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 0.1+i*0.2));
+		btMultiBody* body = createMultiBody(mass, tr, boxShape);
+	}
+	if(0)
+	{
+		btMultiBody* mb = loadRobot("cube_small.urdf");
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 1.));
+		mb->setBaseWorldTransform(tr);
+	}
+}
+
+btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape)
+{
+	btVector3 inertia;
+	collisionShape->calculateLocalInertia(mass, inertia);
+
+	bool canSleep = false;
+	bool isDynamic = mass > 0;
+	btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic, canSleep);
+	btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1);
+	collider->setWorldTransform(trans);
+	mb->setBaseWorldTransform(trans);
+
+	collider->setCollisionShape(collisionShape);
+
+	int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+	int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+
+	this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
+	mb->setBaseCollider(collider);
+
+	mb->finalizeMultiDof();
+
+
+	this->m_dynamicsWorld->addMultiBody(mb);
+	m_dynamicsWorld->forwardKinematics();
+	return mb;
+}
+
+
+
+btMultiBody* BlockSolverExample::loadRobot(std::string filepath)
+{
+	btMultiBody* m_multiBody = 0;
+	BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
+	bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf");
+	if (loadOk)
+	{
+		int rootLinkIndex = u2b.getRootLinkIndex();
+		b3Printf("urdf root link index = %d\n", rootLinkIndex);
+		MyMultiBodyCreator creation(m_guiHelper);
+		btTransform identityTrans;
+		identityTrans.setIdentity();
+		ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix());
+		for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+		{
+			m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+		}
+		m_multiBody = creation.getBulletMultiBody();
+		if (m_multiBody)
+		{
+			b3Printf("Root link name = %s", u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
+		}
+	}
+	return m_multiBody;
+}
+
+CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new BlockSolverExample(options.m_guiHelper, options.m_option);
+}
diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h
new file mode 100644
index 000000000..9d2c137be
--- /dev/null
+++ b/examples/BlockSolver/BlockSolverExample.h
@@ -0,0 +1,19 @@
+
+#ifndef BLOCK_SOLVER_EXAMPLE_H
+#define BLOCK_SOLVER_EXAMPLE_H
+
+enum BlockSolverOptions
+{
+	BLOCK_SOLVER_SI=1<<0,
+	BLOCK_SOLVER_MLCP_PGS = 1 << 1,
+	BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
+	BLOCK_SOLVER_BLOCK = 1 << 3,
+
+	BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5,
+	BLOCK_SOLVER_SCENE_CHAIN = 1<< 6,
+	
+};
+
+class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //BLOCK_SOLVER_EXAMPLE_H
diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp
new file mode 100644
index 000000000..4ada11333
--- /dev/null
+++ b/examples/BlockSolver/btBlockSolver.cpp
@@ -0,0 +1,160 @@
+#include "btBlockSolver.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btQuickprof.h"
+
+struct btBlockSolverInternalData
+{
+	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
+	btConstraintArray m_tmpSolverContactConstraintPool;
+	btConstraintArray m_tmpSolverNonContactConstraintPool;
+	btConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int> m_orderTmpConstraintPool;
+	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+	
+	
+	unsigned long m_btSeed2;
+	int m_fixedBodyId;
+	int m_maxOverrideNumSolverIterations;
+	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
+
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
+
+	btBlockSolverInternalData()
+		:m_btSeed2(0),
+		m_fixedBodyId(-1),
+		m_maxOverrideNumSolverIterations(0),
+		m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()),
+		m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()),
+		m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric())
+	{
+	}
+};
+
+
+
+
+btBlockSolver::btBlockSolver()
+{
+	m_data = new btBlockSolverInternalData;
+}
+
+btBlockSolver::~btBlockSolver()
+{
+	delete m_data;
+}
+
+
+btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+
+	btSISolverSingleIterationData siData(m_data->m_tmpSolverBodyPool,
+		m_data->m_tmpSolverContactConstraintPool,
+		m_data->m_tmpSolverNonContactConstraintPool,
+		m_data->m_tmpSolverContactFrictionConstraintPool,
+		m_data->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data->m_orderTmpConstraintPool,
+		m_data->m_orderNonContactConstraintPool,
+		m_data->m_orderFrictionConstraintPool,
+		m_data->m_tmpConstraintSizesPool,
+		m_data->m_resolveSingleConstraintRowGeneric,
+		m_data->m_resolveSingleConstraintRowLowerLimit,
+		m_data->m_resolveSplitPenetrationImpulse,
+		m_data->m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_data->m_btSeed2,
+		m_data->m_fixedBodyId,
+		m_data->m_maxOverrideNumSolverIterations);
+	
+	m_data->m_fixedBodyId = -1;
+	//todo: setup sse2/4 constraint row methods
+
+	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info);
+	btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info);
+	
+	int i;
+	btPersistentManifold* manifold = 0;
+	//			btCollisionObject* colObj0=0,*colObj1=0;
+
+	for (i = 0; i < numManifolds; i++)
+	{
+		manifold = manifoldPtr[i];
+		btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info);
+	}
+
+
+
+	int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
+
+	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
+	if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+		siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
+	else
+		siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
+	siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
+	{
+		int i;
+		for (i = 0; i < numNonContactPool; i++)
+		{
+			siData.m_orderNonContactConstraintPool[i] = i;
+		}
+		for (i = 0; i < numConstraintPool; i++)
+		{
+			siData.m_orderTmpConstraintPool[i] = i;
+		}
+		for (i = 0; i < numFrictionPool; i++)
+		{
+			siData.m_orderFrictionConstraintPool[i] = i;
+		}
+	}
+
+	btScalar leastSquaresResidual = 0;
+
+	
+
+	{
+		BT_PROFILE("solveGroupCacheFriendlyIterations");
+		///this is a special step to resolve penetrations (just for contacts)
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+		int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations;
+
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		{
+			leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info);
+
+			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+				break;
+			}
+		}
+	}
+
+	btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
+	return res;
+}
+
+
+
+void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+	btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher);
+}
+
+void btBlockSolver::reset()
+{
+	//or just set m_data->m_btSeed2=0?
+	delete m_data;
+	m_data = new btBlockSolverInternalData;
+}
\ No newline at end of file
diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h
new file mode 100644
index 000000000..4d952db92
--- /dev/null
+++ b/examples/BlockSolver/btBlockSolver.h
@@ -0,0 +1,29 @@
+#ifndef BT_BLOCK_SOLVER_H
+#define BT_BLOCK_SOLVER_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btBlockSolver : public btMultiBodyConstraintSolver
+{
+	struct btBlockSolverInternalData* m_data;
+
+public:
+	btBlockSolver();
+	virtual ~btBlockSolver();
+
+	//btRigidBody
+	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+
+	//btMultibody
+	virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+
+	///clear internal cached data and reset random seed
+	virtual void reset();
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_BLOCK_SOLVER;
+	}
+};
+
+#endif //BT_BLOCK_SOLVER_H
\ No newline at end of file
diff --git a/examples/Evolution/NN3DWalkersTimeWarpBase.h b/examples/Evolution/NN3DWalkersTimeWarpBase.h
index 0a237ee98..deef11476 100644
--- a/examples/Evolution/NN3DWalkersTimeWarpBase.h
+++ b/examples/Evolution/NN3DWalkersTimeWarpBase.h
@@ -91,7 +91,7 @@ enum SolverEnumType
 	NNCGSOLVER = 2,
 	DANZIGSOLVER = 3,
 	LEMKESOLVER = 4,
-	FSSOLVER = 5,
+	
 	NUM_SOLVERS = 6
 };
 
@@ -103,7 +103,7 @@ static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver";
 static char NNCGSOLVER[] = "NNCG Solver";
 static char DANZIGSOLVER[] = "Danzig Solver";
 static char LEMKESOLVER[] = "Lemke Solver";
-static char FSSOLVER[] = "FeatherStone Solver";
+
 };  // namespace SolverType
 
 static const char* solverTypes[NUM_SOLVERS];
@@ -324,7 +324,7 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
 		solverTypes[2] = SolverType::NNCGSOLVER;
 		solverTypes[3] = SolverType::DANZIGSOLVER;
 		solverTypes[4] = SolverType::LEMKESOLVER;
-		solverTypes[5] = SolverType::FSSOLVER;
+		
 
 		{
 			ComboBoxParams comboParams;
@@ -499,19 +499,12 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
 				m_solver = new btMLCPSolver(mlcp);
 				break;
 			}
-			case FSSOLVER:
-			{
-				//			b3Printf("=%s=",SolverType::FSSOLVER);
-				//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
-				m_solver = new btMultiBodyConstraintSolver;
-
-				break;
-			}
+			
 			default:
 				break;
 		}
 
-		if (SOLVER_TYPE != FSSOLVER)
+		if (1)
 		{
 			//TODO: Set parameters for other solvers
 
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index ef0789e42..2678fa1b9 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreadedDemo/MultiThreadedDemo.h
 	../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
 	../MultiThreadedDemo/CommonRigidBodyMTBase.h
+	../BlockSolver/btBlockSolver.cpp
+	../BlockSolver/btBlockSolver.h
+	../BlockSolver/BlockSolverExample.cpp
+	../BlockSolver/BlockSolverExample.h
 	../Tutorial/Tutorial.cpp
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
@@ -333,7 +337,6 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
  ../MultiBody/MultiBodyConstraintFeedback.cpp
-  ../MultiBody/SerialChains.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 8f1f7d37a..02789d034 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -1,5 +1,6 @@
 #include "ExampleEntries.h"
 
+#include "../BlockSolver/BlockSolverExample.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
@@ -29,7 +30,7 @@
 #include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../MultiBody/InvertedPendulumPDControl.h"
-#include "../MultiBody/SerialChains.h"
+
 #include "../RigidBody/RigidBodySoftContact.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
@@ -136,8 +137,8 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 		ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 		ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0),
-		ExampleEntry(1, "Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc, 0),
-
+		
+	
 		ExampleEntry(0, "Physics Client-Server"),
 		ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",
 					 PhysicsServerCreateFuncBullet2),
@@ -150,6 +151,13 @@ static ExampleEntry gDefaultExamples[] =
 		//
 		//	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
 
+
+		ExampleEntry(0, "BlockSolver"),
+		ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK+ BLOCK_SOLVER_SI),
+		ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
+		ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
+		ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
+
 		ExampleEntry(0, "Inverse Dynamics"),
 		ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF),
 		ExampleEntry(1, "Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_PROGRAMMATICALLY),
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 94cd3f7ee..222ad911e 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -162,6 +162,7 @@ project "App_BulletExampleBrowser"
 		"../Evolution/NN3DWalkers.h",
 		"../Collision/*",
 		"../RoboticsLearning/*",
+		"../BlockSolver/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
 		"../MultiThreadedDemo/*",
diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 808433477..68a4a07a1 100644
--- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -35,6 +35,7 @@ enum btConstraintSolverType
 	BT_MLCP_SOLVER = 2,
 	BT_NNCG_SOLVER = 4,
 	BT_MULTIBODY_SOLVER = 8,
+	BT_BLOCK_SOLVER = 16,
 };
 
 class btConstraintSolver
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index def3227b4..9f76713db 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -394,6 +394,18 @@ btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstr
 	return gResolveSingleConstraintRowLowerLimit_scalar_reference;
 }
 
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric()
+{
+	return gResolveSplitPenetrationImpulse_scalar_reference;
+}
+
+btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2SplitPenetrationImpulseGeneric()
+{
+	return gResolveSplitPenetrationImpulse_sse2;
+}
+
+
+
 #ifdef USE_SIMD
 btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric()
 {
@@ -421,6 +433,11 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2()
 	return m_btSeed2;
 }
 
+unsigned long btSequentialImpulseConstraintSolver::btRand2a(unsigned long& seed)
+{
+	seed = (1664525L * seed + 1013904223L) & 0xffffffff;
+	return seed;
+}
 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
 int btSequentialImpulseConstraintSolver::btRandInt2(int n)
 {
@@ -454,42 +471,44 @@ int btSequentialImpulseConstraintSolver::btRandInt2(int n)
 	return (int)(r % un);
 }
 
-void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
+int btSequentialImpulseConstraintSolver::btRandInt2a(int n, unsigned long& seed)
 {
-	btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
-
-	solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
-	solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
-	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
-	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+	// seems good; xor-fold and modulus
+	const unsigned long un = static_cast<unsigned long>(n);
+	unsigned long r = btSequentialImpulseConstraintSolver::btRand2a(seed);
 
-	if (rb)
-	{
-		solverBody->m_worldTransform = rb->getWorldTransform();
-		solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
-		solverBody->m_originalBody = rb;
-		solverBody->m_angularFactor = rb->getAngularFactor();
-		solverBody->m_linearFactor = rb->getLinearFactor();
-		solverBody->m_linearVelocity = rb->getLinearVelocity();
-		solverBody->m_angularVelocity = rb->getAngularVelocity();
-		solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
-		solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
-	}
-	else
+	// note: probably more aggressive than it needs to be -- might be
+	//       able to get away without one or two of the innermost branches.
+	if (un <= 0x00010000UL)
 	{
-		solverBody->m_worldTransform.setIdentity();
-		solverBody->internalSetInvMass(btVector3(0, 0, 0));
-		solverBody->m_originalBody = 0;
-		solverBody->m_angularFactor.setValue(1, 1, 1);
-		solverBody->m_linearFactor.setValue(1, 1, 1);
-		solverBody->m_linearVelocity.setValue(0, 0, 0);
-		solverBody->m_angularVelocity.setValue(0, 0, 0);
-		solverBody->m_externalForceImpulse.setValue(0, 0, 0);
-		solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
+		r ^= (r >> 16);
+		if (un <= 0x00000100UL)
+		{
+			r ^= (r >> 8);
+			if (un <= 0x00000010UL)
+			{
+				r ^= (r >> 4);
+				if (un <= 0x00000004UL)
+				{
+					r ^= (r >> 2);
+					if (un <= 0x00000002UL)
+					{
+						r ^= (r >> 1);
+					}
+				}
+			}
+		}
 	}
+
+	return (int)(r % un);
 }
 
-btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
+void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep)
+{
+	btSISolverSingleIterationData::initSolverBody(solverBody, collisionObject, timeStep);
+}
+
+btScalar btSequentialImpulseConstraintSolver::restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
 {
 	//printf("rel_vel =%f\n", rel_vel);
 	if (btFabs(rel_vel) < velocityThreshold)
@@ -498,6 +517,10 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
 	btScalar rest = restitution * -rel_vel;
 	return rest;
 }
+btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
+{
+	return btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, restitution, velocityThreshold);
+}
 
 void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode)
 {
@@ -513,13 +536,13 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
 	}
 }
 
-void btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+void btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
-	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+	btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB];
 
-	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody;
+	btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -605,30 +628,47 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 	}
 }
 
+void btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	btSequentialImpulseConstraintSolver::setupFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+}
+
+btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	btSolverConstraint& solverConstraint = tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+		colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+
+
 btSolverConstraint& btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
 	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
-							colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
+		colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
-void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
-																		   btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
-																		   btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
-																		   btScalar desiredVelocity, btScalar cfmSlip)
+
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+	btScalar desiredVelocity, btScalar cfmSlip)
 
 {
 	btVector3 normalAxis(0, 0, 0);
 
 	solverConstraint.m_contactNormal1 = normalAxis;
 	solverConstraint.m_contactNormal2 = -normalAxis;
-	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+	btSolverBody& solverBodyA = tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody& solverBodyB = tmpSolverBodyPool[solverBodyIdB];
 
-	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
-	btRigidBody* bodyA = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+	btRigidBody* body0 = tmpSolverBodyPool[solverBodyIdA].m_originalBody;
+	btRigidBody* bodyA = tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -677,15 +717,250 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSol
 	}
 }
 
+
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+	btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+	btScalar desiredVelocity, btScalar cfmSlip)
+
+{
+	setupTorsionalFrictionConstraintInternal(m_tmpSolverBodyPool, solverConstraint, normalAxis1, solverBodyIdA, solverBodyIdB,
+		cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
+		colObj0, colObj1, relaxation,
+		desiredVelocity, cfmSlip);
+
+}
+
+btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	btSolverConstraint& solverConstraint = tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupTorsionalFrictionConstraintInternal(tmpSolverBodyPool, solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
+		colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+
 btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
 	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2,
-									 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+		colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
+int btSISolverSingleIterationData::getOrInitSolverBody(btCollisionObject & body, btScalar timeStep)
+{
+#if BT_THREADSAFE
+	int solverBodyId = -1;
+	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		if (solverBodyId < 0)
+		{
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			body.setCompanionId(solverBodyId);
+		}
+	}
+	else if (isRigidBodyType && body.isKinematicObject())
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		// if no table entry yet,
+		if (solverBodyId == INVALID_SOLVER_BODY_ID)
+		{
+			// create a table entry for this body
+			solverBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
+		}
+	}
+	else
+	{
+		bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		if (!isMultiBodyType)
+		{
+			btAssert(body.isStaticOrKinematicObject());
+		}
+		//it could be a multibody link collider
+		// all fixed bodies (inf mass) get mapped to a single solver id
+		if (m_fixedBodyId < 0)
+		{
+			m_fixedBodyId = m_tmpSolverBodyPool.size();
+			btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&fixedBody, 0, timeStep);
+		}
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+#else   // BT_THREADSAFE
+
+	int solverBodyIdA = -1;
+
+	if (body.getCompanionId() >= 0)
+	{
+		//body has already been converted
+		solverBodyIdA = body.getCompanionId();
+		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	}
+	else
+	{
+		btRigidBody* rb = btRigidBody::upcast(&body);
+		//convert both active and kinematic objects (for their velocity)
+		if (rb && (rb->getInvMass() || rb->isKinematicObject()))
+		{
+			solverBodyIdA = m_tmpSolverBodyPool.size();
+			btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+			initSolverBody(&solverBody, &body, timeStep);
+			body.setCompanionId(solverBodyIdA);
+		}
+		else
+		{
+			if (m_fixedBodyId < 0)
+			{
+				m_fixedBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&fixedBody, 0, timeStep);
+			}
+			return m_fixedBodyId;
+			//			return 0;//assume first one is a fixed solver body
+		}
+	}
+
+	return solverBodyIdA;
+#endif  // BT_THREADSAFE
+}
+void btSISolverSingleIterationData::initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep)
+{
+	btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0;
+
+	solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
+	solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
+
+	if (rb)
+	{
+		solverBody->m_worldTransform = rb->getWorldTransform();
+		solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor());
+		solverBody->m_originalBody = rb;
+		solverBody->m_angularFactor = rb->getAngularFactor();
+		solverBody->m_linearFactor = rb->getLinearFactor();
+		solverBody->m_linearVelocity = rb->getLinearVelocity();
+		solverBody->m_angularVelocity = rb->getAngularVelocity();
+		solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep;
+		solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep;
+	}
+	else
+	{
+		solverBody->m_worldTransform.setIdentity();
+		solverBody->internalSetInvMass(btVector3(0, 0, 0));
+		solverBody->m_originalBody = 0;
+		solverBody->m_angularFactor.setValue(1, 1, 1);
+		solverBody->m_linearFactor.setValue(1, 1, 1);
+		solverBody->m_linearVelocity.setValue(0, 0, 0);
+		solverBody->m_angularVelocity.setValue(0, 0, 0);
+		solverBody->m_externalForceImpulse.setValue(0, 0, 0);
+		solverBody->m_externalTorqueImpulse.setValue(0, 0, 0);
+	}
+}
+
+int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const
+{
+#if BT_THREADSAFE
+	int solverBodyId = -1;
+	bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
+	if (isRigidBodyType && !body.isStaticOrKinematicObject())
+	{
+		// dynamic body
+		// Dynamic bodies can only be in one island, so it's safe to write to the companionId
+		solverBodyId = body.getCompanionId();
+		btAssert(solverBodyId >= 0);
+	}
+	else if (isRigidBodyType && body.isKinematicObject())
+	{
+		//
+		// NOTE: must test for kinematic before static because some kinematic objects also
+		//   identify as "static"
+		//
+		// Kinematic bodies can be in multiple islands at once, so it is a
+		// race condition to write to them, so we use an alternate method
+		// to record the solverBodyId
+		int uniqueId = body.getWorldArrayIndex();
+		const int INVALID_SOLVER_BODY_ID = -1;
+		if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size())
+		{
+			m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
+		}
+		solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
+		btAssert(solverBodyId != INVALID_SOLVER_BODY_ID);
+	}
+	else
+	{
+		bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK);
+		// Incorrectly set collision object flags can degrade performance in various ways.
+		if (!isMultiBodyType)
+		{
+			btAssert(body.isStaticOrKinematicObject());
+		}
+		btAssert(m_fixedBodyId >= 0);
+		solverBodyId = m_fixedBodyId;
+	}
+	btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
+	return solverBodyId;
+#else   // BT_THREADSAFE
+	int solverBodyIdA = -1;
+
+	if (body.getCompanionId() >= 0)
+	{
+		//body has already been converted
+		solverBodyIdA = body.getCompanionId();
+		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	}
+	else
+	{
+		btRigidBody* rb = btRigidBody::upcast(&body);
+		//convert both active and kinematic objects (for their velocity)
+		if (rb && (rb->getInvMass() || rb->isKinematicObject()))
+		{
+			btAssert(0);
+		}
+		else
+		{
+			if (m_fixedBodyId < 0)
+			{
+				btAssert(0);
+			}
+			return m_fixedBodyId;
+			//			return 0;//assume first one is a fixed solver body
+		}
+	}
+
+	return solverBodyIdA;
+#endif  // BT_THREADSAFE
+}
+
 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep)
 {
 #if BT_THREADSAFE
@@ -789,17 +1064,20 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 }
 #include <stdio.h>
 
-void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
-																 int solverBodyIdA, int solverBodyIdB,
-																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																 btScalar& relaxation,
-																 const btVector3& rel_pos1, const btVector3& rel_pos2)
+
+
+void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData,
+	btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+	btScalar& relaxation,
+	const btVector3& rel_pos1, const btVector3& rel_pos2)
 {
 	//	const btVector3& pos1 = cp.getPositionWorldOnA();
 	//	const btVector3& pos2 = cp.getPositionWorldOnB();
 
-	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+	btSolverBody* bodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* bodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* rb0 = bodyA->m_originalBody;
 	btRigidBody* rb1 = bodyB->m_originalBody;
@@ -906,7 +1184,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 		solverConstraint.m_friction = cp.m_combinedFriction;
 
-		restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
+		restitution = btSequentialImpulseConstraintSolver::restitutionCurveInternal(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
 		if (restitution <= btScalar(0.))
 		{
 			restitution = 0.f;
@@ -920,7 +1198,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 		if (rb0)
 			bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
 		if (rb1)
-			bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() , -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
+			bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
 	}
 	else
 	{
@@ -974,25 +1252,59 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 	}
 }
 
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
-																	   int solverBodyIdA, int solverBodyIdB,
-																	   btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+	btScalar& relaxation,
+	const btVector3& rel_pos1, const btVector3& rel_pos2)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations
+	);
+
+
+	setupContactConstraintInternal(siData, solverConstraint,
+		solverBodyIdA, solverBodyIdB,
+		cp, infoGlobal,
+		relaxation,
+		rel_pos1, rel_pos2);
+}
+
+
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
+	btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 {
-	btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+	btSolverBody* bodyA = &tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* bodyB = &tmpSolverBodyPool[solverBodyIdB];
 
 	btRigidBody* rb0 = bodyA->m_originalBody;
 	btRigidBody* rb1 = bodyB->m_originalBody;
 
 	{
-		btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
+		btSolverConstraint& frictionConstraint1 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 		{
 			frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 			if (rb0)
-				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass() , frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
+				bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass(), frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
 			if (rb1)
-				bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass() , -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse);
+				bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse);
 		}
 		else
 		{
@@ -1002,7 +1314,7 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverC
 
 	if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 	{
-		btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
+		btSolverConstraint& frictionConstraint2 = tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1];
 		if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 		{
 			frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
@@ -1018,21 +1330,31 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverC
 	}
 }
 
-void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
+	int solverBodyIdA, int solverBodyIdB,
+	btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
+{
+	setFrictionConstraintImpulseInternal(m_tmpSolverBodyPool, m_tmpSolverContactFrictionConstraintPool,
+		solverConstraint,
+		solverBodyIdA, solverBodyIdB,
+		cp, infoGlobal);
+
+}
+void btSequentialImpulseConstraintSolver::convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
 {
 	btCollisionObject *colObj0 = 0, *colObj1 = 0;
 
 	colObj0 = (btCollisionObject*)manifold->getBody0();
 	colObj1 = (btCollisionObject*)manifold->getBody1();
 
-	int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
-	int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
+	int solverBodyIdA = siData.getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
+	int solverBodyIdB = siData.getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
 
 	//	btRigidBody* bodyA = btRigidBody::upcast(colObj0);
 	//	btRigidBody* bodyB = btRigidBody::upcast(colObj1);
 
-	btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
-	btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
+	btSolverBody* solverBodyA = &siData.m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody* solverBodyB = &siData.m_tmpSolverBodyPool[solverBodyIdB];
 
 	///avoid collision response between two static objects
 	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
@@ -1049,8 +1371,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 rel_pos2;
 			btScalar relaxation;
 
-			int frictionIndex = m_tmpSolverContactConstraintPool.size();
-			btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
+			int frictionIndex = siData.m_tmpSolverContactConstraintPool.size();
+			btSolverConstraint& solverConstraint = siData.m_tmpSolverContactConstraintPool.expandNonInitializing();
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
@@ -1071,16 +1393,20 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			btVector3 vel = vel1 - vel2;
 			btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
 
-			setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
+			setupContactConstraintInternal(siData, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
 
 			/////setup the friction constraints
 
-			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
+			solverConstraint.m_frictionIndex = siData.m_tmpSolverContactFrictionConstraintPool.size();
 
 			if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0))
 			{
 				{
-					addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+
+					btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool,
+						siData.m_tmpSolverContactRollingFrictionConstraintPool,
+						cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+
 					btVector3 axis0, axis1;
 					btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1);
 					axis0.normalize();
@@ -1091,11 +1417,17 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					if (axis0.length() > 0.001)
-						addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
-													   cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					{
+						btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool,
+							siData.m_tmpSolverContactRollingFrictionConstraintPool, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					}
 					if (axis1.length() > 0.001)
-						addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
-													   cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					{
+						btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraintInternal(siData.m_tmpSolverBodyPool,
+							siData.m_tmpSolverContactRollingFrictionConstraintPool, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp,
+							cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+					}
 				}
 			}
 
@@ -1124,7 +1456,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
 					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+					btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+						cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
@@ -1132,7 +1465,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						cp.m_lateralFrictionDir2.normalize();  //??
 						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+						btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+							cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
 				}
 				else
@@ -1141,13 +1475,15 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+					btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+						cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
+						btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+							cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
@@ -1158,16 +1494,44 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			}
 			else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+				btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+					cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+				{
+					btSequentialImpulseConstraintSolver::addFrictionConstraintInternal(siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+						cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+				}
 			}
-			setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
+			btSequentialImpulseConstraintSolver::setFrictionConstraintImpulseInternal(
+				siData.m_tmpSolverBodyPool, siData.m_tmpSolverContactFrictionConstraintPool,
+				solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 		}
 	}
 }
 
+void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, infoGlobal);
+}
+
 void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
 	int i;
@@ -1181,22 +1545,24 @@ void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold**
 	}
 }
 
-void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
-													   btTypedConstraint* constraint,
-													   const btTypedConstraint::btConstraintInfo1& info1,
-													   int solverBodyIdA,
-													   int solverBodyIdB,
-													   const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
+	int& maxOverrideNumSolverIterations,
+	btSolverConstraint* currentConstraintRow,
+	btTypedConstraint* constraint,
+	const btTypedConstraint::btConstraintInfo1& info1,
+	int solverBodyIdA,
+	int solverBodyIdB,
+	const btContactSolverInfo& infoGlobal)
 {
 	const btRigidBody& rbA = constraint->getRigidBodyA();
 	const btRigidBody& rbB = constraint->getRigidBodyB();
 
-	const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
-	const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
+	const btSolverBody* bodyAPtr = &tmpSolverBodyPool[solverBodyIdA];
+	const btSolverBody* bodyBPtr = &tmpSolverBodyPool[solverBodyIdB];
 
 	int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
-	if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
-		m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
+	if (overrideNumSolverIterations > maxOverrideNumSolverIterations)
+		maxOverrideNumSolverIterations = overrideNumSolverIterations;
 
 	for (int j = 0; j < info1.m_numConstraintRows; j++)
 	{
@@ -1236,7 +1602,7 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre
 	info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2;
 	info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
 	info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar);  //check this
-	///the size of btSolverConstraint needs be a multiple of btScalar
+																	///the size of btSolverConstraint needs be a multiple of btScalar
 	btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint));
 	info2.m_constraintError = &currentConstraintRow->m_rhs;
 	currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
@@ -1313,7 +1679,16 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre
 	}
 }
 
-void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow,
+	btTypedConstraint* constraint,
+	const btTypedConstraint::btConstraintInfo1& info1,
+	int solverBodyIdA,
+	int solverBodyIdB,
+	const btContactSolverInfo& infoGlobal)
+{
+}
+
+void btSequentialImpulseConstraintSolver::convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("convertJoints");
 	for (int j = 0; j < numConstraints; j++)
@@ -1325,11 +1700,11 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons
 
 	int totalNumRows = 0;
 
-	m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
+	siData.m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
 	//calculate the total number of contraint rows
 	for (int i = 0; i < numConstraints; i++)
 	{
-		btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i];
 		btJointFeedback* fb = constraints[i]->getJointFeedback();
 		if (fb)
 		{
@@ -1350,34 +1725,58 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons
 		}
 		totalNumRows += info1.m_numConstraintRows;
 	}
-	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
+	siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 
 	///setup the btSolverConstraints
 	int currentRow = 0;
 
 	for (int i = 0; i < numConstraints; i++)
 	{
-		const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+		const btTypedConstraint::btConstraintInfo1& info1 = siData.m_tmpConstraintSizesPool[i];
 
 		if (info1.m_numConstraintRows)
 		{
 			btAssert(currentRow < totalNumRows);
 
-			btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
+			btSolverConstraint* currentConstraintRow = &siData.m_tmpSolverNonContactConstraintPool[currentRow];
 			btTypedConstraint* constraint = constraints[i];
 			btRigidBody& rbA = constraint->getRigidBodyA();
 			btRigidBody& rbB = constraint->getRigidBodyB();
 
-			int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
-			int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
+			int solverBodyIdA = siData.getOrInitSolverBody(rbA, infoGlobal.m_timeStep);
+			int solverBodyIdB = siData.getOrInitSolverBody(rbB, infoGlobal.m_timeStep);
 
-			convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
+			convertJointInternal(siData.m_tmpSolverBodyPool, siData.m_maxOverrideNumSolverIterations,
+				currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal);
 		}
 		currentRow += info1.m_numConstraintRows;
 	}
 }
 
-void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	convertJointsInternal(siData, constraints, numConstraints, infoGlobal);
+}
+
+
+void btSequentialImpulseConstraintSolver::convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("convertBodies");
 	for (int i = 0; i < numBodies; i++)
@@ -1385,23 +1784,23 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi
 		bodies[i]->setCompanionId(-1);
 	}
 #if BT_THREADSAFE
-	m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
+	siData.m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
 #endif  // BT_THREADSAFE
 
-	m_tmpSolverBodyPool.reserve(numBodies + 1);
-	m_tmpSolverBodyPool.resize(0);
+	siData.m_tmpSolverBodyPool.reserve(numBodies + 1);
+	siData.m_tmpSolverBodyPool.resize(0);
 
 	//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
 	//initSolverBody(&fixedBody,0);
 
 	for (int i = 0; i < numBodies; i++)
 	{
-		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+		int bodyId = siData.getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
 
 		btRigidBody* body = btRigidBody::upcast(bodies[i]);
 		if (body && body->getInvMass())
 		{
-			btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+			btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId];
 			btVector3 gyroForce(0, 0, 0);
 			if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 			{
@@ -1422,6 +1821,29 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi
 	}
 }
 
+
+void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	convertBodiesInternal(siData, bodies, numBodies, infoGlobal);
+}
+
 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	m_fixedBodyId = -1;
@@ -1545,14 +1967,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	return 0.f;
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
+btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("solveSingleIteration");
 	btScalar leastSquaresResidual = 0.f;
 
-	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
-	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
-	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
+	int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
 
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
@@ -1560,10 +1982,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			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;
+				int tmp = siData.m_orderNonContactConstraintPool[j];
+				int swapi = btRandInt2a(j + 1, siData.m_seed);
+				siData.m_orderNonContactConstraintPool[j] = siData.m_orderNonContactConstraintPool[swapi];
+				siData.m_orderNonContactConstraintPool[swapi] = tmp;
 			}
 
 			//contact/friction constraints are not solved more than
@@ -1571,30 +1993,30 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 			{
 				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;
+					int tmp = siData.m_orderTmpConstraintPool[j];
+					int swapi = btRandInt2a(j + 1, siData.m_seed);
+					siData.m_orderTmpConstraintPool[j] = siData.m_orderTmpConstraintPool[swapi];
+					siData.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;
+					int tmp = siData.m_orderFrictionConstraintPool[j];
+					int swapi = btRandInt2a(j + 1, siData.m_seed);
+					siData.m_orderFrictionConstraintPool[j] = siData.m_orderFrictionConstraintPool[swapi];
+					siData.m_orderFrictionConstraintPool[swapi] = tmp;
 				}
 			}
 		}
 	}
 
 	///solve all joint constraints
-	for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++)
+	for (int j = 0; j < siData.m_tmpSolverNonContactConstraintPool.size(); j++)
 	{
-		btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
+		btSolverConstraint& constraint = siData.m_tmpSolverNonContactConstraintPool[siData.m_orderNonContactConstraintPool[j]];
 		if (iteration < constraint.m_overrideNumSolverIterations)
 		{
-			btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
+			btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint);
 			leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 		}
 	}
@@ -1605,10 +2027,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		{
 			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];
+				int bodyAid = siData.getSolverBody(constraints[j]->getRigidBodyA());
+				int bodyBid = siData.getSolverBody(constraints[j]->getRigidBodyB());
+				btSolverBody& bodyA = siData.m_tmpSolverBodyPool[bodyAid];
+				btSolverBody& bodyB = siData.m_tmpSolverBodyPool[bodyBid];
 				constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 			}
 		}
@@ -1616,7 +2038,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		///solve all contact constraints
 		if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 		{
-			int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+			int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size();
 			int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 
 			for (int c = 0; c < numPoolConstraints; c++)
@@ -1624,8 +2046,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				btScalar totalImpulse = 0;
 
 				{
-					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
-					btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[c]];
+					btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 
 					totalImpulse = solveManifold.m_appliedImpulse;
@@ -1634,28 +2056,28 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				if (applyFriction)
 				{
 					{
-						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
+						btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier]];
 
 						if (totalImpulse > btScalar(0))
 						{
 							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
 							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 						}
 					}
 
 					if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 					{
-						btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
+						btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[c * multiplier + 1]];
 
 						if (totalImpulse > btScalar(0))
 						{
 							solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
 							solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
 
-							btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+							btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 							leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 						}
 					}
@@ -1665,40 +2087,40 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 		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 numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size();
 			int j;
 
 			for (j = 0; j < numPoolConstraints; j++)
 			{
-				const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-				btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+				const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]];
+				btScalar residual = siData.m_resolveSingleConstraintRowLowerLimit(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
 
 			///solve all friction constraints
 
-			int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
+			int numFrictionPoolConstraints = siData.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;
+				btSolverConstraint& solveManifold = siData.m_tmpSolverContactFrictionConstraintPool[siData.m_orderFrictionConstraintPool[j]];
+				btScalar totalImpulse = siData.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 residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+					btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 					leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 				}
 			}
 		}
 
-		int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+		int numRollingFrictionPoolConstraints = siData.m_tmpSolverContactRollingFrictionConstraintPool.size();
 		for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
 		{
-			btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
-			btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+			btSolverConstraint& rollingFrictionConstraint = siData.m_tmpSolverContactRollingFrictionConstraintPool[j];
+			btScalar totalImpulse = siData.m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
 			if (totalImpulse > btScalar(0))
 			{
 				btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
@@ -1708,7 +2130,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
 				rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
 
-				btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
+				btScalar residual = siData.m_resolveSingleConstraintRowGeneric(siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], siData.m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
 				leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 			}
 		}
@@ -1716,8 +2138,56 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 	return leastSquaresResidual;
 }
 
+
+btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	btScalar leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData,
+		iteration, constraints, numConstraints, infoGlobal);
+	return leastSquaresResidual;
+}
+
 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	solveGroupCacheFriendlySplitImpulseIterationsInternal(siData,
+		bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+
+}
+void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 	int iteration;
 	if (infoGlobal.m_splitImpulse)
@@ -1727,13 +2197,13 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
 			{
 				btScalar leastSquaresResidual = 0.f;
 				{
-					int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
+					int numPoolConstraints = siData.m_tmpSolverContactConstraintPool.size();
 					int j;
 					for (j = 0; j < numPoolConstraints; j++)
 					{
-						const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
+						const btSolverConstraint& solveManifold = siData.m_tmpSolverContactConstraintPool[siData.m_orderTmpConstraintPool[j]];
 
-						btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
+						btScalar residual = siData.m_resolveSplitPenetrationImpulse(siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], siData.m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
 						leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
 					}
 				}
@@ -1760,7 +2230,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 		int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
 
 		for (int iteration = 0; iteration < maxIterations; iteration++)
-		//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
+			//for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
 		{
 			m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
@@ -1776,31 +2246,42 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 	return 0.f;
 }
 
-void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+void btSequentialImpulseConstraintSolver::writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
 	for (int j = iBegin; j < iEnd; j++)
 	{
-		const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
+		const btSolverConstraint& solveManifold = tmpSolverContactConstraintPool[j];
 		btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
 		btAssert(pt);
 		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
 		//	float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 		//	printf("pt->m_appliedImpulseLateral1 = %f\n", f);
-		pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+		pt->m_appliedImpulseLateral1 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
 		//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
 		if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 		{
-			pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
+			pt->m_appliedImpulseLateral2 = tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse;
 		}
 		//do a callback here?
 	}
 }
 
+void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
+	writeBackContactsInternal(m_tmpSolverContactConstraintPool, m_tmpSolverContactFrictionConstraintPool, iBegin, iEnd, infoGlobal);
+
+}
+
 void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
+	writeBackJointsInternal(m_tmpSolverNonContactConstraintPool, iBegin, iEnd, infoGlobal);
+}
+
+void btSequentialImpulseConstraintSolver::writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
 	for (int j = iBegin; j < iEnd; j++)
 	{
-		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
+		const btSolverConstraint& solverConstr = tmpSolverNonContactConstraintPool[j];
 		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
 		btJointFeedback* fb = constr->getJointFeedback();
 		if (fb)
@@ -1821,53 +2302,79 @@ void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd,
 
 void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 {
+	writeBackBodiesInternal(m_tmpSolverBodyPool, iBegin, iEnd, infoGlobal);
+}
+void btSequentialImpulseConstraintSolver::writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
+{
 	for (int i = iBegin; i < iEnd; i++)
 	{
-		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+		btRigidBody* body = tmpSolverBodyPool[i].m_originalBody;
 		if (body)
 		{
 			if (infoGlobal.m_splitImpulse)
-				m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
+				tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
 			else
-				m_tmpSolverBodyPool[i].writebackVelocity();
+				tmpSolverBodyPool[i].writebackVelocity();
 
-			m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
-				m_tmpSolverBodyPool[i].m_linearVelocity +
-				m_tmpSolverBodyPool[i].m_externalForceImpulse);
+			tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(
+				tmpSolverBodyPool[i].m_linearVelocity +
+				tmpSolverBodyPool[i].m_externalForceImpulse);
 
-			m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
-				m_tmpSolverBodyPool[i].m_angularVelocity +
-				m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
+			tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
+				tmpSolverBodyPool[i].m_angularVelocity +
+				tmpSolverBodyPool[i].m_externalTorqueImpulse);
 
 			if (infoGlobal.m_splitImpulse)
-				m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
+				tmpSolverBodyPool[i].m_originalBody->setWorldTransform(tmpSolverBodyPool[i].m_worldTransform);
 
-			m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
+			tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
 		}
 	}
 }
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("solveGroupCacheFriendlyFinish");
 
 	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 	{
-		writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
+		writeBackContactsInternal(siData.m_tmpSolverContactConstraintPool, siData.m_tmpSolverContactFrictionConstraintPool, 0, siData.m_tmpSolverContactConstraintPool.size(), infoGlobal);
 	}
 
-	writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
-	writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
+	writeBackJointsInternal(siData.m_tmpSolverNonContactConstraintPool, 0, siData.m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
+	writeBackBodiesInternal(siData.m_tmpSolverBodyPool, 0, siData.m_tmpSolverBodyPool.size(), infoGlobal);
 
-	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
-	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
-	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
-	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+	siData.m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	siData.m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	siData.m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	siData.m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
 
-	m_tmpSolverBodyPool.resizeNoInitialize(0);
+	siData.m_tmpSolverBodyPool.resizeNoInitialize(0);
 	return 0.f;
 }
 
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	btSISolverSingleIterationData siData(m_tmpSolverBodyPool,
+		m_tmpSolverContactConstraintPool,
+		m_tmpSolverNonContactConstraintPool,
+		m_tmpSolverContactFrictionConstraintPool,
+		m_tmpSolverContactRollingFrictionConstraintPool,
+		m_orderTmpConstraintPool,
+		m_orderNonContactConstraintPool,
+		m_orderFrictionConstraintPool,
+		m_tmpConstraintSizesPool,
+		m_resolveSingleConstraintRowGeneric,
+		m_resolveSingleConstraintRowLowerLimit,
+		m_resolveSplitPenetrationImpulse,
+		m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_btSeed2,
+		m_fixedBodyId,
+		m_maxOverrideNumSolverIterations);
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, infoGlobal);
+}
+
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/)
 {
@@ -1886,4 +2393,4 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 void btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
-}
+}
\ No newline at end of file
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 70db83b06..aca1d0988 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -29,6 +29,68 @@ class btCollisionObject;
 
 typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
 
+struct btSISolverSingleIterationData
+{
+	btAlignedObjectArray<btSolverBody>& m_tmpSolverBodyPool;
+	btConstraintArray& m_tmpSolverContactConstraintPool;
+	btConstraintArray& m_tmpSolverNonContactConstraintPool;
+	btConstraintArray& m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray& m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int>& m_orderTmpConstraintPool;
+	btAlignedObjectArray<int>& m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int>& m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& m_tmpConstraintSizesPool;
+	unsigned long& m_seed;
+
+	btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse;
+	btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable;
+	int& m_fixedBodyId;
+	int& m_maxOverrideNumSolverIterations;
+	int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
+	static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
+	int getSolverBody(btCollisionObject& body) const;
+
+
+	btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
+		btConstraintArray& tmpSolverContactConstraintPool,
+		btConstraintArray& tmpSolverNonContactConstraintPool,
+		btConstraintArray& tmpSolverContactFrictionConstraintPool,
+		btConstraintArray& tmpSolverContactRollingFrictionConstraintPool,
+		btAlignedObjectArray<int>& orderTmpConstraintPool,
+		btAlignedObjectArray<int>& orderNonContactConstraintPool,
+		btAlignedObjectArray<int>& orderFrictionConstraintPool,
+		btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool,
+		btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric,
+		btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit,
+		btSingleConstraintRowSolver& resolveSplitPenetrationImpulse,
+		btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable,
+		unsigned long& seed,
+		int& fixedBodyId,
+		int& maxOverrideNumSolverIterations
+	)
+		:m_tmpSolverBodyPool(tmpSolverBodyPool),
+		m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool),
+		m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool),
+		m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool),
+		m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool),
+		m_orderTmpConstraintPool(orderTmpConstraintPool),
+		m_orderNonContactConstraintPool(orderNonContactConstraintPool),
+		m_orderFrictionConstraintPool(orderFrictionConstraintPool),
+		m_tmpConstraintSizesPool(tmpConstraintSizesPool),
+		m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
+		m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
+		m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
+		m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
+		m_seed(seed),
+		m_fixedBodyId(fixedBodyId),
+		m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
+	{
+	}
+};
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class)
 btSequentialImpulseConstraintSolver : public btConstraintSolver
@@ -64,26 +126,26 @@ protected:
 	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,
-								 const btContactSolverInfo& infoGlobal,
-								 btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+		btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		const btContactSolverInfo& infoGlobal,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
 	void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
-										  btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
-										  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
-										  btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+		btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 
 	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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
 	btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
 
 	void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
-								const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
+		const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
 
 	static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
 
 	void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
-									  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
 
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long m_btSeed2;
@@ -97,6 +159,7 @@ protected:
 	virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
 	void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
 
+
 	virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
 	btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
@@ -122,7 +185,8 @@ protected:
 		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
 	}
 
-protected:
+public:
+
 	void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -130,6 +194,7 @@ 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);
 	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
@@ -141,13 +206,52 @@ public:
 
 	virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
 
+	static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
+	static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
+	static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation,
+		const btVector3& rel_pos1, const btVector3& rel_pos2);
+	static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
+	static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.);
+	static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
+		btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
+		btScalar desiredVelocity, btScalar cfmSlip);
+	static void setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip);
+	static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
+	static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
+
+		btSolverConstraint& solverConstraint,
+		int solverBodyIdA, int solverBodyIdB,
+		btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
+	static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
+		int& maxOverrideNumSolverIterations,
+		btSolverConstraint* currentConstraintRow,
+		btTypedConstraint* constraint,
+		const btTypedConstraint::btConstraintInfo1& info1,
+		int solverBodyIdA,
+		int solverBodyIdB,
+		const btContactSolverInfo& infoGlobal);
+
+	static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+
+	static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+
+	static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
+	static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+
 	///clear internal cached data and reset random seed
 	virtual void reset();
 
 	unsigned long btRand2();
-
 	int btRandInt2(int n);
 
+	static unsigned long btRand2a(unsigned long& seed);
+	static int btRandInt2a(int n, unsigned long& seed);
+
 	void setRandSeed(unsigned long seed)
 	{
 		m_btSeed2 = seed;
@@ -180,14 +284,18 @@ public:
 	}
 
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
-	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
+	static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
 
 	///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
-	btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
-	btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
+	static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
+
+	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
+	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
+
 };
 
-#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
+#endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
\ No newline at end of file
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
index 6be36ba14..77fdb86bb 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
@@ -156,7 +156,7 @@ protected:
 		btTypedConstraint** constraints,
 		int numConstraints,
 		const btContactSolverInfo& infoGlobal,
-		btIDebugDraw* debugDrawer) BT_OVERRIDE;
+		btIDebugDraw* debugDrawer) ;
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR()