code clean up + check in examples

18 files changed, 1552 insertions, 412 deletions

diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp
index 67f670d07..136aefb20 100644
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/BasicDemo/BasicExample.cpp
@@ -16,9 +16,9 @@ subject to the following restrictions:
 #include "BasicExample.h"
 
 #include "btBulletDynamicsCommon.h"
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Z 5
+#define ARRAY_SIZE_Y 1
+#define ARRAY_SIZE_X 1
+#define ARRAY_SIZE_Z 1
 
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btAlignedObjectArray.h"
diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp
new file mode 100644
index 000000000..b70751e2f
--- /dev/null
+++ b/examples/DeformableContact/DeformableContact.cpp
@@ -0,0 +1,412 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#include "DeformableContact.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../SoftDemo/BunnyMesh.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+///The DeformableContact demo deformable bodies self-collision
+static bool g_floatingBase = true;
+static float friction = 1.;
+class DeformableContact : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+public:
+    
+    struct TetraBunny
+    {
+        #include "../SoftDemo/bunny.inl"
+    };
+
+    
+	DeformableContact(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper)
+	{
+	}
+    
+	virtual ~DeformableContact()
+	{
+	}
+    
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 15;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    virtual void stepSimulation(float deltaTime);
+    
+    btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+    
+    void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+    
+
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonMultiBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void DeformableContact::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///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();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+    btMultiBodyConstraintSolver* sol;
+    sol = new btMultiBodyConstraintSolver;
+	m_solver = sol;
+    
+    m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    {
+        ///create a ground
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
+
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -40, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+
+    
+    {
+        bool damping = true;
+        bool gyro = true;
+        int numLinks = 3;
+        bool spherical = true;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(1, 1, 1);
+        btVector3 baseHalfExtents(1, 1, 1);
+        
+        btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.f);
+            mbC->setAngularDamping(0.f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.1f);
+            mbC->setAngularDamping(0.9f);
+        }
+
+        if (numLinks > 0)
+        {
+            btScalar q0 = 0.f * SIMD_PI / 180.f;
+            if (!spherical)
+            {
+                mbC->setJointPosMultiDof(0, &q0);
+            }
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+        addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
+    }
+    
+    // create a patch of cloth
+    {
+        const btScalar s = 6;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+                                                         btVector3(+s, 0, -s),
+                                                         btVector3(-s, 0, +s),
+                                                         btVector3(+s, 0, +s),
+                                                         20,20,
+                                                         1 + 2 + 4 + 8, true);
+
+        psb->getCollisionShape()->setMargin(0.25);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(.5);
+        psb->setDampingCoefficient(0.01);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 0;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+
+    }
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableContact::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+void DeformableContact::stepSimulation(float deltaTime)
+{
+//    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
+    m_dynamicsWorld->stepSimulation(deltaTime, 4, 1./240.);
+}
+
+
+btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = .05f;
+    
+    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, !floating, canSleep);
+    
+    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    pMultiBody->setBasePos(basePosition);
+    pMultiBody->setWorldToBaseRot(baseOriQuat);
+    btVector3 vel(0, 0, 0);
+    //    pMultiBody->setBaseVel(vel);
+    
+    //init the links
+    btVector3 hingeJointAxis(1, 0, 0);
+    float linkMass = .05f;
+    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 DeformableContact::addColliders_testMultiDof(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();
+    
+    {
+        //    float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+        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];
+        //    float pos[4]={posr.x(),posr.y(),posr.z(),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;
+    }
+}
+class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DeformableContact(options.m_guiHelper);
+}
+
+
diff --git a/examples/DeformableContact/DeformableContact.h b/examples/DeformableContact/DeformableContact.h
new file mode 100644
index 000000000..591c6bab1
--- /dev/null
+++ b/examples/DeformableContact/DeformableContact.h
@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 _DEFORMABLE_CONTACT_H
+#define _DEFORMABLE_CONTACT_H
+
+class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_DEFORMABLE_CONTACT_H
diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp
index 990bdd7b9..7ce94a314 100644
--- a/examples/DeformableDemo/DeformableDemo.cpp
+++ b/examples/DeformableDemo/DeformableDemo.cpp
@@ -35,6 +35,7 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
@@ -59,17 +60,23 @@ public:
 
 	void resetCamera()
 	{
-        float dist = 15;
+        float dist = 20;
         float pitch = -45;
         float yaw = 100;
-        float targetPos[3] = {0, -5, 0};
+        float targetPos[3] = {0, -3, 0};
 		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
     
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
     
     void Ctor_RbUpStack(int count)
     {
-        float mass = 1;
+        float mass = 0.2;
         
         btCompoundShape* cylinderCompound = new btCompoundShape;
         btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
@@ -84,18 +91,30 @@ public:
         
         btCollisionShape* shape[] = {
             new btBoxShape(btVector3(1, 1, 1)),
-            cylinderCompound,
-            new btSphereShape(0.75)
-            
+//            new btSphereShape(0.75),
+//            cylinderCompound
         };
-        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
-        for (int i = 0; i < count; ++i)
-        {
-            btTransform startTransform;
-            startTransform.setIdentity();
-            startTransform.setOrigin(btVector3(0, 3 + 3 * i, 0));
-            createRigidBody(mass, startTransform, shape[i % nshapes]);
-        }
+//        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+//        for (int i = 0; i < count; ++i)
+//        {
+//            btTransform startTransform;
+//            startTransform.setIdentity();
+//            startTransform.setOrigin(btVector3(0, 2+ 2 * i, 0));
+//            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+//            createRigidBody(mass, startTransform, shape[i % nshapes]);
+//        }
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(1, 2, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(1, 2, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 2, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 2, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(0, 4, 0));
+        createRigidBody(mass, startTransform, shape[0]);
     }
     
     virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
@@ -143,14 +162,16 @@ void DeformableDemo::initPhysics()
     btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
 
 	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
-	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
 	m_solver = sol;
 
-	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver);
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
     deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
 	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-    getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    
+//    getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     {
@@ -161,8 +182,8 @@ void DeformableDemo::initPhysics()
 
         btTransform groundTransform;
         groundTransform.setIdentity();
-        groundTransform.setOrigin(btVector3(0, -50, 0));
-        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
+        groundTransform.setOrigin(btVector3(0, -30, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
         //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
         btScalar mass(0.);
 
@@ -177,7 +198,7 @@ void DeformableDemo::initPhysics()
         btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
         btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
         btRigidBody* body = new btRigidBody(rbInfo);
-        body->setFriction(.5);
+        body->setFriction(1);
 
         //add the ground to the dynamics world
         m_dynamicsWorld->addRigidBody(body);
@@ -185,24 +206,37 @@ void DeformableDemo::initPhysics()
     
     // create a piece of cloth
     {
+        bool onGround = true;
         const btScalar s = 4;
-        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
                                                          btVector3(+s, 0, -s),
                                                          btVector3(-s, 0, +s),
                                                          btVector3(+s, 0, +s),
-//                                                         3, 3,
-                                                                 20,20,
+//                                                         3,3,
+                                                          20,20,
                                                           1 + 2 + 4 + 8, true);
-//                                                         0, true);
+//                                                          0, true);
+
+        if (onGround)
+            psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
+                                                 btVector3(+s, 0, -s),
+                                                 btVector3(-s, 0, +s),
+                                                 btVector3(+s, 0, +s),
+                                                 20,20,
+                                                 0, true);
         
-        psb->getCollisionShape()->setMargin(0.25);
+        psb->getCollisionShape()->setMargin(0.1);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(2);
-        psb->setDampingCoefficient(0.02);
+        psb->setTotalMass(10);
+        psb->setSpringStiffness(10);
+        psb->setDampingCoefficient(0.01);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 1;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         
         // add a few rigid bodies
-        Ctor_RbUpStack(10);
+        Ctor_RbUpStack(1);
     }
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 6d3db549e..f29cef2ba 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -48,6 +48,9 @@
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
 #include "../DeformableDemo/DeformableDemo.h"
+#include "../Pinch/Pinch.h"
+#include "../DeformableContact/DeformableContact.h"
+#include "../VolumetricDeformable/VolumetricDeformable.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -119,7 +122,10 @@ static ExampleEntry gDefaultExamples[] =
 
 		ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
 
-        ExampleEntry(0, "Deformable", "Deformable test", DeformableCreateFunc),
+        ExampleEntry(0, "Deformable-RigidBody Contact", "Deformable test", DeformableCreateFunc),
+        ExampleEntry(0, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
+        ExampleEntry(0, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
+        ExampleEntry(0, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableContactCreateFunc),
         
 		ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
 					 AllConstraintCreateFunc),
diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp
new file mode 100644
index 000000000..cedc51750
--- /dev/null
+++ b/examples/Pinch/Pinch.cpp
@@ -0,0 +1,297 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#include "Pinch.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The Pinch shows the use of rolling friction.
+///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
+///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+struct TetraBunny
+{
+#include "../SoftDemo/bunny.inl"
+};
+
+
+class Pinch : public CommonRigidBodyBase
+{
+public:
+	Pinch(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~Pinch()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 25;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, -3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 2;
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(3, 3, 0.5)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(10, 0, 0));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
+{
+    btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
+    if (rbs.size()<2)
+        return;
+    btRigidBody* rb0 = rbs[0];
+    btScalar pressTime = 0.9;
+    btTransform rbTransform;
+    rbTransform.setIdentity();
+    btVector3 translation = btVector3(0.5,3,4);
+    btVector3 velocity = btVector3(0,5,0);
+    if (time < pressTime)
+    {
+        velocity = btVector3(0,0,-2);
+        translation += velocity * time;
+    }
+    else
+        translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity;
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb0->setCenterOfMassTransform(rbTransform);
+    rb0->setAngularVelocity(btVector3(0,0,0));
+    rb0->setLinearVelocity(velocity);
+    
+    btRigidBody* rb1 = rbs[1];
+    translation = btVector3(0.5,3,-4);
+    velocity = btVector3(0,5,0);
+    if (time < pressTime)
+    {
+        velocity = btVector3(0,0,2);
+        translation += velocity * time;
+    }
+    else
+        translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity;
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb1->setCenterOfMassTransform(rbTransform);
+    rb1->setAngularVelocity(btVector3(0,0,0));
+    rb1->setLinearVelocity(velocity);
+    
+    rb0->setFriction(2);
+    rb1->setFriction(2);
+}
+
+void Pinch::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
+	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    
+    getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    //create a ground
+    {
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
+
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -25, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    // create a soft block
+    {
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 4, 0));
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->setTotalMass(1);
+        psb->setSpringStiffness(10);
+        psb->setDampingCoefficient(0.01);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 0.5;
+        // add a grippers
+        createGrip();
+    }
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void Pinch::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options)
+{
+	return new Pinch(options.m_guiHelper);
+}
+
+
diff --git a/examples/Pinch/Pinch.h b/examples/Pinch/Pinch.h
new file mode 100644
index 000000000..6c7e2fb3c
--- /dev/null
+++ b/examples/Pinch/Pinch.h
@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 _PINCH_H
+#define _PINCH_H
+
+class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_PINCH_H
diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp
new file mode 100644
index 000000000..b30e9bc24
--- /dev/null
+++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp
@@ -0,0 +1,294 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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.
+*/
+
+///create 125 (5x5x5) dynamic object
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+
+//maximum number of objects (and allow user to shoot additional boxes)
+#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
+
+///scaling of the objects (0.1 = 20 centimeter boxes )
+#define SCALING 1.
+#define START_POS_X -5
+#define START_POS_Y -5
+#define START_POS_Z -3
+
+#include "VolumetricDeformable.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The VolumetricDeformable shows the use of rolling friction.
+///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same.
+///Generally it is best to leave the rolling friction coefficient zero (or close to zero).
+
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+class VolumetricDeformable : public CommonRigidBodyBase
+{
+public:
+	VolumetricDeformable(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~VolumetricDeformable()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 20;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, 3, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
+    {
+        btCollisionShape* box = new btBoxShape(halfEdge);
+        m_collisionShapes.push_back(box);
+        
+        btTransform Transform;
+        Transform.setIdentity();
+        Transform.setOrigin(translation);
+        Transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            box->calculateLocalInertia(mass, localInertia);
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(Transform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, box, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+        
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    void Ctor_RbUpStack(int count)
+    {
+        float mass = 0.02;
+        
+        btCompoundShape* cylinderCompound = new btCompoundShape;
+        btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
+        btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5));
+        btTransform localTransform;
+        localTransform.setIdentity();
+        cylinderCompound->addChildShape(localTransform, boxShape);
+        btQuaternion orn(SIMD_HALF_PI, 0, 0);
+        localTransform.setRotation(orn);
+        //    localTransform.setOrigin(btVector3(1,1,1));
+        cylinderCompound->addChildShape(localTransform, cylinderShape);
+        
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(1, 1, 1)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(i, 10 + 2 * i, i-1));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void VolumetricDeformable::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///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();
+    btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+	btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver();
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
+	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
+	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    {
+        ///create a ground
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(50.), btScalar(150.)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -50, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0));
+    createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0));
+    createStaticBox(btVector3(5, 5, 1), btVector3(0,0,5));
+    createStaticBox(btVector3(5, 5, 1), btVector3(0,0,-5));
+    
+    // create volumetric soft body
+    {
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 5, 0));
+//        psb->setVolumeMass(10);
+        psb->getCollisionShape()->setMargin(0.25);
+//        psb->generateBendingConstraints(2);
+        psb->setTotalMass(1);
+        psb->setSpringStiffness(1);
+        psb->setDampingCoefficient(0.01);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 0.5;
+    }
+    // add a few rigid bodies
+    Ctor_RbUpStack(4); 
+    
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void VolumetricDeformable::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options)
+{
+	return new VolumetricDeformable(options.m_guiHelper);
+}
+
+
diff --git a/examples/VolumetricDeformable/VolumetricDeformable.h b/examples/VolumetricDeformable/VolumetricDeformable.h
new file mode 100644
index 000000000..af1cc38b6
--- /dev/null
+++ b/examples/VolumetricDeformable/VolumetricDeformable.h
@@ -0,0 +1,20 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+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 _VOLUMETRIC_DEFORMABLE_H
+#define _VOLUMETRIC_DEFORMABLE_H
+
+class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_VOLUMETRIC_DEFORMABLE__H
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index 9174107ba..024cc66b6 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -77,6 +77,51 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
     }
 }
 
