Merge remote-tracking branch 'bp/master'

14 files changed, 521 insertions, 128 deletions

diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp
index f3ab0cfed..ba699ee64 100644
--- a/examples/BlockSolver/BlockSolverExample.cpp
+++ b/examples/BlockSolver/BlockSolverExample.cpp
@@ -13,6 +13,7 @@
 class BlockSolverExample : public CommonMultiBodyBase
 {
 	int m_option;
+
 public:
 	BlockSolverExample(GUIHelperInterface* helper, int option);
 	virtual ~BlockSolverExample();
@@ -35,10 +36,9 @@ public:
 	btMultiBody* loadRobot(std::string filepath);
 };
 
-
 BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
 	: CommonMultiBodyBase(helper),
-	m_option(option)
+	  m_option(option)
 {
 	m_guiHelper->setUpAxis(2);
 }
@@ -51,13 +51,12 @@ BlockSolverExample::~BlockSolverExample()
 void BlockSolverExample::stepSimulation(float deltaTime)
 {
 	//use a smaller internal timestep, there are stability issues
-	btScalar internalTimeStep = 1./240.f;
+	btScalar internalTimeStep = 1. / 240.f;
 	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
 }
 
 void BlockSolverExample::initPhysics()
 {
-	
 	///collision configuration contains default setup for memory, collision setup
 	m_collisionConfiguration = new btDefaultCollisionConfiguration();
 
@@ -66,37 +65,34 @@ void BlockSolverExample::initPhysics()
 
 	m_broadphase = new btDbvtBroadphase();
 
-
 	btMLCPSolverInterface* mlcp;
-	if (m_option&BLOCK_SOLVER_SI)
+	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)
+	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)
+	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)
+	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);
@@ -104,16 +100,14 @@ void BlockSolverExample::initPhysics()
 	m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
 	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);  //todo: what value is good?
 
-	if (m_option&BLOCK_SOLVER_SCENE_MB_STACK)
+	if (m_option & BLOCK_SOLVER_SCENE_MB_STACK)
 	{
 		createMultiBodyStack();
 	}
-	
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
 
-
 void BlockSolverExample::createMultiBodyStack()
 {
 	///create a few basic rigid bodies
@@ -134,7 +128,7 @@ void BlockSolverExample::createMultiBodyStack()
 		btMultiBody* body = createMultiBody(mass, tr, groundShape);
 	}
 
-	for (int i=0;i<10;i++)
+	for (int i = 0; i < 10; i++)
 	{
 		btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
 		m_collisionShapes.push_back(boxShape);
@@ -143,10 +137,10 @@ void BlockSolverExample::createMultiBodyStack()
 			mass = 100;
 		btTransform tr;
 		tr.setIdentity();
-		tr.setOrigin(btVector3(0, 0, 0.1+i*0.2));
+		tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
 		btMultiBody* body = createMultiBody(mass, tr, boxShape);
 	}
-	if(0)
+	if (0)
 	{
 		btMultiBody* mb = loadRobot("cube_small.urdf");
 		btTransform tr;
@@ -173,25 +167,21 @@ btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransfor
 	int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
 	int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 
-
 	this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
 	mb->setBaseCollider(collider);
 
 	mb->finalizeMultiDof();
 
-
 	this->m_dynamicsWorld->addMultiBody(mb);
 	m_dynamicsWorld->forwardKinematics();
 	return mb;
 }
 
