prepare small experiment with block solver

examples/BlockSolver/BlockSolverExample.cpp

@@ -115,7 +115,7 @@ void BlockSolverExample::initPhysics()
 	}
 	if (m_option&BLOCK_SOLVER_BLOCK)
 	{
-		//m_solver = new btBlockSolver();
+		m_solver = new btBlockSolver();
 	}
 	
 	btAssert(m_solver);

examples/BlockSolver/btBlockSolver.h

@@ -3,7 +3,7 @@
 
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 
-class btBlockSolver : public btConstraintSolver
+class btBlockSolver : public btMultiBodyConstraintSolver
 {
 	struct btBlockSolverInternalData* m_data;
 

examples/ConstraintSolvers/BoxStacks.cpp

@@ -0,0 +1,335 @@
+#include "BoxStacks.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/btMultiBodyBlockConstraintSolver.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 "../OpenGLWindow/GLInstancingRenderer.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+class BoxStacks : public CommonMultiBodyBase
+{
+public:
+    BoxStacks(GUIHelperInterface* helper);
+    virtual ~BoxStacks();
+
+	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]);
+	}
+
+	void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY);
+	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.;
+static int g_constraintSolverType = 0;
+
+BoxStacks::BoxStacks(GUIHelperInterface* helper)
+	: CommonMultiBodyBase(helper)
+{
+	m_guiHelper->setUpAxis(1);
+}
+
+BoxStacks::~BoxStacks()
+{
+	// Do nothing
+}
+
+void BoxStacks::stepSimulation(float deltaTime)
+{
+	//use a smaller internal timestep, there are stability issues
+	float internalTimeStep = 1. / 240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+}
+
+void BoxStacks::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();
+
+	if (g_constraintSolverType == 3)
+	{
+		g_constraintSolverType = 0;
+		g_fixedBase = !g_fixedBase;
+	}
+
+	btMLCPSolverInterface* mlcp;
+	switch (g_constraintSolverType++)
+	{
+		case 0:
+			m_solver = new btMultiBodyConstraintSolver;
+			b3Printf("Constraint Solver: Sequential Impulse");
+			break;
+		case 1:
+			mlcp = new btSolveProjectedGaussSeidel();
+			m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + PGS");
+			break;
+		default:
+			mlcp = new btDantzigSolver();
+			m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + Dantzig");
+			break;
+	}
+	m_solver = new btMultiBodyBlockConstraintSolver();
+
+	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_dynamicsWorld = world;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0)));
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
+
+	/// Create a few basic rigid bodies
+	btVector3 groundHalfExtents(50, 50, 50);
+	btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
+
+	m_collisionShapes.push_back(groundShape);
+
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, -50, 00));
+
+	btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
+	btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
+
+	createBoxStack(1, 0, 0);
+
+	btScalar groundHeight = btScalar(-51.55);
+	btScalar mass = btScalar(0.0);
+
+	btVector3 localInertia(0, 0, 0);
+	groundShape->calculateLocalInertia(mass, localInertia);
+
+	// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, groundHeight, 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);
+
+	createGround();
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void BoxStacks::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ)
+{
+	//create a few dynamic rigidbodies
+	// Re-using the same collision is better for memory usage and performance
+
+	const btScalar boxHalfSize = btScalar(0.1);
+
+	btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize));
+	m_collisionShapes.push_back(colShape);
+
+	/// Create Dynamic Objects
+	btTransform startTransform;
+	startTransform.setIdentity();
+
+	btScalar mass(1.0);
+
+	btVector3 localInertia(0, 0, 0);
+	colShape->calculateLocalInertia(mass,localInertia);
+
+	for (int i = 0; i < numBoxes; ++i)
+	{
+		startTransform.setOrigin(btVector3(centerX, 1+btScalar(btScalar(2) * boxHalfSize * i), centerZ));
+		createRigidBody(mass, startTransform, colShape);
+	}
+}
+
+btMultiBody* BoxStacks::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 BoxStacks::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 BoxStacks::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* BoxStacksCreateFunc(CommonExampleOptions& options)
+{
+    return new BoxStacks(options.m_guiHelper);
+}

examples/ConstraintSolvers/BoxStacks.h

@@ -0,0 +1,7 @@
+
+#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H
+#define CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H
+
+class CommonExampleInterface* BoxStacksCreateFunc(struct CommonExampleOptions& options);
+
+#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H

examples/ConstraintSolvers/BoxStacks_MLCP.cpp

@@ -0,0 +1,224 @@
+#include "BoxStacks_MLCP.h"
+#include "../OpenGLWindow/SimpleOpenGL3App.h"
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+
+
+class BoxStacks_MLCP : public CommonMultiBodyBase
+{
+public:
+	BoxStacks_MLCP(GUIHelperInterface* helper);
+	virtual ~BoxStacks_MLCP();
+
+	virtual void initPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+
+	virtual void resetCamera()
+	{
+		float dist = 2;
+		float pitch = -35;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+	btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
+
+	btMultiBody* loadRobot(std::string filepath = "kuka_iiwa/model.urdf");
+};
+
+
+
+static int g_constraintSolverType = 0;
+
+BoxStacks_MLCP::BoxStacks_MLCP(GUIHelperInterface* helper)
+	: CommonMultiBodyBase(helper)
+{
+	
+}
+
+BoxStacks_MLCP::~BoxStacks_MLCP()
+{
+	// Do nothing
+}
+
+void BoxStacks_MLCP::stepSimulation(float deltaTime)
+{
+	float internalTimeStep = 1. / 240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+	for (int i = 0; i < m_dynamicsWorld->getNumMultibodies(); i++)
+	{
+		btVector3 pos = m_dynamicsWorld->getMultiBody(i)->getBaseWorldTransform().getOrigin();
+		printf("pos[%d]=%f,%f,%f\n", i, pos.x(), pos.y(), pos.z());
+	}
+}
+
+void BoxStacks_MLCP::initPhysics()
+{
+	m_guiHelper->setUpAxis(2);
+
+	createEmptyDynamicsWorld();
+	m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	if (g_constraintSolverType == 5)
+	{
+		g_constraintSolverType = 0;
+	}
+
+	btMultiBodyConstraintSolver* sol = 0;
+	
+	btMLCPSolverInterface* mlcp;
+	switch (g_constraintSolverType++)
+	{
+		case 0:
+			sol = new btMultiBodyConstraintSolver;
+			b3Printf("Constraint Solver: Sequential Impulse");
+			break;
+		case 1:
+			mlcp = new btSolveProjectedGaussSeidel();
+			sol = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + PGS");
+			break;
+		case 2:
+			mlcp = new btDantzigSolver();
+			sol = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + Dantzig");
+			break;
+		case 3:
+			mlcp = new btLemkeSolver();
+			sol = new btMultiBodyMLCPConstraintSolver(mlcp);
+			
+			b3Printf("Constraint Solver: MLCP + Lemke");
+			break;
+			
+		default:
+			sol = new btMultiBodyBlockConstraintSolver();
+			b3Printf("btMultiBodyBlockConstraintSolver");
+			break;
+	}
+
+	m_solver = sol;
+
+	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
+	m_dynamicsWorld = world;
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4);
+
+
+	m_dynamicsWorld->setGravity(btVector3(0,0,-10));
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	if (m_dynamicsWorld->getDebugDrawer())
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+
+	///create a few basic rigid bodies
+	bool loadPlaneFromURDF = true;
+	if (loadPlaneFromURDF)
+	{
+		loadRobot("plane.urdf");
+	} else
+	{
+		btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
+		m_collisionShapes.push_back(groundShape);
+		btScalar mass = 0;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, -50));
+		btMultiBody* body = createMultiBody(mass, tr, groundShape);
+	}
+
+	{
+		btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
+		m_collisionShapes.push_back(boxShape);
+		btScalar mass = 10;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 0.5));
+		btMultiBody* body = createMultiBody(mass, tr, boxShape);
+	}
+
+	{
+		btMultiBody* mb = loadRobot("cube_small.urdf");
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 1.));
+		mb->setBaseWorldTransform(tr);
+	}
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+
+btMultiBody* BoxStacks_MLCP::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape)
+{
+	btVector3 inertia;
+	collisionShape->calculateLocalInertia(mass, inertia);
+	
+	bool canSleep = false;
+	bool isDynamic = mass > 0;
+	btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic,canSleep);
+	btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1);
+	collider->setWorldTransform(trans);
+	mb->setBaseWorldTransform(trans);
+
+	collider->setCollisionShape(collisionShape);
+
+	int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+	int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+
+	this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
+	mb->setBaseCollider(collider);
+
+	mb->finalizeMultiDof();
+
+
+	this->m_dynamicsWorld->addMultiBody(mb);
+	m_dynamicsWorld->forwardKinematics();
+	return mb;
+}
+
+
+
+btMultiBody*BoxStacks_MLCP::loadRobot(std::string filepath)
+{
+	btMultiBody* m_multiBody = 0;
+	BulletURDFImporter u2b(m_guiHelper,0,0,1,0);
+	bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf");
+	if (loadOk)
+	{
+			int rootLinkIndex = u2b.getRootLinkIndex();
+			b3Printf("urdf root link index = %d\n",rootLinkIndex);
+			MyMultiBodyCreator creation(m_guiHelper);
+			btTransform identityTrans;
+			identityTrans.setIdentity();
+			ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix());
+			for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+			{
+				m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+			}
+			m_multiBody = creation.getBulletMultiBody();
+			if (m_multiBody)
+			{
+				b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
+			}
+	}
+	return m_multiBody;
+}
+
+CommonExampleInterface* BoxStacks_MLCPCreateFunc(CommonExampleOptions& options)
+{
+	return new BoxStacks_MLCP(options.m_guiHelper);
+}

examples/ConstraintSolvers/BoxStacks_MLCP.h

@@ -0,0 +1,7 @@
+
+#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H
+#define CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H
+
+class CommonExampleInterface* BoxStacks_MLCPCreateFunc(struct CommonExampleOptions& options);
+
+#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H

examples/ConstraintSolvers/Grasp_Block.cpp