+void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
+{
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
+            psb->m_nodes[j].m_v += one_over_mass * force[counter++];
+        }
+    }
+    if (setZero)
+    {
+        for (int i = 0; i < force.size(); ++i)
+            force[i].setZero();
+    }
+}
+
+void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
+{
+    // add implicit force
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->addScaledImplicitForce(dt, residual);
+    }
+}
+
+btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const
+{
+    btScalar norm_squared = 0;
+    for (int i = 0; i < residual.size(); ++i)
+    {
+        norm_squared += residual[i].length2();
+    }
+    return std::sqrt(norm_squared+SIMD_EPSILON);
+}
+
+void btBackwardEulerObjective::applyExplicitForce(TVStack& force)
+{
+    for (int i = 0; i < m_lf.size(); ++i)
+        m_lf[i]->addScaledExplicitForce(m_dt, force);
+    applyForce(force, true);
+}
+
 void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
 {
     size_t counter = 0;
@@ -85,7 +130,7 @@ void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual
         btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
-            dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1;
+            dv[counter] = psb->m_nodes[j].m_im * residual[counter];
             ++counter;
         }
     }
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index e9642f6ae..68f7f66fe 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -12,66 +12,10 @@
 #include "btLagrangianForce.h"
 #include "btMassSpring.h"
 #include "btContactProjection.h"