-
-
 btMultiBody* BlockSolverExample::loadRobot(std::string filepath)
 {
 	btMultiBody* m_multiBody = 0;
 	BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
-	bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf");
+	bool loadOk = u2b.loadURDF(filepath.c_str());  // lwr / kuka.urdf");
 	if (loadOk)
 	{
 		int rootLinkIndex = u2b.getRootLinkIndex();
diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h
index 9d2c137be..faa11d885 100644
--- a/examples/BlockSolver/BlockSolverExample.h
+++ b/examples/BlockSolver/BlockSolverExample.h
@@ -2,18 +2,6 @@
 #ifndef BLOCK_SOLVER_EXAMPLE_H
 #define BLOCK_SOLVER_EXAMPLE_H
 
-enum BlockSolverOptions
-{
-	BLOCK_SOLVER_SI=1<<0,
-	BLOCK_SOLVER_MLCP_PGS = 1 << 1,
-	BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
-	BLOCK_SOLVER_BLOCK = 1 << 3,
-
-	BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5,
-	BLOCK_SOLVER_SCENE_CHAIN = 1<< 6,
-	
-};
-
 class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
 
 #endif  //BLOCK_SOLVER_EXAMPLE_H
diff --git a/examples/BlockSolver/RigidBodyBoxes.cpp b/examples/BlockSolver/RigidBodyBoxes.cpp
new file mode 100644
index 000000000..2b3190df2
--- /dev/null
+++ b/examples/BlockSolver/RigidBodyBoxes.cpp
@@ -0,0 +1,150 @@
+#include "RigidBodyBoxes.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "BlockSolverExample.h"
+#include "btBlockSolver.h"
+
+class RigidBodyBoxes : public CommonRigidBodyBase
+{
+	int m_option;
+	int m_numIterations;
+	int m_numBoxes = 4;
+	btAlignedObjectArray<btRigidBody*> boxes;
+	static btScalar numSolverIterations;
+
+public:
+	RigidBodyBoxes(GUIHelperInterface* helper, int option);
+	virtual ~RigidBodyBoxes();
+
+	virtual void initPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+	void resetCubePosition();
+	virtual void resetCamera()
+	{
+		float dist = 3;
+		float pitch = -35;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, .1};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1],
+								 targetPos[2]);
+	}
+
+	void createRigidBodyStack();
+};
+
+btScalar RigidBodyBoxes::numSolverIterations = 50;
+
+RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option)
+	: CommonRigidBodyBase(helper),
+	  m_option(option),
+	  m_numIterations(numSolverIterations)
+{
+	m_guiHelper->setUpAxis(2);
+}
+
+RigidBodyBoxes::~RigidBodyBoxes()
+{
+	// Do nothing
+}
+
+void RigidBodyBoxes::createRigidBodyStack()
+{
+	// create ground
+	btBoxShape* groundShape =
+		createBoxShape(btVector3(btScalar(5.), btScalar(5.), btScalar(5.)));
+	m_collisionShapes.push_back(groundShape);
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, 0, -5));
+	btScalar mass(0.);
+	btRigidBody* body = createRigidBody(mass, groundTransform, groundShape,
+										btVector4(0, 0, 1, 1));
+
+	// create a few boxes
+	mass = 1;
+	for (int i = 0; i < m_numBoxes; i++)
+	{
+		btBoxShape* boxShape =
+			createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
+		m_collisionShapes.push_back(boxShape);
+		mass *= 4;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
+		boxes.push_back(createRigidBody(mass, tr, boxShape));
+	}
+}
+
+void RigidBodyBoxes::initPhysics()
+{
+	/// collision configuration contains default setup for memory, collision setup
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	/// use the default collision dispatcher. For parallel processing you can use
+	/// a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+	m_broadphase = new btDbvtBroadphase();
+
+	{
+		SliderParams slider("numSolverIterations", &numSolverIterations);
+		slider.m_minVal = 5;
+		slider.m_maxVal = 500;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+
+	if (m_option & BLOCK_SOLVER_SI)
+	{
+		m_solver = new btSequentialImpulseConstraintSolver;
+		b3Printf("Constraint Solver: Sequential Impulse");
+	}
+	if (m_option & BLOCK_SOLVER_BLOCK)
+	{
+		m_solver = new btBlockSolver();
+		b3Printf("Constraint Solver: Block solver");
+	}
+
+	btAssert(m_solver);
+
+	m_dynamicsWorld = new btDiscreteDynamicsWorld(
+		m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+
+	createRigidBodyStack();
+
+	m_dynamicsWorld->getSolverInfo().m_numIterations = numSolverIterations;
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);
+
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void RigidBodyBoxes::resetCubePosition()
+{
+	for (int i = 0; i < m_numBoxes; i++)
+	{
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2));
+		boxes[i]->setWorldTransform(tr);
+	}
+}
+
+void RigidBodyBoxes::stepSimulation(float deltaTime)
+{
+	if ((int)numSolverIterations != m_numIterations)
+	{
+		resetCubePosition();
+		m_numIterations = (int)numSolverIterations;
+		m_dynamicsWorld->getSolverInfo().m_numIterations = m_numIterations;
+		b3Printf("New num iterations; %d", m_numIterations);
+	}
+
+	m_dynamicsWorld->stepSimulation(deltaTime);
+}
+
+CommonExampleInterface* RigidBodyBoxesCreateFunc(
+	CommonExampleOptions& options)
+{
+	return new RigidBodyBoxes(options.m_guiHelper, options.m_option);
+}
diff --git a/examples/BlockSolver/RigidBodyBoxes.h b/examples/BlockSolver/RigidBodyBoxes.h
new file mode 100644
index 000000000..0b6e651d6
--- /dev/null
+++ b/examples/BlockSolver/RigidBodyBoxes.h
@@ -0,0 +1,6 @@
+#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_
+#define BLOCKSOLVER_RIGIDBODYBOXES_H_
+
+class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //BLOCKSOLVER_RIGIDBODYBOXES_H_
diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp
index f908cc487..8cf144c14 100644
--- a/examples/BlockSolver/btBlockSolver.cpp
+++ b/examples/BlockSolver/btBlockSolver.cpp
@@ -1,7 +1,17 @@
 #include "btBlockSolver.h"
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
 #include "LinearMath/btQuickprof.h"
 
