preparation for block solver btRigidBody.

13 files changed, 1376 insertions, 187 deletions

diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp
new file mode 100644
index 000000000..e0bff80bb
--- /dev/null
+++ b/examples/BlockSolver/BlockSolverExample.cpp
@@ -0,0 +1,380 @@
+#include "BlockSolverExample.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "btBulletDynamicsCommon.h"
+
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
+#include "btBlockSolver.h"
+
+#include "../OpenGLWindow/GLInstancingRenderer.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.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 = 1;
+		float pitch = -35;
+		float yaw = 50;
+		float targetPos[3] = {-3, 2.8, -2.5};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+	btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false);
+	void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55));
+	void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+};
+
+static bool g_fixedBase = true;
+static bool g_firstInit = true;
+static float scaling = 0.4f;
+static float friction = 1.;
+
+
+BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
+	: CommonMultiBodyBase(helper),
+	m_option(option)
+{
+	m_guiHelper->setUpAxis(1);
+}
+
+BlockSolverExample::~BlockSolverExample()
+{
+	// Do nothing
+}
+
+void BlockSolverExample::stepSimulation(float deltaTime)
+{
+	//use a smaller internal timestep, there are stability issues
+	float internalTimeStep = 1. / 240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+}
+
+void BlockSolverExample::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	if (g_firstInit)
+	{
+		m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
+		m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
+		g_firstInit = false;
+	}
+	///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, -10, 0));
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4);  //todo: what value is good?
+
+	
+
+	/////////////////////////////////////////////////////////////////
+	/////////////////////////////////////////////////////////////////
+
+	bool damping = true;
+	bool gyro = true;
+	int numLinks = 5;
+	bool spherical = true;      //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+	bool multibodyOnly = true;  //false
+	bool canSleep = true;
+	bool selfCollide = true;
+	btVector3 linkHalfExtents(0.05, 0.37, 0.1);
+	btVector3 baseHalfExtents(0.05, 0.37, 0.1);
+
+	btMultiBody* mbC1 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
+	btMultiBody* mbC2 = createFeatherstoneMultiBody(world, numLinks, btVector3(-0.4f, 3.0f, 0.5f), linkHalfExtents, baseHalfExtents, spherical, g_fixedBase);
+
+	mbC1->setCanSleep(canSleep);
+	mbC1->setHasSelfCollision(selfCollide);
+	mbC1->setUseGyroTerm(gyro);
+
+	if (!damping)
+	{
+		mbC1->setLinearDamping(0.f);
+		mbC1->setAngularDamping(0.f);
+	}
+	else
+	{
+		mbC1->setLinearDamping(0.1f);
+		mbC1->setAngularDamping(0.9f);
+	}
+	//
+	m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
+	//////////////////////////////////////////////
+	if (numLinks > 0)
+	{
+		btScalar q0 = 45.f * SIMD_PI / 180.f;
+		if (!spherical)
+		{
+			mbC1->setJointPosMultiDof(0, &q0);
+		}
+		else
+		{
+			btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+			quat0.normalize();
+			mbC1->setJointPosMultiDof(0, quat0);
+		}
+	}
+	///
+	addColliders(mbC1, world, baseHalfExtents, linkHalfExtents);
+
+	mbC2->setCanSleep(canSleep);
+	mbC2->setHasSelfCollision(selfCollide);
+	mbC2->setUseGyroTerm(gyro);
+	//
+	if (!damping)
+	{
+		mbC2->setLinearDamping(0.f);
+		mbC2->setAngularDamping(0.f);
+	}
+	else
+	{
+		mbC2->setLinearDamping(0.1f);
+		mbC2->setAngularDamping(0.9f);
+	}
+	//
+	m_dynamicsWorld->setGravity(btVector3(0, -9.81, 0));
+	//////////////////////////////////////////////
+	if (numLinks > 0)
+	{
+		btScalar q0 = -45.f * SIMD_PI / 180.f;
+		if (!spherical)
+		{
+			mbC2->setJointPosMultiDof(0, &q0);
+		}
+		else
+		{
+			btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+			quat0.normalize();
+			mbC2->setJointPosMultiDof(0, quat0);
+		}
+	}
+	///
+	addColliders(mbC2, world, baseHalfExtents, linkHalfExtents);
+
+	/////////////////////////////////////////////////////////////////
+	btScalar groundHeight = -51.55;
+	btScalar mass(0.);
+
+	//rigidbody is dynamic if and only if mass is non zero, otherwise static
+	bool isDynamic = (mass != 0.f);
+
+	btVector3 localInertia(0, 0, 0);
+	
+	
+	createGround();
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+
+	/////////////////////////////////////////////////////////////////
+}
+
+btMultiBody* BlockSolverExample::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase)
+{
+	//init the base
+	btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+	float baseMass = 1.f;
+
+	if (baseMass)
+	{
+		btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+		pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+		delete pTempBox;
+	}
+
+	bool canSleep = false;
+
+	btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep);
+
+	btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+	pMultiBody->setBasePos(basePosition);
+	pMultiBody->setWorldToBaseRot(baseOriQuat);
+	btVector3 vel(0, 0, 0);
+
+	//init the links
+	btVector3 hingeJointAxis(1, 0, 0);
+	float linkMass = 1.f;
+	btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+
+	btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+	pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+	delete pTempBox;
+
+	//y-axis assumed up
+	btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);                      //par body's COM to cur body's COM offset
+	btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);                         //cur body's COM to cur body's PIV offset
+	btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;  //par body's COM to cur body's PIV offset
+
+	//////
+	btScalar q0 = 0.f * SIMD_PI / 180.f;
+	btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+	quat0.normalize();
+	/////
+
+	for (int i = 0; i < numLinks; ++i)
+	{
+		if (!spherical)
+			pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
+		else
+			//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+			pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
+	}
+
+	pMultiBody->finalizeMultiDof();
+
+	///
+	pWorld->addMultiBody(pMultiBody);
+	///
+	return pMultiBody;
+}
+
+void BlockSolverExample::createGround(const btVector3& halfExtents, btScalar zOffSet)
+{
+	btCollisionShape* groundShape = new btBoxShape(halfExtents);
+	m_collisionShapes.push_back(groundShape);
+
+	// rigidbody is dynamic if and only if mass is non zero, otherwise static
+	btScalar mass(0.);
+	const bool isDynamic = (mass != 0.f);
+
+	btVector3 localInertia(0, 0, 0);
+	if (isDynamic)
+		groundShape->calculateLocalInertia(mass, localInertia);
+
+	// using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0));
+	btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+	btRigidBody* body = new btRigidBody(rbInfo);
+
+	// add the body to the dynamics world
+	m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
+}
+
+void BlockSolverExample::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+	btAlignedObjectArray<btQuaternion> world_to_local;
+	world_to_local.resize(pMultiBody->getNumLinks() + 1);
+
+	btAlignedObjectArray<btVector3> local_origin;
+	local_origin.resize(pMultiBody->getNumLinks() + 1);
+	world_to_local[0] = pMultiBody->getWorldToBaseRot();
+	local_origin[0] = pMultiBody->getBasePos();
+
+	{
+		btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+
+		if (1)
+		{
+			btCollisionShape* box = new btBoxShape(baseHalfExtents);
+			btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+			col->setCollisionShape(box);
+
+			btTransform tr;
+			tr.setIdentity();
+			tr.setOrigin(local_origin[0]);
+			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+			col->setWorldTransform(tr);
+
+			pWorld->addCollisionObject(col, 2, 1 + 2);
+
+			col->setFriction(friction);
+			pMultiBody->setBaseCollider(col);
+		}
+	}
+
+	for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+	{
+		const int parent = pMultiBody->getParent(i);
+		world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+		local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+	}
+
+	for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+	{
+		btVector3 posr = local_origin[i + 1];
+
+		btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
+
+		btCollisionShape* box = new btBoxShape(linkHalfExtents);
+		btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+
+		col->setCollisionShape(box);
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(posr);
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+		col->setWorldTransform(tr);
+		col->setFriction(friction);
+		pWorld->addCollisionObject(col, 2, 1 + 2);
+
+		pMultiBody->getLink(i).m_collider = col;
+	}
+}
+
+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..0b49e8f71
--- /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_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..14f5f45a8
--- /dev/null
+++ b/examples/BlockSolver/btBlockSolver.cpp
@@ -0,0 +1,161 @@
+#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++)
+			//for ( int iteration = maxIterations-1  ; iteration >= 0;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..8ea3e9742
--- /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 btConstraintSolver
+{
+	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..ba6a3fbd7 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
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 8f1f7d37a..cf8ad2f6b 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"
@@ -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_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_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_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_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..e7a237770 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,6 +628,22 @@ 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();
@@ -614,7 +653,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 	return solverConstraint;
 }
 