+#include "btPreconditioner.h"
 #include "btDeformableRigidDynamicsWorld.h"
 
 class btDeformableRigidDynamicsWorld;
-
-class Preconditioner
-{
-public:
-    using TVStack = btAlignedObjectArray<btVector3>;
-    virtual void operator()(const TVStack& x, TVStack& b) = 0;
-    virtual void reinitialize(bool nodeUpdated) = 0;
-};
-
-class DefaultPreconditioner : public Preconditioner
-{
-public:
-    virtual void operator()(const TVStack& x, TVStack& b)
-    {
-        btAssert(b.size() == x.size());
-        for (int i = 0; i < b.size(); ++i)
-            b[i] = x[i];
-    }
-    virtual void reinitialize(bool nodeUpdated)
-    {
-        
-    }
-};
-
-class MassPreconditioner : public Preconditioner
-{
-    btAlignedObjectArray<btScalar> m_inv_mass;
-    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
-public:
-    MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
-    : m_softBodies(softBodies)
-    {
-    }
-    
-    virtual void reinitialize(bool nodeUpdated)
-    {
-        if (nodeUpdated)
-        {
-            m_inv_mass.clear();
-            for (int i = 0; i < m_softBodies.size(); ++i)
-            {
-                btSoftBody* psb = m_softBodies[i];
-                for (int j = 0; j < psb->m_nodes.size(); ++j)
-                    m_inv_mass.push_back(psb->m_nodes[j].m_im);
-            }
-        }
-    }
-    
-    virtual void operator()(const TVStack& x, TVStack& b)
-    {
-        btAssert(b.size() == x.size());
-        btAssert(m_inv_mass.size() == x.size());
-        for (int i = 0; i < b.size(); ++i)
-            b[i] = x[i] * m_inv_mass[i];
-    }
-};
-
 class btBackwardEulerObjective
 {
 public:
@@ -91,74 +35,60 @@ public:
     
     void initialize(){}
     
-    void computeResidual(btScalar dt, TVStack& residual) const
-    {
-        // add implicit force
-        for (int i = 0; i < m_lf.size(); ++i)
-        {
-            m_lf[i]->addScaledImplicitForce(dt, residual);
-        }
-    }
+    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+    void computeResidual(btScalar dt, TVStack& residual) const;
     
-    void applyExplicitForce(TVStack& force)
-    {
-        for (int i = 0; i < m_lf.size(); ++i)
-            m_lf[i]->addScaledExplicitForce(m_dt, force);
-        
-        size_t counter = 0;
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im;
-                psb->m_nodes[j].m_v += one_over_mass * force[counter];
-                force[counter].setZero();
-                counter++;
-            }
-        }
-    }
+    // add explicit force to the velocity
+    void applyExplicitForce(TVStack& force);
     