@@ -0,0 +1,335 @@
+#include "Grasp_Block.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/btMultiBodyBlockConstraintSolver.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 "../OpenGLWindow/GLInstancingRenderer.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+class Grasp_Block : public CommonMultiBodyBase
+{
+public:
+    Grasp_Block(GUIHelperInterface* helper);
+    virtual ~Grasp_Block();
+
+	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]);
+	}
+
+	void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY);
+	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.;
+static int g_constraintSolverType = 0;
+
+Grasp_Block::Grasp_Block(GUIHelperInterface* helper)
+	: CommonMultiBodyBase(helper)
+{
+	m_guiHelper->setUpAxis(1);
+}
+
+Grasp_Block::~Grasp_Block()
+{
+	// Do nothing
+}
+
+void Grasp_Block::stepSimulation(float deltaTime)
+{
+	//use a smaller internal timestep, there are stability issues
+	float internalTimeStep = 1. / 240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+}
+
+void Grasp_Block::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();
+
+	if (g_constraintSolverType == 3)
+	{
+		g_constraintSolverType = 0;
+		g_fixedBase = !g_fixedBase;
+	}
+
+	btMLCPSolverInterface* mlcp;
+	switch (g_constraintSolverType++)
+	{
+		case 0:
+			m_solver = new btMultiBodyConstraintSolver;
+			b3Printf("Constraint Solver: Sequential Impulse");
+			break;
+		case 1:
+			mlcp = new btSolveProjectedGaussSeidel();
+			m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + PGS");
+			break;
+		default:
+			mlcp = new btDantzigSolver();
+			m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+			b3Printf("Constraint Solver: MLCP + Dantzig");
+			break;
+	}
+	m_solver = new btMultiBodyBlockConstraintSolver();
+
+	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_dynamicsWorld = world;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0)));
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
+
+	/// Create a few basic rigid bodies
+	btVector3 groundHalfExtents(50, 50, 50);
+	btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
+
+	m_collisionShapes.push_back(groundShape);
+
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, -50, 00));
+
+	btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
+	btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
+
+//	createBoxStack(5, 0, 0);
+
+	btScalar groundHeight = btScalar(-51.55);
+	btScalar mass = btScalar(0.0);
+
+	btVector3 localInertia(0, 0, 0);
+	groundShape->calculateLocalInertia(mass, localInertia);
+
+	// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, groundHeight, 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);
+
+	createGround();
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void Grasp_Block::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ)
+{
+	//create a few dynamic rigidbodies
+	// Re-using the same collision is better for memory usage and performance
+
+	const btScalar boxHalfSize = btScalar(0.1);
+
+	btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize));
+	m_collisionShapes.push_back(colShape);
+
+	/// Create Dynamic Objects
+	btTransform startTransform;
+	startTransform.setIdentity();
+
+	btScalar mass(1.0);
+
+	btVector3 localInertia(0, 0, 0);
+	colShape->calculateLocalInertia(mass,localInertia);
+
+	for (int i = 0; i < numBoxes; ++i)
+	{
+		startTransform.setOrigin(btVector3(centerX, btScalar(btScalar(2) * boxHalfSize * i), centerZ));
+		createRigidBody(mass, startTransform, colShape);
+	}
+}
+
+btMultiBody* Grasp_Block::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 Grasp_Block::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 Grasp_Block::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* Grasp_BlockCreateFunc(CommonExampleOptions& options)
+{
+    return new Grasp_Block(options.m_guiHelper);
+}

examples/ConstraintSolvers/Grasp_Block.h

@@ -0,0 +1,7 @@
+
+#ifndef CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H
+#define CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H
+
+class CommonExampleInterface* Grasp_BlockCreateFunc(struct CommonExampleOptions& options);
+
+#endif // CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H

examples/ConstraintSolvers/SerialChains.cpp

@@ -7,6 +7,7 @@
 
 #include "BulletDynamics/Featherstone/btMultiBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -112,6 +113,7 @@ void SerialChains::initPhysics()
 			b3Printf("Constraint Solver: MLCP + Dantzig");
 			break;
 	}
+	m_solver = new btMultiBodyBlockConstraintSolver();
 
 	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
 	m_dynamicsWorld = world;
@@ -237,6 +239,53 @@ void SerialChains::initPhysics()
 
 	createGround();
 
+	{
+		btVector3 halfExtents(.5,.5,.5);
+		btBoxShape* colShape = new btBoxShape(halfExtents);
+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		btScalar	mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0,0,0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass,localInertia);
+
+		startTransform.setOrigin(btVector3(
+							btScalar(0.0),
+							0.0,
+							btScalar(0.0)));
+
+
+		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
+//		btRigidBody* body = new btRigidBody(rbInfo);
+
+//		m_dynamicsWorld->addRigidBody(body);//,1,1+2);
+
+		{
+			btVector3 pointInA = -linkHalfExtents;
+	  //      btVector3 pointInB = halfExtents;
+			btMatrix3x3 frameInA;
+			btMatrix3x3 frameInB;
+			frameInA.setIdentity();
+			frameInB.setIdentity();
+			btVector3 jointAxis(1.0,0.0,0.0);
+			//btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
+			btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbC1, numLinks- 1 , mbC2, numLinks - 1, pointInA, pointInA);
+			p2p->setMaxAppliedImpulse(20.0);
+			m_dynamicsWorld->addMultiBodyConstraint(p2p);
+		}
+	}
+
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 
 	/////////////////////////////////////////////////////////////////

examples/ExampleBrowser/CMakeLists.txt

@@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreadedDemo/MultiThreadedDemo.h
 	../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
 	../MultiThreadedDemo/CommonRigidBodyMTBase.h
+	../ConstraintSolvers/BoxStacks.cpp
+	../ConstraintSolvers/BoxStacks_MLCP.cpp
+	../ConstraintSolvers/Grasp_Block.cpp
+	../ConstraintSolvers/SerialChains.cpp
 	../BlockSolver/btBlockSolver.cpp
 	../BlockSolver/btBlockSolver.h
 	../BlockSolver/BlockSolverExample.cpp
@@ -337,7 +341,6 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
  ../MultiBody/MultiBodyConstraintFeedback.cpp
-  ../MultiBody/SerialChains.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -21,6 +21,10 @@
 #include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
 #include "../Importers/ImportMJCFDemo/ImportMJCFSetup.h"
 #include "../Collision/CollisionTutorialBullet2.h"
+#include "../ConstraintSolvers/SerialChains.h"
+#include "../ConstraintSolvers/BoxStacks.h"
+#include "../ConstraintSolvers/BoxStacks_MLCP.h"
+#include "../ConstraintSolvers/Grasp_Block.h"
 #include "../GyroscopicDemo/GyroscopicSetup.h"
 #include "../Constraints/Dof6Spring2Setup.h"
 #include "../Constraints/ConstraintPhysicsSetup.h"
@@ -30,7 +34,7 @@
 #include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../MultiBody/InvertedPendulumPDControl.h"
-#include "../MultiBody/SerialChains.h"
+
 #include "../RigidBody/RigidBodySoftContact.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
@@ -137,7 +141,13 @@ 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, "Constraint Solvers"),
 		ExampleEntry(1, "Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc, 0),
+		ExampleEntry(1, "Box Stack", "Show box stacks with different constraint solvers for each stack.", BoxStacksCreateFunc, 0),
+		ExampleEntry(1, "Box Stack MLCP", "Show box stacks with different constraint solvers for each stack.", BoxStacks_MLCPCreateFunc, 0),
+		ExampleEntry(1, "Grasp Block", "Show box stacks with different constraint solvers for each stack.", Grasp_BlockCreateFunc, 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.",

examples/ExampleBrowser/premake4.lua

@@ -163,6 +163,7 @@ project "App_BulletExampleBrowser"
 		"../Collision/*",
 		"../RoboticsLearning/*",
 		"../BlockSolver/*",
+		"../ConstraintSolvers/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
 		"../MultiThreadedDemo/*",

src/BulletDynamics/CMakeLists.txt

@@ -32,6 +32,7 @@ SET(BulletDynamics_SRCS
 	Vehicle/btWheelInfo.cpp
 	Featherstone/btMultiBody.cpp
 	Featherstone/btMultiBodyConstraint.cpp
+	Featherstone/btMultiBodyBlockConstraintSolver.cpp
 	Featherstone/btMultiBodyConstraintSolver.cpp
 	Featherstone/btMultiBodyDynamicsWorld.cpp
 	Featherstone/btMultiBodyFixedConstraint.cpp

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -931,10 +931,32 @@ int btSISolverSingleIterationData::getSolverBody(btCollisionObject& body) const
 	return solverBodyId;
 #else   // BT_THREADSAFE
 	int solverBodyIdA = -1;
-	btAssert(body.getCompanionId() >= 0);
+
+	if (body.getCompanionId() >= 0)
+	{
 		//body has already been converted
 		solverBodyIdA = body.getCompanionId();
 		btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
+	}
+	else
+	{
+		btRigidBody* rb = btRigidBody::upcast(&body);
+		//convert both active and kinematic objects (for their velocity)
+		if (rb && (rb->getInvMass() || rb->isKinematicObject()))
+		{
+			btAssert(0);
+		}
+		else
+		{
+			if (m_fixedBodyId < 0)
+			{
+				btAssert(0);
+			}
+			return m_fixedBodyId;
+			//			return 0;//assume first one is a fixed solver body
+		}
+	}
+
 	return solverBodyIdA;
 #endif  // BT_THREADSAFE
 }
@@ -1822,7 +1844,16 @@ void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodi
 	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)
+{
+	solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	solveGroupConvertConstraints(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	return 0.;
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	m_fixedBodyId = -1;
 	BT_PROFILE("solveGroupCacheFriendlySetup");
@@ -1912,6 +1943,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 	convertContacts(manifoldPtr, numManifolds, infoGlobal);
 
+	
+	return 0.f;
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/)
+{
+	return 0;
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/)
+{
 	//	btContactSolverInfo info = infoGlobal;
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
@@ -1942,9 +1984,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 		}
 	}
 
-	return 0.f;
+	return 0;
 }
-
 btScalar btSequentialImpulseConstraintSolver::solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("solveSingleIteration");
@@ -2372,3 +2413,33 @@ void btSequentialImpulseConstraintSolver::reset()
 {
 	m_btSeed2 = 0;
 }
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	return btScalar(0);
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal);
+	}
+
+	writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal);
+	writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal);
+
+	return btScalar(0);
+}
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
+	m_tmpSolverBodyPool.resizeNoInitialize(0);
+
+	return btScalar(0);
+}

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -185,7 +185,7 @@ protected:
 		return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
 	}
 
-protected:
+public:
 	
 	void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
 	void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -296,6 +296,14 @@ public:
 	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
 	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
 