+void setupHelper(btSISolverSingleIterationData& siData,
+				 btCollisionObject** bodies, int numBodies,
+				 const btContactSolverInfo& info,
+				 btTypedConstraint** constraintStart, int constrainNums,
+				 btPersistentManifold** manifoldPtr, int numManifolds);
+
 struct btBlockSolverInternalData
 {
 	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
@@ -13,86 +23,273 @@ struct btBlockSolverInternalData
 	btAlignedObjectArray<int> m_orderTmpConstraintPool;
 	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
 	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
-	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
-	
-	
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>
+		m_tmpConstraintSizesPool;
+
 	unsigned long m_btSeed2;
 	int m_fixedBodyId;
 	int m_maxOverrideNumSolverIterations;
-	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
+	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())
-	{
-	}
+		: m_btSeed2(0),
+		  m_fixedBodyId(-1),
+		  m_maxOverrideNumSolverIterations(0),
+		  m_resolveSingleConstraintRowGeneric(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarConstraintRowSolverGeneric()),
+		  m_resolveSingleConstraintRowLowerLimit(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarConstraintRowSolverLowerLimit()),
+		  m_resolveSplitPenetrationImpulse(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarSplitPenetrationImpulseGeneric()) {}
 };
 
-
-
-
 btBlockSolver::btBlockSolver()
 {
-	m_data2 = new btBlockSolverInternalData;
+	m_data21 = new btBlockSolverInternalData;
+	m_data22 = new btBlockSolverInternalData;
 }
 
 btBlockSolver::~btBlockSolver()
 {
-	delete m_data2;
+	delete m_data21;
+	delete m_data22;
 }
 