-    btScalar computeNorm(const TVStack& residual) const
-    {
-        btScalar norm_squared = 0;
-        for (int i = 0; i < residual.size(); ++i)
-        {
-            norm_squared += residual[i].length2();
-        }
-        return std::sqrt(norm_squared+SIMD_EPSILON);
-    }
+    // apply force to velocity and optionally reset the force to zero
+    void applyForce(TVStack& force, bool setZero);
+    
+    // compute the norm of the residual
+    btScalar computeNorm(const TVStack& residual) const;
     
+    // compute one step of the solve (there is only one solve if the system is linear)
     void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
     
+    // perform A*x = b
     void multiply(const TVStack& x, TVStack& b) const;
     
+    // update the constraints treated as projections
     void updateProjection(const TVStack& dv)
     {
         projection.update(dv, m_backupVelocity);
     }
+    
+    // set initial guess for CG solve
     void initialGuess(TVStack& dv, const TVStack& residual);
     
+    // reset data structure
     void reinitialize(bool nodeUpdated);
     
+    // enforce constraints in CG solve
     void enforceConstraint(TVStack& x)
     {
         projection.enforceConstraint(x);
         updateVelocity(x);
     }
     
+    // add dv to velocity
     void updateVelocity(const TVStack& dv);
     
-    void setConstraintDirections()
+    //set constraints as projections
+    void setConstraints()
     {
-        projection.setConstraintDirections();
+        projection.setConstraints();
     }
+    
+    // update the projections and project the residual
     void project(TVStack& r, const TVStack& dv)
     {
         updateProjection(dv);
         projection(r);
     }
     