-void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
+
+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)
@@ -624,11 +664,11 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSol
 
 	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,6 +717,30 @@ 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();
@@ -686,6 +750,195 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionCon
 	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;
+	btAssert(body.getCompanionId() >= 0);
+	//body has already been converted
+	solverBodyIdA = body.getCompanionId();
+	btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	return solverBodyIdA;
+#endif  // BT_THREADSAFE
+}
+
 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep)
 {
 #if BT_THREADSAFE
@@ -789,7 +1042,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 }
 #include <stdio.h>
 
-void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
+
+
+void btSequentialImpulseConstraintSolver::setupContactConstraintInternal(btSISolverSingleIterationData& siData,
+																btSolverConstraint& solverConstraint,
 																 int solverBodyIdA, int solverBodyIdB,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 																 btScalar& relaxation,
@@ -798,8 +1054,8 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 	//	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 +1162,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;
@@ -974,18 +1230,52 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 	}
 }
 
-void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint,
+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;
@@ -1002,7 +1292,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 +1308,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 +1349,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 +1371,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 +1395,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 +1434,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 +1443,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 +1453,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 +1472,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 +1523,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++)
 	{
@@ -1313,7 +1657,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 +1678,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 +1703,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 +1762,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 +1799,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 +1945,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 +1960,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 +1971,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 +2005,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 +2016,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 +2024,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 +2034,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 +2065,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 +2108,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 +2116,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 +2175,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);
 					}
 				}
@@ -1776,31 +2224,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 +2280,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*/)
 {
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 70db83b06..f9dc59a40 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
@@ -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)
@@ -123,13 +186,15 @@ protected:
 	}
 
 protected:
+	
 	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);
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	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
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 1557987bc..a81092782 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -362,7 +362,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 };
 
 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
-	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+	: btDiscreteDynamicsWorld(dispatcher, pairCache, 0, collisionConfiguration),
 	  m_multiBodyConstraintSolver(constraintSolver)
 {
 	//split impulse is not yet supported for Featherstone hierarchies
@@ -380,7 +380,7 @@ void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstrain
 {
 	m_multiBodyConstraintSolver = solver;
 	m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver);
-	btDiscreteDynamicsWorld::setConstraintSolver(solver);
+	//btDiscreteDynamicsWorld::setConstraintSolver(solver);
 }
 
 void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
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()