+btScalar btBlockSolver::solveGroupInternalBlock(
+	btCollisionObject** bodies, int numBodies,
+	btPersistentManifold** manifoldPtr, int numManifolds,
+	btTypedConstraint** constraints, int numConstraints,
+	const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
+	btDispatcher* dispatcher)
+{
+	// initialize data for two children solvers
+	btSISolverSingleIterationData siData1(
+		m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
+		m_data21->m_tmpSolverNonContactConstraintPool,
+		m_data21->m_tmpSolverContactFrictionConstraintPool,
+		m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data21->m_orderTmpConstraintPool,
+		m_data21->m_orderNonContactConstraintPool,
+		m_data21->m_orderFrictionConstraintPool,
+		m_data21->m_tmpConstraintSizesPool,
+		m_data21->m_resolveSingleConstraintRowGeneric,
+		m_data21->m_resolveSingleConstraintRowLowerLimit,
+		m_data21->m_resolveSplitPenetrationImpulse,
+		m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
+		m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
+
+	btSISolverSingleIterationData siData2(
+		m_data22->m_tmpSolverBodyPool, m_data22->m_tmpSolverContactConstraintPool,
+		m_data22->m_tmpSolverNonContactConstraintPool,
+		m_data22->m_tmpSolverContactFrictionConstraintPool,
+		m_data22->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data22->m_orderTmpConstraintPool,
+		m_data22->m_orderNonContactConstraintPool,
+		m_data22->m_orderFrictionConstraintPool,
+		m_data22->m_tmpConstraintSizesPool,
+		m_data22->m_resolveSingleConstraintRowGeneric,
+		m_data22->m_resolveSingleConstraintRowLowerLimit,
+		m_data22->m_resolveSplitPenetrationImpulse,
+		m_data22->m_kinematicBodyUniqueIdToSolverBodyTable, m_data22->m_btSeed2,
+		m_data22->m_fixedBodyId, m_data22->m_maxOverrideNumSolverIterations);
+
+	m_data21->m_fixedBodyId = -1;
+	m_data22->m_fixedBodyId = -1;
+
+	// set up
+	int halfNumConstraints1 = numConstraints / 2;
+	int halfNumConstraints2 = numConstraints - halfNumConstraints1;
+
+	int halfNumManifolds1 = numConstraints / 2;
+	int halfNumManifolds2 = numManifolds - halfNumManifolds1;
+
+	setupHelper(siData1, bodies, numBodies, info, constraints,
+				halfNumConstraints1, manifoldPtr, halfNumManifolds1);
+
+	setupHelper(siData2, bodies, numBodies, info,
+				constraints + halfNumConstraints1, halfNumConstraints2,
+				manifoldPtr + halfNumManifolds1, halfNumManifolds2);
+	// set up complete
+
+	// begin solve
+	btScalar leastSquaresResidual = 0;
+	{
+		BT_PROFILE("solveGroupCacheFriendlyIterations");
+		/// this is a special step to resolve penetrations (just for contacts)
+		btSequentialImpulseConstraintSolver::
+			solveGroupCacheFriendlySplitImpulseIterationsInternal(
+				siData1, bodies, numBodies, manifoldPtr, halfNumManifolds1,
+				constraints, halfNumConstraints1, info, debugDrawer);
+
+		btSequentialImpulseConstraintSolver::
+			solveGroupCacheFriendlySplitImpulseIterationsInternal(
+				siData2, bodies, numBodies, manifoldPtr + halfNumManifolds1,
+				halfNumManifolds2, constraints + halfNumConstraints1,
+				halfNumConstraints2, info, debugDrawer);
+
+		int maxIterations =
+			siData1.m_maxOverrideNumSolverIterations > info.m_numIterations
+				? siData1.m_maxOverrideNumSolverIterations
+				: info.m_numIterations;
+
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		{
+			btScalar res1 =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData1, iteration, constraints, halfNumConstraints1, info);
+
+			btScalar res2 =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData2, iteration, constraints + halfNumConstraints1,
+					halfNumConstraints2, info);
+			leastSquaresResidual = btMax(res1, res2);
+
+			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 btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+	btScalar res = btSequentialImpulseConstraintSolver::
+		solveGroupCacheFriendlyFinishInternal(siData1, bodies, numBodies, info);
+	+btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(
+		siData2, bodies, numBodies, info);
+
+	return res;
+}
+
+void setupHelper(btSISolverSingleIterationData& siData,
+				 btCollisionObject** bodies, int numBodies,
+				 const btContactSolverInfo& info,
+				 btTypedConstraint** constraintStart, int constrainNums,
+				 btPersistentManifold** manifoldPtr, int numManifolds)
 {
+	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
+															   numBodies, info);
+	btSequentialImpulseConstraintSolver::convertJointsInternal(
+		siData, constraintStart, constrainNums, info);
 
-	btSISolverSingleIterationData siData(m_data2->m_tmpSolverBodyPool,
-		m_data2->m_tmpSolverContactConstraintPool,
-		m_data2->m_tmpSolverNonContactConstraintPool,
-		m_data2->m_tmpSolverContactFrictionConstraintPool,
-		m_data2->m_tmpSolverContactRollingFrictionConstraintPool,
-		m_data2->m_orderTmpConstraintPool,
-		m_data2->m_orderNonContactConstraintPool,
-		m_data2->m_orderFrictionConstraintPool,
-		m_data2->m_tmpConstraintSizesPool,
-		m_data2->m_resolveSingleConstraintRowGeneric,
-		m_data2->m_resolveSingleConstraintRowLowerLimit,
-		m_data2->m_resolveSplitPenetrationImpulse,
-		m_data2->m_kinematicBodyUniqueIdToSolverBodyTable,
-		m_data2->m_btSeed2,
-		m_data2->m_fixedBodyId,
-		m_data2->m_maxOverrideNumSolverIterations);
-	
-	m_data2->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);
+		btSequentialImpulseConstraintSolver::convertContactInternal(siData,
+																	manifold, info);
+
+		int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
+		int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
+		int numFrictionPool =
+			siData.m_tmpSolverContactFrictionConstraintPool.size();
+
+		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 btBlockSolver::solveGroup(btCollisionObject** bodies, int numBodies,
+								   btPersistentManifold** manifoldPtr,
+								   int numManifolds,
+								   btTypedConstraint** constraints,
+								   int numConstraints,
+								   const btContactSolverInfo& info,
+								   btIDebugDraw* debugDrawer,
+								   btDispatcher* dispatcher)
+{
+	// if (m_childSolvers.size())
+	// hard code to use block solver for now
+	return solveGroupInternalBlock(bodies, numBodies, manifoldPtr, numManifolds,
+								   constraints, numConstraints, info, debugDrawer,
+								   dispatcher);
+	//  else
+	//    return solveGroupInternal(bodies, numBodies, manifoldPtr, numManifolds,
+	//                             constraints, numConstraints, info, debugDrawer,
+	//                             dispatcher);
+}
+
+btScalar btBlockSolver::solveGroupInternal(
+	btCollisionObject** bodies, int numBodies,
+	btPersistentManifold** manifoldPtr, int numManifolds,
+	btTypedConstraint** constraints, int numConstraints,
+	const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
+	btDispatcher* dispatcher)
+{
+	btSISolverSingleIterationData siData(
+		m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
+		m_data21->m_tmpSolverNonContactConstraintPool,
+		m_data21->m_tmpSolverContactFrictionConstraintPool,
+		m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data21->m_orderTmpConstraintPool,
+		m_data21->m_orderNonContactConstraintPool,
+		m_data21->m_orderFrictionConstraintPool,
+		m_data21->m_tmpConstraintSizesPool,
+		m_data21->m_resolveSingleConstraintRowGeneric,
+		m_data21->m_resolveSingleConstraintRowLowerLimit,
+		m_data21->m_resolveSplitPenetrationImpulse,
+		m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
+		m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
+
+	m_data21->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
+	// @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);
@@ -118,43 +315,60 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b
 
 	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);
+		/// 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;
+		int maxIterations =
+			siData.m_maxOverrideNumSolverIterations > info.m_numIterations
+				? siData.m_maxOverrideNumSolverIterations
+				: info.m_numIterations;
 
 		for (int iteration = 0; iteration < maxIterations; iteration++)
 		{
-			leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info);
+			leastSquaresResidual =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData, iteration, constraints, numConstraints, info);
 