+    // perform precondition M^(-1) x = b
     void precondition(const TVStack& x, TVStack& b)
     {
         m_preconditioner->operator()(x,b);
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 46979c8f0..ff5eecd8c 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -17,12 +17,12 @@ struct Constraint
     btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
     btAlignedObjectArray<btVector3> m_direction;
     btAlignedObjectArray<btScalar> m_value;
+    // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
+    btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
     
     Constraint(const btSoftBody::RContact& rcontact)
     {
-        m_contact.push_back(&rcontact);
-        m_direction.push_back(rcontact.m_cti.m_normal);
-        m_value.push_back(0);
+        append(rcontact);
     }
     
     Constraint(const btVector3 dir)
@@ -30,31 +30,57 @@ struct Constraint
         m_contact.push_back(nullptr);
         m_direction.push_back(dir);
         m_value.push_back(0);
+        m_accumulated_normal_impulse.push_back(0);
     }
     
     Constraint()
     {
+        m_contact.push_back(nullptr);
+        m_direction.push_back(btVector3(0,0,0));
+        m_value.push_back(0);
+        m_accumulated_normal_impulse.push_back(0);
+    }
+    
+    void append(const btSoftBody::RContact& rcontact)
+    {
+        m_contact.push_back(&rcontact);
+        m_direction.push_back(rcontact.m_cti.m_normal);
+        m_value.push_back(0);
+        m_accumulated_normal_impulse.push_back(0);
+    }
+    
+    ~Constraint()
+    {
     }
 };
 
 struct Friction
 {
-    btAlignedObjectArray<bool> m_static;
-    btAlignedObjectArray<btScalar> m_impulse;
-    btAlignedObjectArray<btScalar> m_dv;
-    btAlignedObjectArray<btVector3> m_direction;
-    btAlignedObjectArray<btVector3> m_direction_prev;
+    
+    btAlignedObjectArray<bool> m_static; // whether the friction is static
+    btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels
+    btAlignedObjectArray<btScalar> m_dv;      // the dv magnitude of the node
+    btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node
+    
     
     btAlignedObjectArray<bool> m_static_prev;
     btAlignedObjectArray<btScalar> m_impulse_prev;
     btAlignedObjectArray<btScalar> m_dv_prev;
+    btAlignedObjectArray<btVector3> m_direction_prev;
     
-    btAlignedObjectArray<bool> m_released;
+    btAlignedObjectArray<bool> m_released; // whether the contact is released
     
-    btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
+
+    
+    // the total impulse the node applied to the rb in the tangential direction in the cg solve
     btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
     Friction()
     {
+        append();
+    }
+    
+    void append()
+    {
         m_static.push_back(false);
         m_static_prev.push_back(false);
         
@@ -67,7 +93,6 @@ struct Friction
         m_dv.push_back(0);
         m_dv_prev.push_back(0);
         
-        m_accumulated_normal_impulse.push_back(0);
         m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
         m_released.push_back(false);
     }
@@ -100,7 +125,7 @@ public:
     // apply the constraints
     virtual void operator()(TVStack& x) = 0;
     
-    virtual void setConstraintDirections() = 0;
+    virtual void setConstraints() = 0;
     
     // update the constraints
     virtual void update(const TVStack& dv, const TVStack& backup_v) = 0;
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index a2b69f48d..f23b26385 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -18,12 +18,10 @@ class btConjugateGradient
     using TVStack = btAlignedObjectArray<btVector3>;
     TVStack r,p,z,temp;
     int max_iterations;
-    btScalar tolerance;
     
 public:
     btConjugateGradient(const int max_it_in)
     : max_iterations(max_it_in)
-    , tolerance(std::numeric_limits<float>::epsilon() * 16)
     {
     }
     
@@ -121,12 +119,6 @@ public:
         return ans;
     }
     
-    void setZero(TVStack& a)
-    {
-        for (int i = 0; i < a.size(); ++i)
-            a[i].setZero();
-    }
-    
     void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
     {
 //        result += s*a
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 041792d08..cf2e05e59 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -61,9 +61,9 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                             multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
                             
                             btScalar vel = 0.0;
-                            for (int j = 0; j < ndof; ++j)
+                            for (int k = 0; k < ndof; ++k)
                             {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k];
                             }
                             va = cti.m_normal * vel * m_dt;
                         }
@@ -75,41 +75,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     btVector3 impulse = c->m_c0 * vr;
                     const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
                     const btVector3 impulse_tangent = impulse - impulse_normal;
+                    
+                    // start friction handling
+                    // copy old data
+                    friction.m_impulse_prev[j] = friction.m_impulse[j];
+                    friction.m_dv_prev[j] = friction.m_dv[j];
+                    friction.m_static_prev[j] = friction.m_static[j];
+                    
+                    // get the current tangent direction
                     btScalar local_tangent_norm = impulse_tangent.norm();
                     btVector3 local_tangent_dir = btVector3(0,0,0);
                     if (local_tangent_norm > SIMD_EPSILON)
                         local_tangent_dir = impulse_tangent.normalized();
 
                     // accumulated impulse on the rb in this and all prev cg iterations
-                    friction.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
-                    btScalar accumulated_normal = friction.m_accumulated_normal_impulse[j];
+                    constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
+                    const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
+                    
+                    // the total tangential impulse required to stop sliding
                     btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
                     btScalar tangent_norm = tangent.norm();
-                    // start friction handling
-                    // copy old data
-                    friction.m_impulse_prev[j] = friction.m_impulse[j];
-                    friction.m_dv_prev[j] = friction.m_dv[j];
-                    friction.m_static_prev[j] = friction.m_static[j];
-                    if (accumulated_normal < 0 && tangent_norm > SIMD_EPSILON)
+  
+                    if (accumulated_normal < 0)
                     {
                         friction.m_direction[j] = -local_tangent_dir;
-                        btScalar compare1 = -accumulated_normal*c->m_c3;
-                        btScalar compare2 = tangent_norm;
                         // do not allow switching from static friction to dynamic friction
                         // it causes cg to explode
                         if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
                         {
                             friction.m_static[j] = false;
                             friction.m_impulse[j] = -accumulated_normal*c->m_c3;
-                            friction.m_dv[j] = 0;
-                            impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
                         }
                         else
                         {
                             friction.m_static[j] = true;
-                            friction.m_impulse[j] = 0;
-                            friction.m_dv[j] = local_tangent_norm * c->m_c2/m_dt;
-                            impulse = impulse_normal + impulse_tangent;
+                            friction.m_impulse[j] = local_tangent_norm;
                         }
                     }
                     else
@@ -117,27 +117,29 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         friction.m_released[j] = true;
                         friction.m_static[j] = false;
                         friction.m_impulse[j] = 0;
-                        friction.m_dv[j] = 0;
                         friction.m_direction[j] = btVector3(0,0,0);
-                        impulse = impulse_normal;
                     }
+                    friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
                     friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
+                    
+                    // the incremental impulse applied to rb in the tangential direction
+                    btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
+                    
                     if (1) // in the same CG solve, the set of constraits doesn't change