+	virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& /*infoGlobal*/, btIDebugDraw* /*debugDrawer*/);
+	virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** /*bodies*/, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** /*constraints*/, int /*numConstraints*/, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/);
+	virtual btScalar solveGroupConvertConstraints(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+	virtual btScalar solveGroupConvertBackPrestep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupConvertBack(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+	virtual btScalar solveGroupConvertBackPoststep(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
+
 };
 
 #endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.cpp

@@ -0,0 +1,760 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btMultiBodyBlockConstraintSolver.h"
+
+#include <string.h>
+
+#include "LinearMath/btQuickprof.h"
+#include "btMultiBodyMLCPConstraintSolver.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+
+static void rigidBodyNotSupported()
+{
+	printf("Attempts to use rigid body for block solver, which is not supported yet.\n");
+	btAssert(false);
+}
+
+btMultiBodyConstraintBlock::btMultiBodyConstraintBlock()
+	: m_constraintConfigId(-1)
+{
+	// Do nothing
+}
+
+btMultiBodyConstraintBlock::btMultiBodyConstraintBlock(
+	btTypedConstraint** m_constraints,
+	int m_numConstraints,
+	btAlignedObjectArray<btSolverBody>* m_solverBodyPool,
+	btConstraintArray& m_nonContactConstraints,
+	btConstraintArray& m_normalContactConstraints,
+	btConstraintArray& m_frictionContactConstraints,
+	btConstraintArray& m_rollingFrictionContactConstraints,
+	btMultiBodyConstraint** m_multiBodyConstraints,
+	int m_numMultiBodyConstraints,
+	btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyNonContactConstraints,
+	btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyNormalContactConstraints,
+	btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyFrictionContactConstraints,
+	btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyTorsionalFrictionContactConstraints,
+	btMultiBodyJacobianData* m_data)
+	: m_constraintConfigId(-1)
+{
+	// Do nothing
+}
+
+static void copyConstraintDynamicDataToBlock(btAlignedObjectArray<btMultiBodySolverConstraint*>& originalConstraints, const btAlignedObjectArray<btMultiBodySolverConstraint>& blockConstraints)
+{
+	btAssert(originalConstraints.size() == blockConstraints.size());
+	for (int i = 0; i < blockConstraints.size(); ++i)
+	{
+		btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i];
+		const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i];
+
+		blockConstraint.m_appliedImpulse = originalConstraint.m_appliedImpulse;
+		blockConstraint.m_appliedPushImpulse = originalConstraint.m_appliedPushImpulse;
+	}
+}
+
+void debugPrint(const btScalar* data, int size)
+{
+	for (int i = 0; i < size; ++i)
+	{
+		printf("\t%.5f", data[i]);
+		if (i != size - 1)
+			printf(",");
+	}
+	printf("\n");
+}
+
+void debugPrintDiff(const btScalar* dataA, const btScalar* dataB, int size, bool ignoreZero = false)
+{
+	for (int i = 0; i < size; ++i)
+	{
+		if (ignoreZero)
+		{
+			if (btFabs(dataA[i] - dataB[i]) < 1e-9)
+				continue;
+		}
+		printf("\t%f", dataA[i] - dataB[i]);
+		if (i != size - 1)
+			printf(",");
+	}
+	printf("\n");
+}
+
+void btMultiBodyConstraintBlock::copyDynamicDataFromOriginalToBlock()
+{
+	copyConstraintDynamicDataToBlock(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints);
+	copyConstraintDynamicDataToBlock(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints);
+	copyConstraintDynamicDataToBlock(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints);
+
+	btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size());
+	btAssert(m_multiBodies.size() == m_deltaVelIndices.size());
+	for (int i = 0; i < m_multiBodies.size(); ++i)
+	{
+		btMultiBody* multiBody = m_multiBodies[i];
+		const int ndof = multiBody->getNumDofs() + 6;
+
+		btMultiBodyJacobianData& originalData = m_internalData.m_data;  // TODO(JS): WRONG !!
+		btAlignedObjectArray<btScalar>& originaDeltaVelocities = originalData.m_deltaVelocities;
+
+		btAlignedObjectArray<btScalar>& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities;
+
+		const int originalIndex = m_originalDeltaVelIndices[i];
+		const int blockIndex = m_deltaVelIndices[i];
+
+		const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex];
+		btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex];
+
+		//		printf("[ original --> block ]\n");
+		//		printf("original: ");
+		//		debugPrint(originalDeltaVelocitiesPtr, ndof);
+		//		printf("block: ");
+		//		debugPrint(blockDeltaVelocitiesPtr, ndof);
+		//		printf("diff: ");
+		//		debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true);
+		//		printf("\n");
+
+		memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar));
+	}
+}
+
+static void copyConstraintDynamicDataFromToOriginal(btAlignedObjectArray<btMultiBodySolverConstraint*>& originalConstraints, const btAlignedObjectArray<btMultiBodySolverConstraint>& blockConstraints)
+{
+	btAssert(originalConstraints.size() == blockConstraints.size());
+	for (int i = 0; i < blockConstraints.size(); ++i)
+	{
+		btMultiBodySolverConstraint& originalConstraint = *originalConstraints[i];
+		const btMultiBodySolverConstraint& blockConstraint = blockConstraints[i];
+
+		originalConstraint.m_appliedImpulse = blockConstraint.m_appliedImpulse;
+		originalConstraint.m_appliedPushImpulse = blockConstraint.m_appliedPushImpulse;
+	}
+}
+
+void btMultiBodyConstraintBlock::copyDynamicDataFromBlockToOriginal()
+{
+	copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyNormalContactConstraintPtrs, m_internalData.m_multiBodyNormalContactConstraints);
+	copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyFrictionContactConstraintPtrs, m_internalData.m_multiBodyFrictionContactConstraints);
+	copyConstraintDynamicDataFromToOriginal(m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, m_internalData.m_multiBodyTorsionalFrictionContactConstraints);
+
+	btAssert(m_multiBodies.size() == m_originalDeltaVelIndices.size());
+	btAssert(m_multiBodies.size() == m_deltaVelIndices.size());
+	for (int i = 0; i < m_multiBodies.size(); ++i)
+	{
+		btMultiBody* multiBody = m_multiBodies[i];
+		const int ndof = multiBody->getNumDofs() + 6;
+
+		btMultiBodyJacobianData& originalData = m_internalData.m_data;
+		btAlignedObjectArray<btScalar>& originaDeltaVelocities = originalData.m_deltaVelocities;
+
+		btAlignedObjectArray<btScalar>& blockDeltaVelocities = m_internalData.m_data.m_deltaVelocities;
+
+		const int originalIndex = m_originalDeltaVelIndices[i];
+		const int blockIndex = m_deltaVelIndices[i];
+
+		btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex];
+		const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex];
+
+		//		printf("[ block --> original ]\n");
+		//		printf("original: ");
+		//		debugPrint(originalDeltaVelocitiesPtr, ndof);
+		//		printf("block: ");
+		//		debugPrint(blockDeltaVelocitiesPtr, ndof);
+		//				printf("diff: ");
+		//				debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true);
+		//		printf("\n");
+
+		memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar));
+	}
+}
+
+btSingleBlockSplittingPolicy::btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver)
+	: m_solver(solver)
+{
+	// Do nothing
+}
+
+btSingleBlockSplittingPolicy::~btSingleBlockSplittingPolicy()
+{
+	// Do nothing
+}
+
+void btSingleBlockSplittingPolicy::split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray<btBlockConstraintSolverConfig>& availableConfigs, btAlignedObjectArray<btMultiBodyConstraintBlock>& blocksOutput)
+{
+	btMultiBodyConstraintBlock newBlock;
+	//	newBlock.m_originalInternalDataBlock = blockInput;
+	//	m_solver->setMultiBodyInternalConstraintData(newBlock.m_originalInternalDataBlock);
+	newBlock.m_solver = m_solver;
+	//	newBlock.m_constraints = blockInput.m_constraints;
+	//	newBlock.m_numConstraints = blockInput.m_numConstraints;
+	//	newBlock.m_multiBodyConstraints = blockInput.m_multiBodyConstraints;
+	//	newBlock.m_numMultiBodyConstraints = blockInput.m_numMultiBodyConstraints;
+
+	blocksOutput.push_back(newBlock);
+}
+
+btDoubleBlockSplittingPolicy::btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver)
+	: m_solver(solver)
+{
+	// Do nothing
+}
+
+btDoubleBlockSplittingPolicy::~btDoubleBlockSplittingPolicy()
+{
+	// Do nothing
+}
+
+template <typename ArrayT>
+void splitContactConstraints(const ArrayT& input, ArrayT& output1, ArrayT& output2)
+{
+	const int totalSize = input.size();
+	const int halfSize = totalSize / 2;
+
+	output1.resize(halfSize);
+	output2.resize(totalSize - halfSize);
+
+	for (int i = 0; i < halfSize; ++i)
+	{
+		output1[i] = input[i];
+	}
+
+	for (int i = halfSize; i < totalSize; ++i)
+	{
+		output2[i - halfSize] = input[i];
+	}
+}
+
+btMultiBodyConstraintBlock initializeConstraintBlock(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& input)
+{
+	btMultiBodyConstraintBlock output;
+
+	// MultiBody
+
+	output.m_internalData.m_multiBodyConstraints = input.m_multiBodyConstraints;
+	output.m_internalData.m_numMultiBodyConstraints = input.m_numMultiBodyConstraints;
+	//output.m_multiBodyConstraintSet.m_data = input.m_multiBodyConstraintSet.m_data;
+
+	btAssert(output.m_internalData.m_multiBodyNormalContactConstraints.size() == 0);
+	btAssert(output.m_internalData.m_multiBodyFrictionContactConstraints.size() == 0);
+
+	return output;
+}
+
+static void setupBlockMultiBodyJacobianData(
+	btMultiBody* multiBody,
+	btAlignedObjectArray<btMultiBody*>& multiBodySet,
+	btAlignedObjectArray<int>& blockDeltaVelIndices,
+	int& blockDeltaVelIndex,
+	int& blockJacobianIndex,
+	btMultiBodyJacobianData& blockJacobianData,
+	btAlignedObjectArray<int>& originalDeltaVelIndices,
+	const int originalDeltaVelIndex,
+	const int originalJacobianIndex,
+	const btMultiBodyJacobianData& originalJacobianData)
+{
+	const int ndof = multiBody->getNumDofs() + 6;
+
+	btAlignedObjectArray<btScalar>& blockJacobians = blockJacobianData.m_jacobians;
+	btAlignedObjectArray<btScalar>& blockDeltaVelocities = blockJacobianData.m_deltaVelocities;
+	btAlignedObjectArray<btScalar>& blockDeltaVelocitiesUnitImpulse = blockJacobianData.m_deltaVelocitiesUnitImpulse;
+
+	const btAlignedObjectArray<btScalar>& originalJacobians = originalJacobianData.m_jacobians;
+	const btAlignedObjectArray<btScalar>& originalDeltaVelocitiesUnitImpulse = originalJacobianData.m_deltaVelocitiesUnitImpulse;
+
+	int indexInBlock = -1;
+	for (int i = 0; i < multiBodySet.size(); ++i)
+	{
+		if (multiBody == multiBodySet[i])
+		{
+			indexInBlock = i;
+			break;
+		}
+	}
+
+	if (indexInBlock == -1)
+	{
+		blockDeltaVelIndex = blockDeltaVelocities.size();
+		blockDeltaVelocities.resize(blockDeltaVelocities.size() + ndof);
+		multiBodySet.push_back(multiBody);
+		originalDeltaVelIndices.push_back(originalDeltaVelIndex);
+		blockDeltaVelIndices.push_back(blockDeltaVelIndex);
+	}
+	else
+	{
+		blockDeltaVelIndex = blockDeltaVelIndices[indexInBlock];
+	}
+
+	blockJacobianIndex = blockJacobians.size();
+	blockJacobians.resize(blockJacobians.size() + ndof);
+	blockDeltaVelocitiesUnitImpulse.resize(blockDeltaVelocitiesUnitImpulse.size() + ndof);
+	btAssert(blockJacobians.size() == blockDeltaVelocitiesUnitImpulse.size());
+
+	btScalar* blockJacobiansRawPtr = &blockJacobians[blockJacobianIndex];
+	const btScalar* originalJacobiansRawPtr = &originalJacobians[originalJacobianIndex];
+	memcpy(blockJacobiansRawPtr, originalJacobiansRawPtr, ndof * sizeof(btScalar));
+
+	btScalar* blockDeltaVelUnitImp = &blockDeltaVelocitiesUnitImpulse[blockJacobianIndex];
+	const btScalar* originalDeltaVelUnitImp = &originalDeltaVelocitiesUnitImpulse[originalJacobianIndex];
+	memcpy(blockDeltaVelUnitImp, originalDeltaVelUnitImp, ndof * sizeof(btScalar));
+
+	btAssert(blockJacobians.size() >= blockDeltaVelocities.size());
+}
+
+static void setupMultiBodyBlockConstraintData(
+	btMultiBodyConstraintBlock& block,
+	btAlignedObjectArray<btMultiBody*>& blockMultiBodySet,
+	btAlignedObjectArray<int>& blockDeltaVelIndices,
+	btMultiBodySolverConstraint& blockContactConstraint,
+	btMultiBodyJacobianData& blockJacobianData,
+	int blockFrictionIndex,
+	btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData,
+	btAlignedObjectArray<int>& originalDeltaVelIndices,
+	const btMultiBodySolverConstraint& originalContactConstraint,
+	const btMultiBodyJacobianData& originalJacobianData)
+{
+	// Copy all the values. Some values will be updated below if necessary.
+	blockContactConstraint = originalContactConstraint;
+	blockContactConstraint.m_frictionIndex = blockFrictionIndex;
+
+	btMultiBody* multiBodyA = blockContactConstraint.m_multiBodyA;
+	if (multiBodyA)
+	{
+		setupBlockMultiBodyJacobianData(
+			multiBodyA,
+			blockMultiBodySet,
+			blockDeltaVelIndices,
+			blockContactConstraint.m_deltaVelAindex,
+			blockContactConstraint.m_jacAindex,
+			blockJacobianData,
+			originalDeltaVelIndices,
+			originalContactConstraint.m_deltaVelAindex,
+			originalContactConstraint.m_jacAindex,
+			originalJacobianData);
+	}
+	else
+	{
+		rigidBodyNotSupported();
+	}
+
+	btMultiBody* multiBodyB = blockContactConstraint.m_multiBodyB;
+	if (multiBodyB)
+	{
+		setupBlockMultiBodyJacobianData(
+			multiBodyB,
+			blockMultiBodySet,
+			blockDeltaVelIndices,
+			blockContactConstraint.m_deltaVelBindex,
+			blockContactConstraint.m_jacBindex,
+			blockJacobianData,
+			originalDeltaVelIndices,
+			originalContactConstraint.m_deltaVelBindex,
+			originalContactConstraint.m_jacBindex,
+			originalJacobianData);
+	}
+	else
+	{
+		rigidBodyNotSupported();
+	}
+}
+
+void btDoubleBlockSplittingPolicy::split(
+	btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData,
+	const btAlignedObjectArray<btBlockConstraintSolverConfig>& availableConfigs,
+	btAlignedObjectArray<btMultiBodyConstraintBlock>& subBlocks)
+{
+	btMultiBodyConstraintBlock constraintBlock1 = initializeConstraintBlock(originalInternalData);
+	btMultiBodyConstraintBlock constraintBlock2 = initializeConstraintBlock(originalInternalData);
+	btMultiBodyConstraintBlock constraintBlock3 = initializeConstraintBlock(originalInternalData);
+
+	constraintBlock1.m_solver = m_solver;
+	constraintBlock2.m_solver = m_solver;
+	constraintBlock3.m_solver = m_solver;
+
+	btDantzigSolver* mlcp = new btDantzigSolver();
+	btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp);
+//	constraintBlock2.m_solver = sol;
+
+	const int totalMultiBodyContactConstraintSize = originalInternalData.m_multiBodyNormalContactConstraints.size();
+	const int halfMultiBodyContactConstraintSize = totalMultiBodyContactConstraintSize / 2;
+
+	for (int i = 0; i < halfMultiBodyContactConstraintSize; ++i)
+	{
+		copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i);
+	}
+
+	for (int i = halfMultiBodyContactConstraintSize; i < totalMultiBodyContactConstraintSize; ++i)
+	{
+		copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i);
+	}
+
+	const int totalMultiBodyNonContactConstraintSize = originalInternalData.m_multiBodyNonContactConstraints.size();
+
+	for (int i = 0; i < totalMultiBodyNonContactConstraintSize; ++i)
+	{
+//		copyMultiBodyNonContactConstraint(constraintBlock3, originalInternalData, i);
+	}
+
+
+//	for (int i = 0; i < totalMultiBodyContactConstraintSize; ++i)
+//	{
+//		if (strcmp(originalInternalData.m_multiBodyNormalContactConstraints[i].m_multiBodyA->getBaseName(), "group1") == 0)
+//			copyMultiBodyContactConstraint(constraintBlock1, originalInternalData, i);
+//		else
+//			copyMultiBodyContactConstraint(constraintBlock2, originalInternalData, i);
+//	}
+
+	subBlocks.push_back(constraintBlock1);
+	subBlocks.push_back(constraintBlock2);
+	subBlocks.push_back(constraintBlock3);
+}
+
+btMultiBodyBlockSplittingPolicy::~btMultiBodyBlockSplittingPolicy()
+{
+	// Do nothing
+}
+
+void btMultiBodyBlockSplittingPolicy::copyMultiBodyNonContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNonContactConstraintIndex)
+{
+	btAlignedObjectArray<btMultiBodySolverConstraint>& originalNonContactConstraints = originalInternalData.m_multiBodyNonContactConstraints;
+	const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data;
+
+	btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& blockNonContactConstraints = blockInternalData.m_multiBodyNonContactConstraints;
+
+	btAlignedObjectArray<btMultiBodySolverConstraint*>& blockOriginalNonContactConstraintPtrs = block.m_originalMultiBodyNonContactConstraintPtrs;
+
+	btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data;
+
+	btAlignedObjectArray<btMultiBody*>& blockMultiBodySet = block.m_multiBodies;
+
+	const int blockFrictionIndex = blockNonContactConstraints.size();
+
+	btMultiBodySolverConstraint& originalNonContactConstraint = originalNonContactConstraints[originalNonContactConstraintIndex];
+
+	btMultiBodySolverConstraint& blockNonContactConstraint = blockNonContactConstraints.expandNonInitializing();
+	blockOriginalNonContactConstraintPtrs.push_back(&originalNonContactConstraint);
+
+	setupMultiBodyBlockConstraintData(
+		block,
+		blockMultiBodySet,
+		block.m_deltaVelIndices,
+		blockNonContactConstraint,
+		blockJacobianData,
+		blockFrictionIndex,
+		originalInternalData,
+		block.m_originalDeltaVelIndices,
+		originalNonContactConstraint,
+		originalJacobianData);
+}
+
+void btMultiBodyBlockSplittingPolicy::copyMultiBodyContactConstraint(btMultiBodyConstraintBlock& block, btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData, int originalNormalContactConstraintIndex)
+{
+	btAlignedObjectArray<btMultiBodySolverConstraint>& originalNormalContactConstraints = originalInternalData.m_multiBodyNormalContactConstraints;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& originalFrictionContactConstraints = originalInternalData.m_multiBodyFrictionContactConstraints;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& originalTortionalFrictionContactConstraints = originalInternalData.m_multiBodyTorsionalFrictionContactConstraints;
+	const btMultiBodyJacobianData& originalJacobianData = originalInternalData.m_data;
+
+	btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInternalData = block.m_internalData;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& blockNormalContactConstraints = blockInternalData.m_multiBodyNormalContactConstraints;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& blockFrictionContactConstraints = blockInternalData.m_multiBodyFrictionContactConstraints;
+	btAlignedObjectArray<btMultiBodySolverConstraint>& blockTortionalFrictionContactConstraints = blockInternalData.m_multiBodyTorsionalFrictionContactConstraints;
+
+	btAlignedObjectArray<btMultiBodySolverConstraint*>& blockOriginalNormalContactConstraintPtrs = block.m_originalMultiBodyNormalContactConstraintPtrs;
+	btAlignedObjectArray<btMultiBodySolverConstraint*>& blockOriginalFrictionContactConstraintPtrs = block.m_originalMultiBodyFrictionContactConstraintPtrs;
+	btAlignedObjectArray<btMultiBodySolverConstraint*>& blockOriginalTorsionalFrictionContactConstraintPtrs = block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs;
+
+	btMultiBodyJacobianData& blockJacobianData = block.m_internalData.m_data;
+
+	const int numFrictionPerContact = originalNormalContactConstraints.size() == originalFrictionContactConstraints.size() ? 1 : 2;
+
+	btAlignedObjectArray<btMultiBody*>& blockMultiBodySet = block.m_multiBodies;
+
+	const int blockFrictionIndex = blockNormalContactConstraints.size();
+
+	//-- 1. Normal contact
+
+	btMultiBodySolverConstraint& originalNormalContactConstraint = originalNormalContactConstraints[originalNormalContactConstraintIndex];
+
+	btMultiBodySolverConstraint& blockNormalContactConstraint = blockNormalContactConstraints.expandNonInitializing();
+	blockOriginalNormalContactConstraintPtrs.push_back(&originalNormalContactConstraint);
+
+	setupMultiBodyBlockConstraintData(
+		block,
+		blockMultiBodySet,
+		block.m_deltaVelIndices,
+		blockNormalContactConstraint,
+		blockJacobianData,
+		blockFrictionIndex,
+		originalInternalData,
+		block.m_originalDeltaVelIndices,
+		originalNormalContactConstraint,
+		originalJacobianData);
+
+	//-- 2. Friction contacts
+
+	btAssert(originalFrictionContactConstraints.size() != 0);
+	const int originalFrictionContactConstraintIndex1 = originalNormalContactConstraintIndex * numFrictionPerContact;
+	btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex1];
+
+	blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint);
+	btMultiBodySolverConstraint& blockFrictionContactConstraint1 = blockFrictionContactConstraints.expandNonInitializing();
+	setupMultiBodyBlockConstraintData(
+		block,
+		blockMultiBodySet,
+		block.m_deltaVelIndices,
+		blockFrictionContactConstraint1,
+		blockJacobianData,
+		blockFrictionIndex,
+		originalInternalData,
+		block.m_originalDeltaVelIndices,
+		originalFrictionContactConstraint,
+		originalJacobianData);
+
+	if (numFrictionPerContact == 2)
+	{
+		const int originalFrictionContactConstraintIndex2 = originalFrictionContactConstraintIndex1 + 1;
+		btMultiBodySolverConstraint& originalFrictionContactConstraint = originalFrictionContactConstraints[originalFrictionContactConstraintIndex2];
+
+		blockOriginalFrictionContactConstraintPtrs.push_back(&originalFrictionContactConstraint);
+		btMultiBodySolverConstraint& blockFrictionContactConstraint2 = blockFrictionContactConstraints.expandNonInitializing();
+		setupMultiBodyBlockConstraintData(
+			block,
+			blockMultiBodySet,
+			block.m_deltaVelIndices,
+			blockFrictionContactConstraint2,
+			blockJacobianData,
+			blockFrictionIndex,
+			originalInternalData,
+			block.m_originalDeltaVelIndices,
+			originalFrictionContactConstraint,
+			originalJacobianData);
+	}
+
+	// TODO(JS): Torsional friction contact constraints
+}
+
+btMultiBodyBlockConstraintSolver::btMultiBodyBlockConstraintSolver()
+{
+	// Do nothing
+}
+
+btMultiBodyBlockConstraintSolver::~btMultiBodyBlockConstraintSolver()
+{
+	// Do nothing
+}
+
+btScalar btMultiBodyBlockConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
+{
+	return btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+}
+
+void btMultiBodyBlockConstraintSolver::solveMultiBodyGroup(
+	btCollisionObject** bodies,
+	int numBodies,
+	btPersistentManifold** manifold,
+	int numManifolds,
+	btTypedConstraint** constraints,
+	int numConstraints,
+	btMultiBodyConstraint** multiBodyConstraints,
+	int numMultiBodyConstraints,
+	const btContactSolverInfo& info,
+	btIDebugDraw* debugDrawer,
+	btDispatcher* /*dispatcher*/)
+{
+	m_tmpMultiBodyConstraints = multiBodyConstraints;
+	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+
+	// 1. Convert rigid bodies/multibodies, joints, contacts into constraints.
+	solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+	// 2. Split constraints into constraint blocks
+	btMultiBodyInternalConstraintData originalInternalDataCopy;
+	getMultiBodyInternalConstraintData(originalInternalDataCopy);
+
+	btAlignedObjectArray<btBlockConstraintSolverConfig> configs;
+	// TODO(JS): This is just for test
+	//m_splittingPolicy = new btSingleBlockSplittingPolicy(new btMultiBodyConstraintSolver());
+
+//	btDantzigSolver* mlcp = new btDantzigSolver();
+//	btMultiBodyMLCPConstraintSolver* sol = new btMultiBodyMLCPConstraintSolver(mlcp);
+//	m_splittingPolicy = new btDoubleBlockSplittingPolicy(sol);
+
+	m_splittingPolicy = new btDoubleBlockSplittingPolicy(new btMultiBodyConstraintSolver());
+
+    btAssert(m_splittingPolicy);
+	m_blocks.resize(0);
+	m_splittingPolicy->split(originalInternalDataCopy, configs, m_blocks);
+
+//	for (int i = 0; i < m_blocks.size(); ++i)
+//	{
+//		btMultiBodyConstraintBlock& block = m_blocks[i];
+//		btMultiBodyConstraintSolver* solver = block.m_solver;
+//		btAssert(solver);
+
+//		solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+//		copyDynamicDataFromOriginalToBlock(block);
+////			block.copyDynamicDataFromOriginalToBlock();
+//		solver->setMultiBodyInternalConstraintData(block.m_internalData, false);
+//		solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+//	}
+
+	// 3. Gauss-Seidel iterations
+
+	const int maxIterations = m_maxOverrideNumSolverIterations > info.m_numIterations ? m_maxOverrideNumSolverIterations : info.m_numIterations;
+
+	m_leastSquaresResidual = 0;
+
+	for (int iteration = 0; iteration < maxIterations; ++iteration)
+	{
+		for (int i = 0; i < m_blocks.size(); ++i)
+		{
+			// Change the sweep direction every iteration
+			const int index = iteration & 1 ? m_blocks.size() - 1 - i : i;
+
+			btMultiBodyConstraintBlock& block = m_blocks[index];
+			btMultiBodyConstraintSolver* solver = block.m_solver;
+			btAssert(solver);
+
+			solver->solveGroupConvertConstraintPrestep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+			copyDynamicDataFromOriginalToBlock(block);
+			solver->setMultiBodyInternalConstraintData(block.m_internalData, false);
+			solver->solveGroupConvertConstraintPoststep(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+			// TODO(JS): Add split impulse
+			btScalar newSquaredResidual = solver->solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+			m_leastSquaresResidual = btMax(m_leastSquaresResidual, newSquaredResidual);
+
+			solver->solveGroupConvertBackPrestep(bodies, numBodies, info);
+			solver->solveGroupConvertBack(bodies, numBodies, info);
+			solver->getMultiBodyInternalConstraintData(block.m_internalData, false);
+			copyDynamicDataFromBlockToOriginal(block);
+			solver->solveGroupConvertBackPoststep(bodies, numBodies, info);
+		}
+
+		if (m_leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+		{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+			printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+			break;
+		}
+	}
+
+//	solveGroupConvertBackPrestep(bodies, numBodies, info);
+//	solveGroupConvertBack(bodies, numBodies, info);
+//	getMultiBodyInternalConstraintData(block.m_internalData, false);
+//	copyDynamicDataFromBlockToOriginal(block);
+//	solveGroupConvertBackPoststep(bodies, numBodies, info);
+
+	solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+
+	m_tmpMultiBodyConstraints = 0;
+	m_tmpNumMultiBodyConstraints = 0;
+}
+
+void btMultiBodyBlockConstraintSolver::setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy)
+{
+	m_splittingPolicy = policy;
+}
+
+void btMultiBodyBlockConstraintSolver::copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block)
+{
+	copyConstraintDynamicDataToBlock(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints);
+	copyConstraintDynamicDataToBlock(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints);
+	copyConstraintDynamicDataToBlock(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints);
+
+	btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size());
+	btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size());
+	for (int i = 0; i < block.m_multiBodies.size(); ++i)
+	{
+		btMultiBody* multiBody = block.m_multiBodies[i];
+		const int ndof = multiBody->getNumDofs() + 6;
+
+		btMultiBodyJacobianData& originalData = m_data;  // TODO(JS): WRONG !!
+		btAlignedObjectArray<btScalar>& originaDeltaVelocities = originalData.m_deltaVelocities;
+
+		btAlignedObjectArray<btScalar>& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities;
+
+		const int originalIndex = block.m_originalDeltaVelIndices[i];
+		const int blockIndex = block.m_deltaVelIndices[i];
+
+		const btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex];
+		btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex];
+
+		//		printf("[ original --> block ]\n");
+		//		printf("original: ");
+		//		debugPrint(originalDeltaVelocitiesPtr, ndof);
+		//		printf("block: ");
+		//		debugPrint(blockDeltaVelocitiesPtr, ndof);
+		//		printf("diff: ");
+		//		debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true);
+		//		printf("\n");
+
+		memcpy(blockDeltaVelocitiesPtr, originalDeltaVelocitiesPtr, ndof * sizeof(btScalar));
+	}
+}
+
+void btMultiBodyBlockConstraintSolver::copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block)
+{
+	copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyNormalContactConstraintPtrs, block.m_internalData.m_multiBodyNormalContactConstraints);
+	copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyFrictionContactConstraints);
+	copyConstraintDynamicDataFromToOriginal(block.m_originalMultiBodyTorsionalFrictionContactConstraintPtrs, block.m_internalData.m_multiBodyTorsionalFrictionContactConstraints);
+
+	btAssert(block.m_multiBodies.size() == block.m_originalDeltaVelIndices.size());
+	btAssert(block.m_multiBodies.size() == block.m_deltaVelIndices.size());
+	for (int i = 0; i < block.m_multiBodies.size(); ++i)
+	{
+		btMultiBody* multiBody = block.m_multiBodies[i];
+		const int ndof = multiBody->getNumDofs() + 6;
+
+		btMultiBodyJacobianData& originalData = m_data;
+		btAlignedObjectArray<btScalar>& originaDeltaVelocities = originalData.m_deltaVelocities;
+
+		btAlignedObjectArray<btScalar>& blockDeltaVelocities = block.m_internalData.m_data.m_deltaVelocities;
+
+		const int originalIndex = block.m_originalDeltaVelIndices[i];
+		const int blockIndex = block.m_deltaVelIndices[i];
+
+		btScalar* originalDeltaVelocitiesPtr = &originaDeltaVelocities[originalIndex];
+		const btScalar* blockDeltaVelocitiesPtr = &blockDeltaVelocities[blockIndex];
+
+		//		printf("[ block --> original ]\n");
+		//		printf("original: ");
+		//		debugPrint(originalDeltaVelocitiesPtr, ndof);
+		//		printf("block: ");
+		//		debugPrint(blockDeltaVelocitiesPtr, ndof);
+		//				printf("diff: ");
+		//				debugPrintDiff(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof, true);
+		//		printf("\n");
+
+		memcpy(originalDeltaVelocitiesPtr, blockDeltaVelocitiesPtr, ndof * sizeof(btScalar));
+	}
+}
+
+int btMultiBodyBlockConstraintSolver::addConfig(btBlockConstraintSolverConfig& config)
+{
+	m_configs.push_back(config);
+	return m_configs.size();
+}
+
+int btMultiBodyBlockConstraintSolver::getNumConfigs() const
+{
+	return m_configs.size();
+}
+
+void btMultiBodyBlockConstraintSolver::removeConfig(int configIndex)
+{
+	m_configs.removeAtIndex(configIndex);
+}