-			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
+				(iteration >= (maxIterations - 1)))
 			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
+					   iteration);
 #endif
 				break;
 			}
 		}
 	}
 
-	btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
+	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)
+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);
+	btMultiBodyConstraintSolver::solveMultiBodyGroup(
+		bodies, numBodies, manifold, numManifolds, constraints, numConstraints,
+		multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer,
+		dispatcher);
 }
 
 void btBlockSolver::reset()
 {
-	//or just set m_data2->m_btSeed2=0?
-	delete m_data2;
-	m_data2 = new btBlockSolverInternalData;
+	// or just set m_data2->m_btSeed2=0?
+	delete m_data21;
+	delete m_data22;
+	m_data21 = new btBlockSolverInternalData;
+	m_data22 = new btBlockSolverInternalData;
 }
diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h
index 7cb5dd3c2..271ad399d 100644
--- a/examples/BlockSolver/btBlockSolver.h
+++ b/examples/BlockSolver/btBlockSolver.h
@@ -1,24 +1,72 @@
 #ifndef BT_BLOCK_SOLVER_H
 #define BT_BLOCK_SOLVER_H
 
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "Bullet3Common/b3Logging.h"
+
+enum BlockSolverOptions
+{
+	BLOCK_SOLVER_SI = 1 << 0,
+	BLOCK_SOLVER_MLCP_PGS = 1 << 1,
+	BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
+	BLOCK_SOLVER_BLOCK = 1 << 3,
+
+	BLOCK_SOLVER_SCENE_MB_STACK = 1 << 5,
+	BLOCK_SOLVER_SCENE_CHAIN = 1 << 6,
+};
 
 class btBlockSolver : public btMultiBodyConstraintSolver
 {
-	struct btBlockSolverInternalData* m_data2;
+  struct btBlockSolverInternalData* m_data21;
+  struct btBlockSolverInternalData* m_data22;
+ 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);
 