-//                    if (dn <= SIMD_EPSILON)
                     {
                         // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
                         
                         // dv = new_impulse + accumulated velocity change in previous CG iterations
                         // so we have the invariant node->m_v = backupVelocity + dv;
-                        btVector3 dv = -impulse * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
-                        btScalar dvn = dv.dot(cti.m_normal);
+//                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
+//                        btScalar dvn = dv.dot(cti.m_normal);
+                        btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
                         constraint.m_value[j] = dvn;
                         
                         // the incremental impulse:
                         // in the normal direction it's the normal component of "impulse"
                         // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
-                        impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
-//                        impulse = impulse_normal;
+                        impulse = impulse_normal + incremental_tangent;
                         if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                         {
                             if (rigidCol)
@@ -147,7 +149,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         {
                             if (multibodyLinkCol)
                             {
-                                double multiplier = 1;
+                                double multiplier = 0.5;
                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
                             }
                         }
@@ -158,8 +160,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
     }
 }
 
-
-void btContactProjection::setConstraintDirections()
+void btContactProjection::setConstraints()
 {
     // set Dirichlet constraint
     for (int i = 0; i < m_softBodies.size(); ++i)
@@ -188,6 +189,7 @@ void btContactProjection::setConstraintDirections()
     {
         btSoftBody* psb = m_softBodies[i];
         btMultiBodyJacobianData jacobianData;
+        btAlignedObjectArray<btScalar> jacobian;
         
         for (int j = 0; j < psb->m_rcontacts.size(); ++j)
         {
@@ -214,6 +216,7 @@ void btContactProjection::setConstraintDirections()
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
+                    jacobian.clear();
                     multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
                     if (multibodyLinkCol)
                     {
@@ -227,9 +230,11 @@ void btContactProjection::setConstraintDirections()
                         multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
                         
                         btScalar vel = 0.0;
+                        jacobian.resize(ndof);
                         for (int j = 0; j < ndof; ++j)
                         {
                             vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                            jacobian[j] = jac[j];
                         }
                         va = cti.m_normal * vel * m_dt;
                     }
@@ -262,22 +267,10 @@ void btContactProjection::setConstraintDirections()
                             btScalar dot_prod = dirs[0].dot(cti.m_normal);
                             if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
                             {
-                                constraints[j].m_contact.push_back(&c);
-                                constraints[j].m_direction.push_back(cti.m_normal);
-                                constraints[j].m_value.push_back(0);
-                                
+                                // group the constraints
+                                constraints[j].append(c);
                                 // push in an empty friction
-                                frictions[j].m_direction.push_back(btVector3(0,0,0));
-                                frictions[j].m_direction_prev.push_back(btVector3(0,0,0));
-                                frictions[j].m_impulse.push_back(0);
-                                frictions[j].m_impulse_prev.push_back(0);
-                                frictions[j].m_dv.push_back(0);
-                                frictions[j].m_dv_prev.push_back(0);
-                                frictions[j].m_static.push_back(false);
-                                frictions[j].m_static_prev.push_back(false);
-                                frictions[j].m_accumulated_normal_impulse.push_back(0);
-                                frictions[j].m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
-                                frictions[j].m_released.push_back(false);
+                                frictions[j].append();
                                 merged = true;
                                 break;
                             }
@@ -295,3 +288,127 @@ void btContactProjection::setConstraintDirections()
         }
     }
 }