src/BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h

@@ -0,0 +1,186 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H
+#define BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+struct btBlockConstraintSolverConfig
+{
+	int m_solverType;  //SI or MLCP Dantzig
+	//to be decided: full or subset of
+
+	btContactSolverInfo m_info;
+};
+
+struct btMultiBodyConstraintBlock
+{
+	/// \{ \name Multi-body Data
+
+	btAlignedObjectArray<btMultiBody*> m_multiBodies;
+	btAlignedObjectArray<int> m_originalDeltaVelIndices;
+	btAlignedObjectArray<int> m_deltaVelIndices;
+
+//	btMultiBodyJacobianData* m_originalDataPtr;
+
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_originalMultiBodyNonContactConstraintPtrs;
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_originalMultiBodyNormalContactConstraintPtrs;
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_originalMultiBodyFrictionContactConstraintPtrs;
+	btAlignedObjectArray<btMultiBodySolverConstraint*> m_originalMultiBodyTorsionalFrictionContactConstraintPtrs;
+
+	/// \}
+
+	btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData m_internalData;
+
+	/// Constraint solver
+	btMultiBodyConstraintSolver* m_solver;
+
+	bool m_ownSolver = false;
+	// TODO(JS): If this is true, then don't copy all the constraint data, but
+	// only dynamic data
+	// TODO(JS): not utilized yet
+
+	/// Index to constraint solver configuration
+	int m_constraintConfigId;
+
+	/// Default constructor
+	btMultiBodyConstraintBlock();
+
+	/// Constructor
+	btMultiBodyConstraintBlock(
+		btTypedConstraint** m_constraints,
+		int m_numConstraints,
+		btAlignedObjectArray<btSolverBody>* m_solverBodyPool,
+		btConstraintArray& m_originalNonContactConstraints,
+		btConstraintArray& m_originalNormalContactConstraints,
+		btConstraintArray& m_originalFrictionContactConstraints,
+		btConstraintArray& m_orginalRollingFrictionContactConstraints,
+		btMultiBodyConstraint** m_multiBodyConstraints,
+		int m_numMultiBodyConstraints,
+		btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyNonContactConstraints,
+		btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyNormalContactConstraints,
+		btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyFrictionContactConstraints,
+		btAlignedObjectArray<btMultiBodySolverConstraint>& m_multiBodyTorsionalFrictionContactConstraints,
+		btMultiBodyJacobianData* m_data);
+
+	void copyDynamicDataFromOriginalToBlock();
+	void copyDynamicDataFromBlockToOriginal();
+};
+
+class btMultiBodyBlockSplittingPolicy
+{
+public:
+	/// Destructor
+	virtual ~btMultiBodyBlockSplittingPolicy();
+
+	/// Splits a set of constraints into multiple subsets.
+	///
+	/// \param[in] blockInput
+	/// \param[in] availableConfigs
+	/// \param[in,out] blocksOutput The splitted blocks. This function adds blocks without clearning the array
+	/// beforehand. Clearning the array is the caller's responsibility.
+	virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray<btBlockConstraintSolverConfig>& availableConfigs, btAlignedObjectArray<btMultiBodyConstraintBlock>& blocksOutput) = 0;
+
+protected:
+	void copyMultiBodyNonContactConstraint(
+		btMultiBodyConstraintBlock& block,
+		btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData,
+		int originalNonContactConstraintIndex);
+
+	void copyMultiBodyContactConstraint(
+		btMultiBodyConstraintBlock& block,
+		btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& originalInternalData,
+		int originalNormalContactConstraintIndex);
+};
+
+class btSingleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy
+{
+protected:
+	btMultiBodyConstraintSolver* m_solver;
+
+public:
+	/// Constructor
+	btSingleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver);
+
+	/// Destructor
+	virtual ~btSingleBlockSplittingPolicy();
+
+	// Documentation inherited
+	virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray<btBlockConstraintSolverConfig>& availableConfigs, btAlignedObjectArray<btMultiBodyConstraintBlock>& blocksOutput);
+};
+
+class btDoubleBlockSplittingPolicy : public btMultiBodyBlockSplittingPolicy
+{
+protected:
+	btMultiBodyConstraintSolver* m_solver;
+
+public:
+	/// Constructor
+	btDoubleBlockSplittingPolicy(btMultiBodyConstraintSolver* solver);
+
+	/// Destructor
+	virtual ~btDoubleBlockSplittingPolicy();
+
+	// Documentation inherited
+	virtual void split(btMultiBodyConstraintSolver::btMultiBodyInternalConstraintData& blockInput, const btAlignedObjectArray<btBlockConstraintSolverConfig>& availableConfigs, btAlignedObjectArray<btMultiBodyConstraintBlock>& blocksOutput);
+};
+
+class btMultiBodyBlockConstraintSolver : public btMultiBodyConstraintSolver
+{
+protected:
+	/// Splitting policy. Assumed not a null.
+	btMultiBodyBlockSplittingPolicy* m_splittingPolicy;
+
+	/// Array of constraint configurations for constraint blocks.
+	btAlignedObjectArray<btBlockConstraintSolverConfig> m_configs;
+
+	/// Array of constraint blocks.
+	btAlignedObjectArray<btMultiBodyConstraintBlock> m_blocks;
+
+public:
+	/// Constructor
+	btMultiBodyBlockConstraintSolver();
+
+	/// Destructor
+	virtual ~btMultiBodyBlockConstraintSolver();
+
+	virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
+
+
+protected:
+	// Documentation inherited.
+	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);
+
+	/// Sets the splitting policy.
+	virtual void setSplittingPolicy(btMultiBodyBlockSplittingPolicy* policy);
+
+	void copyDynamicDataFromOriginalToBlock(btMultiBodyConstraintBlock& block);
+	void copyDynamicDataFromBlockToOriginal(btMultiBodyConstraintBlock& block);
+
+	/// Adds a constraint block configuration and returns the total number of configurations added to this solver.
+	virtual int addConfig(btBlockConstraintSolverConfig& config);
+
+	/// Returns the number of configurations added to this solver.
+	virtual int getNumConfigs() const;
+
+	/// Removes an configuration at \c configIndex
+	///
+	/// \param[in] configIndex The configuration indext in the range of [0, numConfigs). Passing out of the range is an
+	/// undefined behavior.
+	virtual void removeConfig(int configIndex);
+};
+
+#endif  // BT_MULTIBODY_BLOCK_CONSTRAINT_SOLVER_H

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -158,7 +158,17 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	return leastSquaredResidual;
 }
 