-public:
-	btBlockSolver();
-	virtual ~btBlockSolver();
+  btScalar solveGroupInternal(btCollisionObject** bodies, int numBodies,
+                              btPersistentManifold** manifoldPtr,
+                              int numManifolds,
+                              btTypedConstraint** constraints,
+                              int numConstraints,
+                              const btContactSolverInfo& info,
+                              btIDebugDraw* debugDrawer,
+                              btDispatcher* dispatcher);
 
-	//btRigidBody
-	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+  btScalar solveGroupInternalBlock(btCollisionObject** bodies, int numBodies,
+                                   btPersistentManifold** manifoldPtr,
+                                   int numManifolds,
+                                   btTypedConstraint** constraints,
+                                   int numConstraints,
+                                   const btContactSolverInfo& info,
+                                   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);
+  // 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();
+  /// clear internal cached data and reset random seed
+  virtual void reset();
 
 	virtual btConstraintSolverType getSolverType() const
 	{
@@ -26,4 +74,4 @@ public:
 	}
 };
 
-#endif //BT_BLOCK_SOLVER_H
+#endif  //BT_BLOCK_SOLVER_H
diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp
index a5a89e329..6e33cbfc9 100644
--- a/examples/BulletRobotics/FixJointBoxes.cpp
+++ b/examples/BulletRobotics/FixJointBoxes.cpp
@@ -1,4 +1,3 @@
-
 #include "FixJointBoxes.h"
 
 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
@@ -75,7 +74,7 @@ public:
 				if (i > 0)
 				{
 					m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo);
-                    m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0);
+					m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0);
 				}
 
 				m_robotSim.loadURDF("plane.urdf");