+
+void btContactProjection::enforceConstraint(TVStack& x)
+{
+    const int dim = 3;
+    for (auto& it : m_constraints)
+    {
+        const btAlignedObjectArray<Constraint>& constraints = it.second;
+        size_t i = m_indices[it.first];
+        const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
+        btAssert(constraints.size() <= dim);
+        btAssert(constraints.size() > 0);
+        if (constraints.size() == 1)
+        {
+            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+            for (int j = 0; j < constraints[0].m_direction.size(); ++j)
+                x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
+        }
+        else if (constraints.size() == 2)
+        {
+            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+            btAssert(free_dir.norm() > SIMD_EPSILON)
+            free_dir.normalize();
+            x[i] = x[i].dot(free_dir) * free_dir;
+            for (int j = 0; j < constraints.size(); ++j)
+            {
+                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                {
+                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                }
+            }
+            
+        }
+        else
+        {
+            x[i].setZero();
+            for (int j = 0; j < constraints.size(); ++j)
+            {
+                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                {
+                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                }
+            }
+        }
+        
+        // apply friction if the node is not constrained in all directions
+        if (constraints.size() < 3)
+        {
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                const Friction& friction= frictions[f];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
+                {
+                    // clear the old constraint
+                    if (friction.m_static_prev[j] == true)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
+                    }
+                    // add the new constraint
+                    if (friction.m_static[j] == true)
+                    {
+                        x[i] += friction.m_direction[j] * friction.m_dv[j];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btContactProjection::operator()(TVStack& x)
+{
+    const int dim = 3;
+    for (auto& it : m_constraints)
+    {
+        const btAlignedObjectArray<Constraint>& constraints = it.second;
+        size_t i = m_indices[it.first];
+        btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
+        btAssert(constraints.size() <= dim);
+        btAssert(constraints.size() > 0);
+        if (constraints.size() == 1)
+        {
+            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+        }
+        else if (constraints.size() == 2)
+        {
+            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+            btAssert(free_dir.norm() > SIMD_EPSILON)
+            free_dir.normalize();
+            x[i] = x[i].dot(free_dir) * free_dir;
+        }
+        else
+            x[i].setZero();
+        
+        // apply friction if the node is not constrained in all directions
+        if (constraints.size() < 3)
+        {
+            bool has_static_constraint = false;
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                Friction& friction= frictions[f];
+                for (int j = 0; j < friction.m_static.size(); ++j)
+                    has_static_constraint = has_static_constraint || friction.m_static[j];
+            }
+            
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                Friction& friction= frictions[f];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
+                {
+                    // clear the old friction force
+                    if (friction.m_static_prev[j] == false)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
+                    }
+                    
+                    // only add to the rhs if there is no static friction constraint on the node
+                    if (friction.m_static[j] == false && !has_static_constraint)
+                    {
+                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                    }
+                }
+            }
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index 566fd9f92..725cc106b 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -18,171 +18,21 @@ public:
     btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : btCGProjection(softBodies, dt)
     {
-        
     }
     
     virtual ~btContactProjection()
     {
-        
     }
     
-    // apply the constraints
-    virtual void operator()(TVStack& x)
-    {
-        const int dim = 3;
-        for (auto& it : m_constraints)
-        {
-            const btAlignedObjectArray<Constraint>& constraints = it.second;
-            size_t i = m_indices[it.first];
-            btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
-            btAssert(constraints.size() <= dim);
-            btAssert(constraints.size() > 0);
-            if (constraints.size() == 1)
-            {
-                x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
-                Friction& friction= frictions[0];
-                
-                bool has_static_constraint = false;
-                for (int j = 0; j < friction.m_static.size(); ++j)
-                    has_static_constraint = has_static_constraint || friction.m_static[j];
-                
-                for (int j = 0; j < friction.m_direction.size(); ++j)
-                {
-                    // clear the old friction force
-                    if (friction.m_static_prev[j] == false)
-                    {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
-                    }
-                    
-                    // only add to the rhs if there is no static friction constraint on the node
-                    if (friction.m_static[j] == false && !has_static_constraint)
-                    {
-                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
-                    }
-                }
-            }
-            else if (constraints.size() == 2)
-            {
-                // TODO : friction
-                btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
-                btAssert(free_dir.norm() > SIMD_EPSILON)
-                free_dir.normalize();
-                x[i] = x[i].dot(free_dir) * free_dir;
-                
-                bool has_static_constraint = false;
-                for (int f = 0; f < 2; ++f)
-                {
-                    Friction& friction= frictions[f];
-                    for (int j = 0; j < friction.m_static.size(); ++j)
-                        has_static_constraint = has_static_constraint || friction.m_static[j];
-                }
-                
-                for (int f = 0; f < 2; ++f)
-                {
-                    Friction& friction= frictions[f];
-                    for (int j = 0; j < friction.m_direction.size(); ++j)
-                    {
-                        // clear the old friction force
-                        if (friction.m_static_prev[j] == false)
-                        {
-                            x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
-                        }
-                        
-                        // only add to the rhs if there is no static friction constraint on the node
-                        if (friction.m_static[j] == false && !has_static_constraint)
-                        {
-                            x[i] += friction.m_direction[j] * friction.m_impulse[j];
-                        }
-                    }
-                }
-            }
-            else
-                x[i].setZero();
-        }
-    }
-
+    // apply the constraints to the rhs
+    virtual void operator()(TVStack& x);
     
-    virtual void enforceConstraint(TVStack& x)
-    {
-        const int dim = 3;
-        for (auto& it : m_constraints)
-        {
-            const btAlignedObjectArray<Constraint>& constraints = it.second;
-            size_t i = m_indices[it.first];
-            const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
-            btAssert(constraints.size() <= dim);
-            btAssert(constraints.size() > 0);
-            if (constraints.size() == 1)
-            {
-                x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
-                for (int j = 0; j < constraints[0].m_direction.size(); ++j)
-                    x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
-                
-                const Friction& friction= frictions[0];
-                for (int j = 0; j < friction.m_direction.size(); ++j)
-                {
-                    // clear the old constraint
-                    if (friction.m_static_prev[j] == true)
-                    {
-//                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
-                    }
-                    // add the new constraint
-                    if (friction.m_static[j] == true)
-                    {
-                        x[i] += friction.m_direction[j] * friction.m_dv[j];
-                    }
-                }
-            }
-            else if (constraints.size() == 2)
-            {
-                // TODO: friction
-                btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
-                btAssert(free_dir.norm() > SIMD_EPSILON)
-                free_dir.normalize();
-                x[i] = x[i].dot(free_dir) * free_dir;
-                for (int j = 0; j < constraints.size(); ++j)
-                {
-                    for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                    {
-                        x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                    }
-                }
-                
-                for (int f = 0; f < 2; ++f)
-                {
-                    const Friction& friction= frictions[f];
-                    for (int j = 0; j < friction.m_direction.size(); ++j)
-                    {
-                        // clear the old constraint
-                        if (friction.m_static_prev[j] == true)
-                        {
-//                            x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
-                        }
-                        // add the new constraint
-                        if (friction.m_static[j] == true)
-                        {
-                            x[i] += friction.m_direction[j] * friction.m_dv[j];
-                        }
-                    }
-                }
-            }
-            else
-            {
-                x[i].setZero();
-                for (int j = 0; j < constraints.size(); ++j)
-                {
-                    for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                    {
-                        x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                    }
-                }
-            }
-        }
-    }
+    // apply constraints to x in Ax=b
+    virtual void enforceConstraint(TVStack& x);
     
     // update the constraints
     virtual void update(const TVStack& dv, const TVStack& backupVelocity);
     
-    virtual void setConstraintDirections();
+    virtual void setConstraints();
 };
 #endif /* btContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index a8e1e5cd5..f98dfacbb 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -122,14 +122,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
     // apply explicit force
     m_objective->applyExplicitForce(m_residual);
     
-    // remove contact constraints with separating velocity
-    setConstraintDirections();
+    // add constraints to the solver
+    setConstraints();
     
     backupVelocity();
     
     for (int i = 0; i < m_solveIterations; ++i)
     {
         m_objective->computeResidual(solverdt, m_residual);
+        m_objective->initialGuess(m_dv, m_residual);
         m_objective->computeStep(m_dv, m_residual, solverdt);
         updateVelocity();
     }
@@ -153,9 +154,9 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated)
     m_objective->reinitialize(nodeUpdated);
 }
 
-void btDeformableBodySolver::setConstraintDirections()
+void btDeformableBodySolver::setConstraints()
 {
-    m_objective->setConstraintDirections();
+    m_objective->setConstraints();
 }
 
 void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
@@ -173,9 +174,76 @@ void btDeformableBodySolver::updateVelocity()
         btSoftBody* psb = m_softBodySet[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
-//            psb->m_nodes[j].m_v += m_dv[counter];
             psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
             ++counter;
         }
     }
 }