+btScalar btMultiBodyConstraintSolver::solveSingleIterationNew(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	return solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+}
+
 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
+}
+
+btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPrestep(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
@@ -169,7 +179,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
 	m_data.m_deltaVelocities.resize(0);
 
-	for (int i = 0; i < numBodies; i++)
+	for (int i = 0; i < numBodies; ++i)
 	{
 		const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
 		if (fcA)
@@ -178,11 +188,16 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 		}
 	}
 
-	btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+	btScalar val = btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPrestep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
 	return val;
 }
 
+btScalar btMultiBodyConstraintSolver::solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	return btSequentialImpulseConstraintSolver::solveGroupConvertConstraintPoststep(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+}
+
 void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
     for (int i = 0; i < ndof; ++i) 
@@ -191,6 +206,7 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
 
 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
 {
+
 	btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
 	btScalar deltaVelADotn = 0;
 	btScalar deltaVelBDotn = 0;
@@ -223,6 +239,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
 
+
 	deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
 	deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
@@ -254,6 +271,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 	else if (c.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
+
 	}
 	if (c.m_multiBodyB)
 	{
@@ -272,6 +290,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 	return deltaVel;
 }
 
+
 btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1,const btMultiBodySolverConstraint& cB)
 {
 	int ndofA=0;
@@ -289,8 +308,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 			ndofA  = cB.m_multiBodyA->getNumDofs() + 6;
 			for (int i = 0; i < ndofA; ++i) 
 				deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex+i];