@@ -111,8 +110,8 @@ public:
 	{
 		for (int i = 0; i < numCubes; i++)
 		{
-                  btVector3 pos (0, i * (btScalar)0.05, 1);
-                  btQuaternion quar (0, 0, 0, 1);
+			btVector3 pos(0, i * (btScalar)0.05, 1);
+			btQuaternion quar(0, 0, 0, 1);
 			m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar);
 		}
 	}
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index a0dcfe57e..ceb6d9590 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -211,6 +211,8 @@ SET(BulletExampleBrowser_SRCS
 	../BlockSolver/btBlockSolver.h
 	../BlockSolver/BlockSolverExample.cpp
 	../BlockSolver/BlockSolverExample.h
+	../BlockSolver/RigidBodyBoxes.cpp
+	../BlockSolver/RigidBodyBoxes.h
 	../Tutorial/Tutorial.cpp
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
index cfc22e05d..6443df29d 100644
--- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
+++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
@@ -132,14 +132,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 			{
 				btConvexShape* convex = (btConvexShape*)collisionShape;
 				{
-					
 					const btConvexPolyhedron* pol = 0;
 					if (convex->isPolyhedral())
 					{
 						btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
 						pol = poly->getConvexPolyhedron();
 					}
-					
+
 					if (pol)
 					{
 						for (int v = 0; v < pol->m_vertices.size(); v++)
@@ -151,19 +150,16 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 						}
 						for (int f = 0; f < pol->m_faces.size(); f++)
 						{
-
 							for (int ii = 2; ii < pol->m_faces[f].m_indices.size(); ii++)
 							{
 								indicesOut.push_back(pol->m_faces[f].m_indices[0]);
-								indicesOut.push_back(pol->m_faces[f].m_indices[ii-1]);
+								indicesOut.push_back(pol->m_faces[f].m_indices[ii - 1]);
 								indicesOut.push_back(pol->m_faces[f].m_indices[ii]);
 							}
 						}
-
-					} 
+					}
 					else
 					{
-
 						btShapeHull* hull = new btShapeHull(convex);
 						hull->buildHull(0.0, 1);
 
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index a299893f4..f2e3901b7 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -1,6 +1,8 @@
 #include "ExampleEntries.h"
 
+#include "../BlockSolver/btBlockSolver.h"
 #include "../BlockSolver/BlockSolverExample.h"
+#include "../BlockSolver/RigidBodyBoxes.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
@@ -129,8 +131,6 @@ static ExampleEntry gDefaultExamples[] =
 
 		ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc),
 
-
-
 		ExampleEntry(0, "MultiBody"),
 		ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 		ExampleEntry(1, "TestJointTorque", "Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
@@ -139,8 +139,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 		ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 		ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0),
-		
-	
+
 		ExampleEntry(0, "Physics Client-Server"),
 		ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",
 					 PhysicsServerCreateFuncBullet2),
@@ -153,12 +152,13 @@ static ExampleEntry gDefaultExamples[] =
 		//
 		//	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
 
-
 		ExampleEntry(0, "BlockSolver"),
-		ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK+ BLOCK_SOLVER_SI),
+		ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_SI),
 		ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
 		ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
 		ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
+		ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI),
+		ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, 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),
diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
index d5c9ccac5..c7a2e8a69 100644
--- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
+++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp
@@ -1127,7 +1127,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 
 		gui2->registerFileOpenCallback(fileOpenCallback);
 		gui2->registerQuitCallback(quitCallback);
-		
 	}
 
 	return true;
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 702cb3718..da65be89c 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1699,6 +1699,7 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_logPlaybackUid(-1),
 		  m_physicsDeltaTime(1. / 240.),
 		  m_numSimulationSubSteps(0),
+                  m_simulationTimestamp(0),
 		  m_userConstraintUIDGenerator(1),
 		  m_broadphaseCollisionFilterCallback(0),
 		  m_pairCache(0),
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 92866dc0c..fe3b18799 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1277,11 +1277,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	PyObject* localInertiaDiagonalObj = 0;
 	PyObject* anisotropicFrictionObj = 0;
 	double maxJointVelocity = -1;
-
+	
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity",  "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId))
 	{
 		return NULL;
@@ -1374,7 +1374,7 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity);
 		}
-
+				
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 
diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
index 166cf771f..b7fe0a1f3 100644
--- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
+++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp
@@ -123,11 +123,11 @@ protected:
 
 void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
 {
+	m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
+
 	btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
 	freeHandle(proxy0);
 
-	m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
-
 	//validate();
 }