+
+
+void btDeformableBodySolver::advect(btScalar dt)
+{
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            auto& node = psb->m_nodes[j];
+            node.m_x  =  node.m_q + dt * node.m_v;
+        }
+    }
+}
+
+void btDeformableBodySolver::backupVelocity()
+{
+    // serial implementation
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
+        }
+    }
+}
+
+bool btDeformableBodySolver::updateNodes()
+{
+    int numNodes = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+        numNodes += m_softBodySet[i]->m_nodes.size();
+    if (numNodes != m_numNodes)
+    {
+        m_numNodes = numNodes;
+        m_backupVelocity.resize(numNodes);
+        return true;
+    }
+    return false;
+}
+
+
+void btDeformableBodySolver::predictMotion(float solverdt)
+{
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody *psb = m_softBodySet[i];
+        
+        if (psb->isActive())
+        {
+            psb->predictMotion(solverdt);
+        }
+    }
+}
+
+void btDeformableBodySolver::updateSoftBodies()
+{
+    for (int i = 0; i < m_softBodySet.size(); i++)
+    {
+        btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
+        if (psb->isActive())
+        {
+            psb->integrateMotion(); // normal is updated here
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 17f088c72..a1cd45077 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -51,17 +51,7 @@ public:
         return true;
     }
 
-    virtual void updateSoftBodies()
-    {
-        for (int i = 0; i < m_softBodySet.size(); i++)
-        {
-            btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
-            if (psb->isActive())
-            {
-                psb->integrateMotion(); // normal is updated here
-            }
-        }
-    }
+    virtual void updateSoftBodies();
     
     virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false)
     {
@@ -76,64 +66,17 @@ public:
     
     void reinitialize(bool nodeUpdated);
     
-    void setConstraintDirections();
+    void setConstraints();
     
-    void advect(btScalar dt)
-    {
-        size_t counter = 0;
-        for (int i = 0; i < m_softBodySet.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodySet[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                auto& node = psb->m_nodes[j];
-                node.m_x  =  node.m_q + dt * node.m_v;
-            }
-        }
-    }
+    void advect(btScalar dt);
     
-    void backupVelocity()
-    {
-        // serial implementation
-        int counter = 0;
-        for (int i = 0; i < m_softBodySet.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodySet[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
-            }
-        }
-    }
+    void backupVelocity();
     
     void updateVelocity();
     
-    bool updateNodes()
-    {
-        int numNodes = 0;
-        for (int i = 0; i < m_softBodySet.size(); ++i)
-            numNodes += m_softBodySet[i]->m_nodes.size();
-        if (numNodes != m_numNodes)
-        {
-            m_numNodes = numNodes;
-            m_backupVelocity.resize(numNodes);
-            return true;
-        }
-        return false;
-    }
+    bool updateNodes();
     
-    virtual void predictMotion(float solverdt)
-    {
-        for (int i = 0; i < m_softBodySet.size(); ++i)
-        {
-            btSoftBody *psb = m_softBodySet[i];
-            
-            if (psb->isActive())
-            {
-                psb->predictMotion(solverdt);
-            }
-        }
-    }
+    virtual void predictMotion(float solverdt);
 
     virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
 
diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h
new file mode 100644
index 000000000..ad190a3a8
--- /dev/null
+++ b/src/BulletSoftBody/btPreconditioner.h
@@ -0,0 +1,67 @@
+//
+//  btPreconditioner.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/18/19.
+//
+
+#ifndef BT_PRECONDITIONER_H
+#define BT_PRECONDITIONER_H
+
+class Preconditioner
+{
+public:
+    using TVStack = btAlignedObjectArray<btVector3>;
+    virtual void operator()(const TVStack& x, TVStack& b) = 0;
+    virtual void reinitialize(bool nodeUpdated) = 0;
+};
+
+class DefaultPreconditioner : public Preconditioner
+{
+public:
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i];
+    }
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        
+    }
+};
+
+class MassPreconditioner : public Preconditioner
+{
+    btAlignedObjectArray<btScalar> m_inv_mass;
+    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
+public:
+    MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
+    : m_softBodies(softBodies)
+    {
+    }
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        if (nodeUpdated)
+        {
+            m_inv_mass.clear();
+            for (int i = 0; i < m_softBodies.size(); ++i)
+            {
+                btSoftBody* psb = m_softBodies[i];
+                for (int j = 0; j < psb->m_nodes.size(); ++j)
+                    m_inv_mass.push_back(psb->m_nodes[j].m_im);
+            }
+        }
+    }
+    
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        btAssert(m_inv_mass.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i] * m_inv_mass[i];
+    }
+};
+
+#endif /* BT_PRECONDITIONER_H */