-		}
+		} else if(cB.m_solverBodyIdA >= 0)
-		else if (cB.m_solverBodyIdA >= 0)
 		{
 			bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
 			deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -301,13 +319,13 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 			ndofB  = cB.m_multiBodyB->getNumDofs() + 6;
 			for (int i = 0; i < ndofB; ++i) 
 				deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex+i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex+i];
-		}
+		} else if(cB.m_solverBodyIdB >= 0)
-		else if (cB.m_solverBodyIdB >= 0)
 		{
 			bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
 			deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 		}
 
+	
 		deltaImpulseB	-=	deltaVelADotn*cB.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
 		deltaImpulseB	-=	deltaVelBDotn*cB.m_jacDiagABInv;
 		sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
@@ -326,8 +344,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 				ndofA  = cA.m_multiBodyA->getNumDofs() + 6;
 				for (int i = 0; i < ndofA; ++i) 
 					deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex+i];
-			}
+			} else if(cA.m_solverBodyIdA >= 0)
-			else if (cA.m_solverBodyIdA >= 0)
 			{
 				bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
 				deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) 	+ cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
@@ -338,13 +355,13 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 				ndofB  = cA.m_multiBodyB->getNumDofs() + 6;
 				for (int i = 0; i < ndofB; ++i) 
 					deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex+i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex+i];
-			}
+			} else if(cA.m_solverBodyIdB >= 0)
-			else if (cA.m_solverBodyIdB >= 0)
 			{
 				bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
 				deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 			}
 
+	
 			deltaImpulseA	-=	deltaVelADotn*cA.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
 			deltaImpulseA	-=	deltaVelBDotn*cA.m_jacDiagABInv;
 			sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
@@ -357,6 +374,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 		btScalar sumAclipped = btFabs(cA.m_lowerLimit*btSin(angle));
 		btScalar sumBclipped = btFabs(cB.m_lowerLimit*btCos(angle));
 
+		
 		if (sumA < -sumAclipped)
 		{
 			deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
@@ -405,10 +423,10 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex],deltaImpulseA);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	}
+	} else if(cA.m_solverBodyIdA >= 0)
-	else if (cA.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(cA.m_contactNormal1*bodyA->internalGetInvMass(),cA.m_angularComponentA,deltaImpulseA);
+
 	}
 	if (cA.m_multiBodyB)
 	{
@@ -418,8 +436,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex],deltaImpulseA);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	}
+	} else if(cA.m_solverBodyIdB >= 0)
-	else if (cA.m_solverBodyIdB >= 0)
 	{
 		bodyB->internalApplyImpulse(cA.m_contactNormal2*bodyB->internalGetInvMass(),cA.m_angularComponentB,deltaImpulseA);
 	}
@@ -432,8 +449,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex],deltaImpulseB);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	}
+	} else if(cB.m_solverBodyIdA >= 0)
-	else if (cB.m_solverBodyIdA >= 0)
 	{
 		bodyA->internalApplyImpulse(cB.m_contactNormal1*bodyA->internalGetInvMass(),cB.m_angularComponentA,deltaImpulseB);
 	}
@@ -445,8 +461,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 		//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 		cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex],deltaImpulseB);
 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
-	}
+	} else if(cB.m_solverBodyIdB >= 0)
-	else if (cB.m_solverBodyIdB >= 0)
 	{
 		bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB);
 	}
@@ -455,12 +470,16 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
     return deltaVel;
 }
 
+
+
+
 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
 																 const btVector3& contactNormal,
 																 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 																 btScalar& relaxation,
 																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
+			
 	BT_PROFILE("setupMultiBodyContactConstraint");
 	btVector3 rel_pos1;
 	btVector3 rel_pos2;
@@ -495,8 +514,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	{
 		cfm = infoGlobal.m_frictionCFM;
 		erp = infoGlobal.m_frictionERP;
-	}
+	} else
-	else
 	{
 		cfm = infoGlobal.m_globalCfm;
 		erp = infoGlobal.m_erp2;
@@ -507,8 +525,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				cfm  = cp.m_contactCFM;
 			if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
 				erp = cp.m_contactERP;                
-		}
+		} else
-		else
 		{
 			if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
 			{
@@ -530,8 +547,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (solverConstraint.m_linkA<0)
 		{
 			rel_pos1 = pos1 - multiBodyA->getBasePos();
-		}
+		} else
-		else
 		{
 			rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
 		}
@@ -544,8 +560,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
 			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-		}
+		} else
-		else
 		{
 			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
 		}
@@ -563,8 +578,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = contactNormal;
-	}
+	} else
-	else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -572,13 +586,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
 	}
 
+	
+
 	if (multiBodyB)
 	{
 		if (solverConstraint.m_linkB<0)
 		{
 			rel_pos2 = pos2 - multiBodyB->getBasePos();
-		}
+		} else
-		else
 		{
 			rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
 		}
@@ -605,8 +620,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 		solverConstraint.m_contactNormal2 = -contactNormal;
-	}
+	
-	else
+	} else
 	{
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
 		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
@@ -616,6 +631,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	}
 
 	{
+						
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
@@ -635,8 +651,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				btScalar l =lambdaA[i];
 				denom0 += j*l;
 			}
-		}
+		} else
-		else
 		{
 			if (rb0)
 			{
@@ -655,8 +670,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				btScalar l =lambdaB[i];
 				denom1 += j*l;
 			}
-		}
+
-		else
+		} else
 		{
 			if (rb1)
 			{
@@ -665,27 +680,31 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			}
 		}
 
+		 
+
 		 btScalar d = denom0+denom1+cfm;
 		 if (d>SIMD_EPSILON)
 		 {
 			solverConstraint.m_jacDiagABInv = relaxation/(d);
-		}
+		 } else
-		else
 		 {
 			//disable the constraint row to handle singularity/redundant constraint
 			solverConstraint.m_jacDiagABInv  = 0.f;
 		 }
+		
 	}
 
+	
 	//compute rhs and remaining solverConstraint fields
 
+	
+
 	btScalar restitution = 0.f;
     btScalar distance = 0;
     if (!isFriction)
     {
         distance = cp.getDistance()+infoGlobal.m_linearSlop;
-	}
+    } else
-	else
     {
         if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
         {
@@ -693,10 +712,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
         }
     }
   
+    
 	btScalar rel_vel = 0.f;
 	int ndofA  = 0;
 	int ndofB  = 0;
 	{
+
 		btVector3 vel1,vel2;
 		if (multiBodyA)
 		{
@@ -704,8 +725,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		}
+		} else
-		else
 		{
 			if (rb0)
 			{
@@ -721,8 +741,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-		}
+
-		else
+		} else
 		{
 			if (rb1)
 			{
@@ -745,6 +765,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 	}
 
+
 	///warm starting (or zero if disabled)
 	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
 	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
@@ -760,8 +781,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
 				
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-			}
+			} else
-			else
 			{
 				if (rb0)
 					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
@@ -772,15 +792,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 				multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-			}
+			} else
-			else
 			{
 				if (rb1)
 					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
 			}
 		}
-	}
+	} else
-	else
 	{
 		solverConstraint.m_appliedImpulse = 0.f;
 	}
@@ -788,20 +806,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	solverConstraint.m_appliedPushImpulse = 0.f;
 
 	{
+
 		btScalar positionalError = 0.f;
 		btScalar velocityError = restitution - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
 		if (isFriction)
 		{
 			positionalError = -distance * erp/infoGlobal.m_timeStep;
-		}
+		} else
-		else
 		{
     			if (distance>0)
 			{
 				positionalError = 0;
 				velocityError -= distance / infoGlobal.m_timeStep;
-			}
+
-			else
+			} else
 			{
 				positionalError = -distance * erp/infoGlobal.m_timeStep;
 			}
@@ -817,6 +835,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 				//combine position and velocity into rhs
 				solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 				solverConstraint.m_rhsPenetration = 0.f;
+
 			}
 		/*else
 			{
@@ -837,7 +856,11 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 
 		solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
+		
+
+
 	}
+
 }
 
 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
@@ -848,6 +871,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
                                                                   btScalar& relaxation,
                                                                   bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
+    
     BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
     btVector3 rel_pos1;
     btVector3 rel_pos2;
@@ -873,13 +897,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
     
    // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
     
+    
     if (multiBodyA)
     {
         if (solverConstraint.m_linkA<0)
         {
             rel_pos1 = pos1 - multiBodyA->getBasePos();
-		}
+        } else
-		else
         {
             rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
         }
@@ -892,8 +916,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
             solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
             multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
             m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
-		}
+        } else
-		else
         {
             btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
         }
@@ -911,8 +934,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         btVector3 torqueAxis0 = -constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
         solverConstraint.m_contactNormal1 = btVector3(0,0,0);
-	}
+    } else
-	else
     {
         btVector3 torqueAxis0 = -constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
@@ -920,13 +942,14 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
     }
     
+    
+    
     if (multiBodyB)
     {
         if (solverConstraint.m_linkB<0)
         {
             rel_pos2 = pos2 - multiBodyB->getBasePos();
-		}
+        } else
-		else
         {
             rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
         }
@@ -953,8 +976,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         btVector3 torqueAxis1 = constraintNormal;
         solverConstraint.m_relpos2CrossNormal = torqueAxis1;
         solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
-	}
+        
-	else
+    } else
     {
         btVector3 torqueAxis1 = constraintNormal;
         solverConstraint.m_relpos2CrossNormal = torqueAxis1;
@@ -964,6 +987,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
     }
     
     {
+        
         btScalar denom0 = 0.f;
         btScalar denom1 = 0.f;
         btScalar* jacB = 0;
@@ -982,8 +1006,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
                 btScalar l =lambdaA[i];
                 denom0 += j*l;
             }
-		}
+        } else
-		else
         {
             if (rb0)
             {
@@ -1002,8 +1025,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
                 btScalar l =lambdaB[i];
                 denom1 += j*l;
             }
-		}
+            
-		else
+        } else
         {
             if (rb1)
             {
@@ -1012,20 +1035,25 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
             }
         }
         
+        
+        
         btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
         if (d>SIMD_EPSILON)
         {
             solverConstraint.m_jacDiagABInv = relaxation/(d);
-		}
+        } else
-		else
         {
             //disable the constraint row to handle singularity/redundant constraint
             solverConstraint.m_jacDiagABInv  = 0.f;
         }
+        
     }
     
+    
     //compute rhs and remaining solverConstraint fields
     
+    
+    
     btScalar restitution = 0.f;
     btScalar penetration = isFriction? 0 : cp.getDistance();
     
@@ -1033,6 +1061,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
     int ndofA  = 0;
     int ndofB  = 0;
     {
+        
         btVector3 vel1,vel2;
         if (multiBodyA)
         {
@@ -1040,8 +1069,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
             btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
             for (int i = 0; i < ndofA ; ++i)
                 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		}
+        } else
-		else
 		{
 			if (rb0)
 			{
@@ -1055,8 +1083,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
             btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
             for (int i = 0; i < ndofB ; ++i)
                 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-		}
+            
-		else
+        } else
         {
 			if (rb1)
 			{
@@ -1077,12 +1105,16 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         }
     }
     
+    
     solverConstraint.m_appliedImpulse = 0.f;
     solverConstraint.m_appliedPushImpulse = 0.f;
     
     {
+        
         btScalar velocityError = 0 - rel_vel;// * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
         
+       
+        
         btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
         
         solverConstraint.m_rhs = velocityImpulse;
@@ -1091,7 +1123,11 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
         solverConstraint.m_upperLimit = solverConstraint.m_friction;
         
         solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
+        
+        
+        
     }
+    
 }
 
 btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
@@ -1188,6 +1224,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 //	btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
 //	btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
 
+
 	///avoid collision response between two static objects
 //	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
 	//	return;
@@ -1197,10 +1234,12 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
     
 	for (int j=0;j<manifold->getNumContacts();j++)
 	{
+
 		btManifoldPoint& cp = manifold->getContactPoint(j);
 
 		if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 		{
+		
 			btScalar relaxation;
 
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
@@ -1262,6 +1301,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
                 }
                 if (cp.m_combinedRollingFriction>0)
                 {
+
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
@@ -1299,10 +1339,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				} else
 				*/
 				{
+					
+
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
                     
+
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
@@ -1315,8 +1358,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 						cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
-			}
+
-			else
+			} else
 			{
 				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM);
 
@@ -1329,7 +1372,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				solverConstraint.m_appliedPushImpulse = 0.f;
 			}
 		
+
 #endif //ENABLE_FRICTION
+
 		}
 	}
 }
@@ -1347,8 +1392,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 		{
 			//the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
 			convertContact(manifold,infoGlobal);
-		}
+		} else
-		else
 		{
 			convertMultiBodyContact(manifold,infoGlobal);
 		}
@@ -1356,6 +1400,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 	//also convert the multibody constraints, if any
 
+	
 	for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
 	{
 		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
@@ -1364,6 +1409,79 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 		
 		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
 	}
+
+}
+
+static void copyConstraintsFromProxy(btAlignedObjectArray<btMultiBodySolverConstraint>& constraints, const btAlignedObjectArray<btMultiBodySolverConstraint>& constraintsProxy, bool onlyDynamicData)
+{
+	if (onlyDynamicData)
+	{
+		btAssert(constraints.size() == constraintsProxy.size());
+		for (int i = 0; i < constraintsProxy.size(); ++i)
+		{
+			btMultiBodySolverConstraint& destConstraint = constraints[i];
+			const btMultiBodySolverConstraint& srcConstraint = constraintsProxy[i];
+
+			destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse;
+			destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse;
+		}
+		return;
+	}
+
+	constraints.resize(constraintsProxy.size());
+	for (int i = 0; i < constraintsProxy.size(); ++i)
+	{
+		constraints[i] = constraintsProxy[i];
+	}
+}
+
+static void copyConstraintsToProxy(btAlignedObjectArray<btMultiBodySolverConstraint>& constraintsProxy, btAlignedObjectArray<btMultiBodySolverConstraint>& constraints, bool onlyDynamicData)
+{
+	if (onlyDynamicData)
+	{
+		btAssert(constraintsProxy.size() == constraints.size());
+		for (int i = 0; i < constraints.size(); ++i)
+		{
+			btMultiBodySolverConstraint& destConstraint = constraintsProxy[i];
+			const btMultiBodySolverConstraint& srcConstraint = constraints[i];
+
+			destConstraint.m_appliedImpulse = srcConstraint.m_appliedImpulse;
+			destConstraint.m_appliedPushImpulse = srcConstraint.m_appliedPushImpulse;
+		}
+		return;
+	}
+
+	constraintsProxy.resize(constraints.size());
+	for (int i = 0; i < constraints.size(); ++i)
+	{
+		constraintsProxy[i] = constraints[i];
+	}
+}
+
+void btMultiBodyConstraintSolver::setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& data, bool onlyDynamicData)
+{
+	copyConstraintsFromProxy(m_multiBodyNonContactConstraints, data.m_multiBodyNonContactConstraints, onlyDynamicData);
+	copyConstraintsFromProxy(m_multiBodyNormalContactConstraints, data.m_multiBodyNormalContactConstraints, onlyDynamicData);
+	copyConstraintsFromProxy(m_multiBodyFrictionContactConstraints, data.m_multiBodyFrictionContactConstraints, onlyDynamicData);
+	copyConstraintsFromProxy(m_multiBodyTorsionalFrictionContactConstraints, data.m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData);
+
+	if (onlyDynamicData)
+		m_data.m_deltaVelocities = data.m_data.m_deltaVelocities;
+	else
+		m_data = data.m_data;
+}
+
+void btMultiBodyConstraintSolver::getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData)
+{
+	copyConstraintsToProxy(data.m_multiBodyNonContactConstraints, m_multiBodyNonContactConstraints, onlyDynamicData);
+	copyConstraintsToProxy(data.m_multiBodyNormalContactConstraints, m_multiBodyNormalContactConstraints, onlyDynamicData);
+	copyConstraintsToProxy(data.m_multiBodyFrictionContactConstraints, m_multiBodyFrictionContactConstraints, onlyDynamicData);
+	copyConstraintsToProxy(data.m_multiBodyTorsionalFrictionContactConstraints, m_multiBodyTorsionalFrictionContactConstraints, onlyDynamicData);
+
+	if (onlyDynamicData)
+		data.m_data.m_deltaVelocities = m_data.m_deltaVelocities;
+	else
+		data.m_data = m_data;
 }
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
@@ -1393,6 +1511,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
 }
 #endif
 
+
 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
 {
 #if 1 
@@ -1405,8 +1524,10 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
 	}
 	
+
 	if (c.m_multiBodyA)
 	{
+		
 		c.m_multiBodyA->setCompanionId(-1);
 		btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
 		btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
@@ -1414,8 +1535,7 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 		{
 			c.m_multiBodyA->addBaseConstraintForce(force);
 			c.m_multiBodyA->addBaseConstraintTorque(torque);
-		}
+		} else
-		else
 		{
 			c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
 				//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
@@ -1433,14 +1553,14 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 			{
 				c.m_multiBodyB->addBaseConstraintForce(force);
 				c.m_multiBodyB->addBaseConstraintTorque(torque);
-			}
+			} else
-			else
 			{
 				{
 					c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
 					//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
 					c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
 				}
+					
 			}
 		}
 	}
@@ -1458,6 +1578,9 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 		c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
 	}
 #endif
+
+
+
 }
 
 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
@@ -1465,6 +1588,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 	BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
 	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
 	
+
 	//write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
 	//or as applied force, so we can measure the joint reaction forces easier
 	for (int i=0;i<numPoolConstraints;i++)
@@ -1480,12 +1604,14 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		}
 	}
 
+
 	for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
 	{
 		btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
 		writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
 	}
 
+	
 	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 	{
 		BT_PROFILE("warm starting write back");
@@ -1509,109 +1635,11 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 		
 			//do a callback here?
 	}
-#if 0
-	//multibody joint feedback
-	{
-		BT_PROFILE("multi body joint feedback");
-		for (int j=0;j<numPoolConstraints;j++)
-		{
-			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
-		
-			//apply the joint feedback into all links of the btMultiBody
-			//todo: double-check the signs of the applied impulse
-
-			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-			}
-#if 0
-			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-
-			}
-			if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
-					m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-			}
-		
-			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-			{
-				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
-				{
-					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-				}
-
-				if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
-				{
-					applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
-						m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
-				}
-			}
-#endif
-		}
-	
-		for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
-		{
-			const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
-			if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-			if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
-			{
-				applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
-			}
-		}
-	}
-
-	numPoolConstraints = m_multiBodyNonContactConstraints.size();
-
-#if 0
-	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
-	for (int i=0;i<numPoolConstraints;i++)
-	{
-		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
-
-		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
-		btJointFeedback* fb = constr->getJointFeedback();
-		if (fb)
-		{
-			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
-			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
-			
-		}
-
-		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
-		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
-		{
-			constr->setEnabled(false);
-		}
-
-	}
-#endif
-#endif
 
 	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 }
 
+
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
 	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
@@ -1624,4 +1652,6 @@ void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies
 
 	m_tmpMultiBodyConstraints = 0;
 	m_tmpNumMultiBodyConstraints = 0;
+	
+
 }

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -28,7 +28,9 @@ class btMultiBody;
 ATTRIBUTE_ALIGNED16(class)
 btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
 {
+
 protected:
+
 	btMultiBodyConstraintArray			m_multiBodyNonContactConstraints;
 
 	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
@@ -46,6 +48,7 @@ protected:
 	//solve 2 friction directions and clamp against the implicit friction cone
 	btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB);
 	
+
 	void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);
     
     btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
@@ -81,15 +84,55 @@ protected:
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	void	applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
 	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
-
 public:
+
+	struct btMultiBodyInternalConstraintData
+	{
+		/// Multibody (joint) constraints. This is shared by all the blocks.
+		btMultiBodyConstraint** m_multiBodyConstraints;
+
+		/// Number of multibody (joint) constraints. This is shared by all the
+		/// blocks.
+		int m_numMultiBodyConstraints;
+
+		/// Array of multibody non-contact constraints
+		btAlignedObjectArray<btMultiBodySolverConstraint> m_multiBodyNonContactConstraints;
+
+		/// Array of multibody normal contact constraints
+		btAlignedObjectArray<btMultiBodySolverConstraint> m_multiBodyNormalContactConstraints;
+
+		/// Array of multibody friction contact constraints
+		btAlignedObjectArray<btMultiBodySolverConstraint> m_multiBodyFrictionContactConstraints;
+
+		/// Array of multibody rolling friction contact constraints
+		btAlignedObjectArray<btMultiBodySolverConstraint> m_multiBodyTorsionalFrictionContactConstraints;
+
+		/// Pointer to the block constraint solver's multi body Jacobian data, which
+		/// is shared by all the constraint blocks.
+		btMultiBodyJacobianData m_data;
+	};
+
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
+	/// Copies internal constraint data from \p proxy
+	virtual void setMultiBodyInternalConstraintData(const btMultiBodyInternalConstraintData& proxy, bool onlyDynamicData = false);
+
+	/// Copies internal constraint data to \p proxy
+	virtual void getMultiBodyInternalConstraintData(btMultiBodyInternalConstraintData& data, bool onlyDynamicData = false);
+
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
+	virtual btScalar solveGroupConvertConstraintPrestep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveGroupConvertConstraintPoststep(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
+	virtual btScalar solveSingleIterationNew(int iteration, 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 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);
 };
 
+	
+	
+
+
 #endif //BT_MULTIBODY_CONSTRAINT_SOLVER_H
+