Merge remote-tracking branch 'bp/master'

54 files changed, 5598 insertions, 843 deletions

diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp
index b6a638c7f..8d3db3955 100644
--- a/Extras/obj2sdf/obj2sdf.cpp
+++ b/Extras/obj2sdf/obj2sdf.cpp
@@ -77,9 +77,10 @@ int main(int argc, char* argv[])
 	b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
 
 	std::vector<tinyobj::shape_t> shapes;
+	tinyobj::attrib_t attribute;
 
 	b3BulletDefaultFileIO fileIO;
-	std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath,&fileIO);
+	std::string err = tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath,&fileIO);
 
 	char sdfFileName[MAX_PATH_LEN];
 	sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");
@@ -117,20 +118,20 @@ int main(int argc, char* argv[])
 			int curTexcoords = shapeC->texcoords.size() / 2;
 
 			int faceCount = shape.mesh.indices.size();
-			int vertexCount = shape.mesh.positions.size();
+			int vertexCount = attribute.vertices.size();
 			for (int v = 0; v < vertexCount; v++)
 			{
-				shapeC->positions.push_back(shape.mesh.positions[v]);
+				shapeC->positions.push_back(attribute.vertices[v]);
 			}
-			int numNormals = int(shape.mesh.normals.size());
+			int numNormals = int(attribute.normals.size());
 			for (int vn = 0; vn < numNormals; vn++)
 			{
-				shapeC->normals.push_back(shape.mesh.normals[vn]);
+				shapeC->normals.push_back(attribute.normals[vn]);
 			}
-			int numTexCoords = int(shape.mesh.texcoords.size());
+			int numTexCoords = int(attribute.texcoords.size());
 			for (int vt = 0; vt < numTexCoords; vt++)
 			{
-				shapeC->texcoords.push_back(shape.mesh.texcoords[vt]);
+				shapeC->texcoords.push_back(attribute.texcoords[vt]);
 			}
 
 			for (int face = 0; face < faceCount; face += 3)
@@ -140,9 +141,9 @@ int main(int argc, char* argv[])
 					continue;
 				}
 
-				shapeC->indices.push_back(shape.mesh.indices[face] + curPositions);
-				shapeC->indices.push_back(shape.mesh.indices[face + 1] + curPositions);
-				shapeC->indices.push_back(shape.mesh.indices[face + 2] + curPositions);
+				shapeC->indices.push_back(shape.mesh.indices[face].vertex_index + curPositions);
+				shapeC->indices.push_back(shape.mesh.indices[face + 1].vertex_index + curPositions);
+				shapeC->indices.push_back(shape.mesh.indices[face + 2].vertex_index + curPositions);
 			}
 		}
 	}
@@ -329,7 +330,7 @@ int main(int argc, char* argv[])
 			}
 
 			int faceCount = shape.mesh.indices.size();
-			int vertexCount = shape.mesh.positions.size();
+			int vertexCount = attribute.vertices.size();
 			tinyobj::material_t mat = shape.material;
 			if (shape.name.length())
 			{
@@ -339,7 +340,7 @@ int main(int argc, char* argv[])
 			}
 			for (int v = 0; v < vertexCount / 3; v++)
 			{
-				fprintf(f, "v %f %f %f\n", shape.mesh.positions[v * 3 + 0], shape.mesh.positions[v * 3 + 1], shape.mesh.positions[v * 3 + 2]);
+				fprintf(f, "v %f %f %f\n", attribute.vertices[v * 3 + 0], attribute.vertices[v * 3 + 1], attribute.vertices[v * 3 + 2]);
 			}
 
 			if (mat.name.length())
@@ -352,18 +353,18 @@ int main(int argc, char* argv[])
 			}
 
 			fprintf(f, "\n");
-			int numNormals = int(shape.mesh.normals.size());
+			int numNormals = int(attribute.normals.size());
 
 			for (int vn = 0; vn < numNormals / 3; vn++)
 			{
-				fprintf(f, "vn %f %f %f\n", shape.mesh.normals[vn * 3 + 0], shape.mesh.normals[vn * 3 + 1], shape.mesh.normals[vn * 3 + 2]);
+				fprintf(f, "vn %f %f %f\n", attribute.normals[vn * 3 + 0], attribute.normals[vn * 3 + 1], attribute.normals[vn * 3 + 2]);
 			}
 
 			fprintf(f, "\n");
-			int numTexCoords = int(shape.mesh.texcoords.size());
+			int numTexCoords = int(attribute.texcoords.size());
 			for (int vt = 0; vt < numTexCoords / 2; vt++)
 			{
-				fprintf(f, "vt %f %f\n", shape.mesh.texcoords[vt * 2 + 0], shape.mesh.texcoords[vt * 2 + 1]);
+				fprintf(f, "vt %f %f\n", attribute.texcoords[vt * 2 + 0], attribute.texcoords[vt * 2 + 1]);
 			}
 
 			fprintf(f, "s off\n");
@@ -375,9 +376,9 @@ int main(int argc, char* argv[])
 					continue;
 				}
 				fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n",
-						shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1,
-						shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1,
-						shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1);
+						shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1,
+						shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1,
+						shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1);
 			}
 			fclose(f);
 
@@ -437,4 +438,4 @@ int main(int argc, char* argv[])
 	fclose(sdfFile);
 
 	return 0;
-}
\ No newline at end of file
+}
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index 1918a9003..ba9efac19 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -1,4 +1,4 @@
-SUBDIRS( HelloWorld BasicDemo )
+SUBDIRS( HelloWorld BasicDemo)
 IF(BUILD_BULLET3)
 	SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint )
 ENDIF()
diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp
new file mode 100644
index 000000000..e947a5585
--- /dev/null
+++ b/examples/DeformableDemo/DeformableMultibody.cpp
@@ -0,0 +1,409 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+///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 "DeformableMultibody.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 DeformableMultibody demo deformable bodies self-collision
+static bool g_floatingBase = true;
+static float friction = 1.;
+class DeformableMultibody : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+public:
+	DeformableMultibody(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper)
+	{
+	}
+    
+	virtual ~DeformableMultibody()
+	{
+	}
+    
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 30;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -10, 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 DeformableMultibody::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());
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	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.));
+        //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 = false;
+        int numLinks = 4;
+        bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(.4, 1, .4);
+        btVector3 baseHalfExtents(.4, 1, .4);
+        
+        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.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+
+        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
+    {
+        btScalar h = 0;
+        const btScalar s = 4;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s),
+                                                         20,20,
+//                                                         3,3,
+                                                         1 + 2 + 4 + 8, true);
+
+        psb->getCollisionShape()->setMargin(0.25);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(5);
+        psb->setSpringStiffness(2);
+        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;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+    }
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableMultibody::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 DeformableMultibody::stepSimulation(float deltaTime)
+{
+//    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
+    m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
+}
+
+
+btMultiBody* DeformableMultibody::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 = .1f;
+    
+    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 = .1f;
+    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 DeformableMultibody::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* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DeformableMultibody(options.m_guiHelper);
+}
+
+
diff --git a/examples/DeformableDemo/DeformableMultibody.h b/examples/DeformableDemo/DeformableMultibody.h
new file mode 100644
index 000000000..acd9d421d
--- /dev/null
+++ b/examples/DeformableDemo/DeformableMultibody.h
@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _DEFORMABLE_MULTIBODY_H
+#define _DEFORMABLE_MULTIBODY_H
+
+class CommonExampleInterface* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_DEFORMABLE_MULTIBODY_H
diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp
new file mode 100644
index 000000000..403ce4907
--- /dev/null
+++ b/examples/DeformableDemo/DeformableRigid.cpp
@@ -0,0 +1,291 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+///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 "DeformableRigid.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 DeformableRigid 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).
+class DeformableRigid : public CommonRigidBodyBase
+{
+public:
+	DeformableRigid(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~DeformableRigid()
+	{
+	}
+	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 Ctor_RbUpStack(int count)
+    {
+        float mass = 0.2;
+        
+        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)),
+//            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, 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, 1.5, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(1, 1.5, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 1.5, 1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(-1, 1.5, -1));
+        createRigidBody(mass, startTransform, shape[0]);
+        startTransform.setOrigin(btVector3(0, 3.5, 0));
+        createRigidBody(mass, startTransform, shape[0]);
+    }
+    
+    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 DeformableRigid::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
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+    
+//    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, -32, 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(1);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    // create a piece of cloth
+    {
+        bool onGround = false;
+        const btScalar s = 4;
+        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,
+                                                          1 + 2 + 4 + 8, 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,
+                                                 2,2,
+                                                 0, true);
+        
+        psb->getCollisionShape()->setMargin(0.1);
+        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 = 1;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+        
+        // add a few rigid bodies
+        Ctor_RbUpStack(1);
+    }
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableRigid::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* DeformableRigidCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DeformableRigid(options.m_guiHelper);
+}
+
+
diff --git a/examples/DeformableDemo/DeformableRigid.h b/examples/DeformableDemo/DeformableRigid.h
new file mode 100644
index 000000000..0d0e0dc5e
--- /dev/null
+++ b/examples/DeformableDemo/DeformableRigid.h
@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _DEFORMABLE_RIGID_H
+#define _DEFORMABLE_RIGID_H
+
+class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_DEFORMABLE_RIGID_H
diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp
new file mode 100644
index 000000000..44e5f4e5f
--- /dev/null
+++ b/examples/DeformableDemo/Pinch.cpp
@@ -0,0 +1,397 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+///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 = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -0, 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 = 1e6;
+        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;
+    btScalar liftTime = 2.5;
+    btScalar shiftTime = 3.5;
+    btScalar holdTime = 4.5*1000;
+    btScalar dropTime = 5.3*1000;
+    btTransform rbTransform;
+    rbTransform.setIdentity();
+    btVector3 translation;
+    btVector3 velocity;
+    
+    btVector3 initialTranslationLeft = btVector3(0.5,3,4);
+    btVector3 initialTranslationRight = btVector3(0.5,3,-4);
+    btVector3 pinchVelocityLeft = btVector3(0,0,-2);
+    btVector3 pinchVelocityRight = btVector3(0,0,2);
+    btVector3 liftVelocity = btVector3(0,5,0);
+    btVector3 shiftVelocity = btVector3(0,0,5);
+    btVector3 holdVelocity = btVector3(0,0,0);
+    btVector3 openVelocityLeft = btVector3(0,0,4);
+    btVector3 openVelocityRight = btVector3(0,0,-4);
+    
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
+    }
+    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];
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
+    }
+    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(20);
+    rb1->setFriction(20);
+}
+
+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
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+    
+    getDeformableDynamicsWorld()->setSolverCallback(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
+    {
+        btScalar verts[24] = {0.f, 0.f, 0.f,
+            1.f, 0.f, 0.f,
+            0.f, 1.f, 0.f,
+            0.f, 0.f, 1.f,
+            1.f, 1.f, 0.f,
+            0.f, 1.f, 1.f,
+            1.f, 0.f, 1.f,
+            1.f, 1.f, 1.f
+        };
+        int triangles[60] = {0, 6, 3,
+            0,1,6,
+            7,5,3,
+            7,3,6,
+            4,7,6,
+            4,6,1,
+            7,2,5,
+            7,4,2,
+            0,3,2,
+            2,3,5,
+            0,2,4,
+            0,4,1,
+            0,6,5,
+            0,6,4,
+            3,4,2,
+            3,4,7,
+            2,7,3,
+            2,7,1,
+            4,5,0,
+            4,5,6,
+        };
+//       btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
+////
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
+                                                                  TetraCube::getElements(),
+                                                                  0,
+                                                                  TetraCube::getNodes(),
+                                                                  false, true, true);
+        
+        psb->scale(btVector3(2, 2, 2));
+        psb->translate(btVector3(0, 4, 0));
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->setTotalMass(1);
+//        psb->scale(btVector3(5, 5, 5));
+//        psb->translate(btVector3(-2.5, 4, -2.5));
+//        psb->getCollisionShape()->setMargin(0.1);
+//        psb->setTotalMass(1);
+        psb->setSpringStiffness(2);
+        psb->setDampingCoefficient(0.02);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 2;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+        // 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/DeformableDemo/Pinch.h b/examples/DeformableDemo/Pinch.h
new file mode 100644
index 000000000..1616ec39a
--- /dev/null
+++ b/examples/DeformableDemo/Pinch.h
@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _PINCH_H
+#define _PINCH_H
+
+class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_PINCH_H
diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp
new file mode 100644
index 000000000..74d9678a8
--- /dev/null
+++ b/examples/DeformableDemo/VolumetricDeformable.cpp
@@ -0,0 +1,296 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+///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
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	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;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+    }
+    // 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/DeformableDemo/VolumetricDeformable.h b/examples/DeformableDemo/VolumetricDeformable.h
new file mode 100644
index 000000000..fe5d326a5
--- /dev/null
+++ b/examples/DeformableDemo/VolumetricDeformable.h
@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _VOLUMETRIC_DEFORMABLE_H
+#define _VOLUMETRIC_DEFORMABLE_H
+
+class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_VOLUMETRIC_DEFORMABLE__H
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index a04e9f9b4..19334b2db 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -359,6 +359,14 @@ SET(BulletExampleBrowser_SRCS
  ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
+  ../DeformableDemo/Pinch.cpp
+  ../DeformableDemo/Pinch.h
+  ../DeformableDemo/DeformableMultibody.cpp
+  ../DeformableDemo/DeformableMultibody.h
+  ../DeformableDemo/DeformableRigid.cpp
+  ../DeformableDemo/DeformableRigid.h
+  ../DeformableDemo/VolumetricDeformable.cpp
+  ../DeformableDemo/VolumetricDeformable.h
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index e80b4eee9..481d36c5d 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -47,6 +47,10 @@
 #include "../FractureDemo/FractureDemo.h"
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
+#include "../DeformableDemo/DeformableRigid.h"
+#include "../DeformableDemo/Pinch.h"
+#include "../DeformableDemo/DeformableMultibody.h"
+#include "../DeformableDemo/VolumetricDeformable.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -117,7 +121,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Basic Example", "Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
 
 		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(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),
 
@@ -190,6 +194,13 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc, TUT_SPHERE_PLANE_BULLET2),
 //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3),
 
+        ExampleEntry(0, "Deformabe Body"),
+        ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc),
+        ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
+        ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
+        ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
+        // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
+        
 #ifdef INCLUDE_CLOTH_DEMOS
 		ExampleEntry(0, "Soft Body"),
 		ExampleEntry(1, "Cloth", "Simulate a patch of cloth.", SoftDemoCreateFunc, 0),
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 58b67a3bf..a0b8f1587 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -182,6 +182,7 @@ project "App_BulletExampleBrowser"
 		"../RenderingExamples/*",
 		"../VoronoiFracture/*",
 		"../SoftDemo/*",
+		"../DeformableDemo/*",
 		"../RollingFrictionDemo/*",
 		"../rbdl/*",
 		"../FractureDemo/*",
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index d630f8599..f868bcb13 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -2265,7 +2265,7 @@ int BulletMJCFImporter::getBodyUniqueId() const
 	return m_data->m_activeBodyUniqueId;
 }
 
-static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
+static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
 {
 	btCompoundShape* compound = new btCompoundShape();
 	compound->setMargin(collisionMargin);
@@ -2278,25 +2278,26 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::sha
 		btConvexHullShape* convexHull = new btConvexHullShape();
 		convexHull->setMargin(collisionMargin);
 		tinyobj::shape_t& shape = shapes[s];
+
 		int faceCount = shape.mesh.indices.size();
 
 		for (int f = 0; f < faceCount; f += 3)
 		{
 			btVector3 pt;
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 2]);
 
 			convexHull->addPoint(pt * geomScale, false);
 
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
 			convexHull->addPoint(pt * geomScale, false);
 
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
 			convexHull->addPoint(pt * geomScale, false);
 		}
 
@@ -2391,10 +2392,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 							else
 							{
 								std::vector<tinyobj::shape_t> shapes;
-								std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
+								tinyobj::attrib_t attribute;
+								std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
 								//create a convex hull for each shape, and store it in a btCompoundShape
 
-								childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
+								childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
 							}
 							break;
 						}
diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
index b4e02243d..6f13d86b2 100644
--- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
@@ -65,13 +65,14 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 		btVector3 shift(0, 0, 0);
 
 		std::vector<tinyobj::shape_t> shapes;
+		tinyobj::attrib_t attribute;
 		{
 			B3_PROFILE("tinyobj::LoadObj");
-			std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO);
+			std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
 			//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
 		}
 
-		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
 		{
 			B3_PROFILE("Load Texture");
 			//int textureIndex = -1;
@@ -84,7 +85,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 				meshData.m_rgbaColor[2] = shape.material.diffuse[2];
 				meshData.m_rgbaColor[3] = shape.material.transparency;
 				meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR;
-				
+
 				meshData.m_specularColor[0] = shape.material.specular[0];
 				meshData.m_specularColor[1] = shape.material.specular[1];
 				meshData.m_specularColor[2] = shape.material.specular[2];
diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
index 566682d0a..e5bd6633c 100644
--- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
@@ -12,6 +12,7 @@ struct CachedObjResult
 {
 	std::string m_msg;
 	std::vector<tinyobj::shape_t> m_shapes;
+	tinyobj::attrib_t m_attribute;
 };
 
 static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
@@ -31,24 +32,26 @@ void b3EnableFileCaching(int enable)
 }
 
 std::string LoadFromCachedOrFromObj(
+	tinyobj::attrib_t& attribute,
 	std::vector<tinyobj::shape_t>& shapes,  // [output]
 	const char* filename,
 	const char* mtl_basepath,
-	struct CommonFileIOInterface* fileIO
-	)
+	struct CommonFileIOInterface* fileIO)
 {
 	CachedObjResult* resultPtr = gCachedObjResults[filename];
 	if (resultPtr)
 	{
 		const CachedObjResult& result = *resultPtr;
 		shapes = result.m_shapes;
+		attribute = result.m_attribute;
 		return result.m_msg;
 	}
 
-	std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO);
+	std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
 	CachedObjResult result;
 	result.m_msg = err;
 	result.m_shapes = shapes;
+	result.m_attribute = attribute;
 	if (gEnableFileCaching)
 	{
 		gCachedObjResults.insert(filename, result);
@@ -60,14 +63,15 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const cha
 {
 	B3_PROFILE("LoadMeshFromObj");
 	std::vector<tinyobj::shape_t> shapes;
+	tinyobj::attrib_t attribute;
 	{
 		B3_PROFILE("tinyobj::LoadObj2");
-		std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO);
+		std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
 	}
 
 	{
 		B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj");
-		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes);
+		GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes);
 		return gfxShape;
 	}
 }
diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
index 9b62074c1..54bc66312 100644
--- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
+++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
@@ -8,8 +8,8 @@ struct GLInstanceGraphicsShape;
 int b3IsFileCachingEnabled();
 void b3EnableFileCaching(int enable);
 
-
 std::string LoadFromCachedOrFromObj(
+	tinyobj::attrib_t& attribute,
 	std::vector<tinyobj::shape_t>& shapes,  // [output]
 	const char* filename,
 	const char* mtl_basepath,
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
index ead2769a0..08fa521ae 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
@@ -9,7 +9,7 @@
 #include "../../OpenGLWindow/GLInstancingRenderer.h"
 #include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
 
-GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading)
+GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading)
 {
 	b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
 	{
@@ -36,19 +36,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
 					}
 
 					GLInstanceVertex vtx0;
-					vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 0];
-					vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 1];
-					vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f] * 3 + 2];
+					tinyobj::index_t v_0 = shape.mesh.indices[f];
+					vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index];
+					vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1];
+					vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
 					vtx0.xyzw[3] = 0.f;
 
-					if (shape.mesh.texcoords.size())
+					if (attribute.texcoords.size())
 					{
-						int uv0Index = shape.mesh.indices[f] * 2 + 0;
-						int uv1Index = shape.mesh.indices[f] * 2 + 1;
-						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())))
+						int uv0Index = 2 * v_0.texcoord_index;
+						int uv1Index = 2 * v_0.texcoord_index + 1;
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())))
 						{
-							vtx0.uv[0] = shape.mesh.texcoords[uv0Index];
-							vtx0.uv[1] = shape.mesh.texcoords[uv1Index];
+							vtx0.uv[0] = attribute.texcoords[uv0Index];
+							vtx0.uv[1] = attribute.texcoords[uv1Index];
 						}
 						else
 						{
@@ -64,19 +65,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
 					}
 
 					GLInstanceVertex vtx1;
-					vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0];
-					vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1];
-					vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2];
+					tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+					vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index];
+					vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1];
+					vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
 					vtx1.xyzw[3] = 0.f;
 
-					if (shape.mesh.texcoords.size())
+					if (attribute.texcoords.size())
 					{
-						int uv0Index = shape.mesh.indices[f + 1] * 2 + 0;
-						int uv1Index = shape.mesh.indices[f + 1] * 2 + 1;
-						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+						int uv0Index = 2 * v_1.texcoord_index;
+						int uv1Index = 2 * v_1.texcoord_index + 1;
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
 						{
-							vtx1.uv[0] = shape.mesh.texcoords[uv0Index];
-							vtx1.uv[1] = shape.mesh.texcoords[uv1Index];
+							vtx1.uv[0] = attribute.texcoords[uv0Index];
+							vtx1.uv[1] = attribute.texcoords[uv1Index];
 						}
 						else
 						{
@@ -92,18 +94,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
 					}
 
 					GLInstanceVertex vtx2;
-					vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0];
-					vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1];
-					vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2];
+					tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+					vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index];
+					vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1];
+					vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];
 					vtx2.xyzw[3] = 0.f;
-					if (shape.mesh.texcoords.size())
+					if (attribute.texcoords.size())
 					{
-						int uv0Index = shape.mesh.indices[f + 2] * 2 + 0;
-						int uv1Index = shape.mesh.indices[f + 2] * 2 + 1;
-						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))
+						int uv0Index = 2 * v_2.texcoord_index;
+						int uv1Index = 2 * v_2.texcoord_index + 1;
+
+						if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))
 						{
-							vtx2.uv[0] = shape.mesh.texcoords[uv0Index];
-							vtx2.uv[1] = shape.mesh.texcoords[uv1Index];
+							vtx2.uv[0] = attribute.texcoords[uv0Index];
+							vtx2.uv[1] = attribute.texcoords[uv1Index];
 						}
 						else
 						{
@@ -123,16 +127,21 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
 					btVector3 v2(vtx2.xyzw[0], vtx2.xyzw[1], vtx2.xyzw[2]);
 
 					unsigned int maxIndex = 0;
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 0);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 1);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f] * 3 + 2);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 0);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 1);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 1] * 3 + 2);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 0);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 1);
-					maxIndex = b3Max(maxIndex, shape.mesh.indices[f + 2] * 3 + 2);
-					bool hasNormals = (shape.mesh.normals.size() && maxIndex < shape.mesh.normals.size());
+					unsigned n0Index = shape.mesh.indices[f].normal_index;
+					unsigned n1Index = shape.mesh.indices[f + 1].normal_index;
+					unsigned n2Index = shape.mesh.indices[f + 2].normal_index;
+
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n0Index + 2);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n1Index + 2);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 0);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 1);
+					maxIndex = b3Max(maxIndex, 3 * n2Index + 2);
+
+					bool hasNormals = (attribute.normals.size() && maxIndex < attribute.normals.size());
 
 					if (flatShading || !hasNormals)
 					{
@@ -159,15 +168,15 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tiny
 					}
 					else
 					{
-						vtx0.normal[0] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 0];
-						vtx0.normal[1] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 1];
-						vtx0.normal[2] = shape.mesh.normals[shape.mesh.indices[f] * 3 + 2];  //shape.mesh.indices[f+1]*3+0
-						vtx1.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 0];
-						vtx1.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 1];
-						vtx1.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 1] * 3 + 2];
-						vtx2.normal[0] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 0];
-						vtx2.normal[1] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 1];
-						vtx2.normal[2] = shape.mesh.normals[shape.mesh.indices[f + 2] * 3 + 2];
+						vtx0.normal[0] = attribute.normals[3 * n0Index+ 0];
+						vtx0.normal[1] = attribute.normals[3 * n0Index+ 1];
+						vtx0.normal[2] = attribute.normals[3 * n0Index+ 2];
+						vtx1.normal[0] = attribute.normals[3 * n1Index+ 0];
+						vtx1.normal[1] = attribute.normals[3 * n1Index+ 1];
+						vtx1.normal[2] = attribute.normals[3 * n1Index+ 2];
+						vtx2.normal[0] = attribute.normals[3 * n2Index+ 0];
+						vtx2.normal[1] = attribute.normals[3 * n2Index+ 1];
+						vtx2.normal[2] = attribute.normals[3 * n2Index+ 2];
 					}
 					vertices->push_back(vtx0);
 					vertices->push_back(vtx1);
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
index 2840a85d7..4054b4dab 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
@@ -4,6 +4,6 @@
 #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
 #include <vector>
 
-struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
+struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
 
 #endif  //WAVEFRONT2GRAPHICS_H
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index f295458a8..16c85d0b5 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -509,7 +509,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
 	return true;
 }
 
-static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
+static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
 {
 	B3_PROFILE("createConvexHullFromShapes");
 	btCompoundShape* compound = new btCompoundShape();
@@ -528,20 +528,20 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
 		for (int f = 0; f < faceCount; f += 3)
 		{
 			btVector3 pt;
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
 
 			convexHull->addPoint(pt * geomScale, false);
 
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
 			convexHull->addPoint(pt * geomScale, false);
 
-			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
-						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
-						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+			pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
+						attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
+						attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
 			convexHull->addPoint(pt * geomScale, false);
 		}
 
@@ -558,8 +558,6 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
 	return compound;
 }
 
-
-
 int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
 {
 	UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape);
@@ -718,10 +716,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 					else
 					{
 						std::vector<tinyobj::shape_t> shapes;
-						std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
+						tinyobj::attrib_t attribute;
+						std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
 						//create a convex hull for each shape, and store it in a btCompoundShape
-
-						shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
+						shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
 						m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
 						return shape;
 					}
diff --git a/examples/MultiBodyBaseline/MultiBodyBaseline.cpp b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp
new file mode 100644
index 000000000..663c2d33a
--- /dev/null
+++ b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp
@@ -0,0 +1,358 @@
+/*
+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 "MultiBodyBaseline.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 MultiBodyBaseline demo deformable bodies self-collision
+static bool g_floatingBase = true;
+static float friction = 1.;
+class MultiBodyBaseline : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+    btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+public:
+	MultiBodyBaseline(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper)
+	{
+	}
+    
+	virtual ~MultiBodyBaseline()
+	{
+	}
+    
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 30;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -10, 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);
+
+};
+
+void MultiBodyBaseline::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();
+    btMultiBodyConstraintSolver* sol;
+    sol = new btMultiBodyConstraintSolver;
+	m_solver = sol;
+    
+    btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
+    m_dynamicsWorld = world;
+    //    m_dynamicsWorld->setDebugDrawer(&gDebugDraw);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+
+    {
+        ///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.));
+        //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 = false;
+        int numLinks = 4;
+        bool spherical = false;  //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.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+
+        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);
+    }
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void MultiBodyBaseline::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 MultiBodyBaseline::stepSimulation(float deltaTime)
+{
+//    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
+    m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
+}
+
+
+btMultiBody* MultiBodyBaseline::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 = .1f;
+    
+    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 = .1f;
+    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 MultiBodyBaseline::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* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options)
+{
+	return new MultiBodyBaseline(options.m_guiHelper);
+}
+
+
diff --git a/examples/MultiBodyBaseline/MultiBodyBaseline.h b/examples/MultiBodyBaseline/MultiBodyBaseline.h
new file mode 100644
index 000000000..35734c63c
--- /dev/null
+++ b/examples/MultiBodyBaseline/MultiBodyBaseline.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 _MULTIBODY_BASELINE_H
+#define _MULTIBODY_BASELINE_H
+
+class CommonExampleInterface*  MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_MULTIBODY_BASELINE_H
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index f6b48a8ce..d9db25737 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -4861,7 +4861,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						else
 						{
 							std::vector<tinyobj::shape_t> shapes;
-							std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
+							tinyobj::attrib_t attribute;
+							std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
 
 							//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
 							//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
@@ -4882,20 +4883,20 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 								for (int f = 0; f < faceCount; f += 3)
 								{
 									btVector3 pt;
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
-												shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
-												shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+									pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0],
+												attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1],
+												attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]);
 
 									convexHull->addPoint(pt * meshScale, false);
 
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
-												shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
-												shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+									pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0],
+												attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1],
+												attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]);
 									convexHull->addPoint(pt * meshScale, false);
 
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
-												shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
-												shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+									pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0],
+												attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1],
+												attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]);
 									convexHull->addPoint(pt * meshScale, false);
 								}
 
@@ -7958,8 +7959,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
 	bool hasStatus = true;
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
-	double scale = 0.1;
-	double mass = 0.1;
+	double scale = 1;
+	double mass = 1;
 	double collisionMargin = 0.02;
 	const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
 	if (m_data->m_verboseOutput)
@@ -8012,23 +8013,24 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		std::string out_found_filename;
 		int out_type;
 
-		bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
+		bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
 		std::vector<tinyobj::shape_t> shapes;
-		std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO);
+		tinyobj::attrib_t attribute;
+		std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
 		if (!shapes.empty())
 		{
 			const tinyobj::shape_t& shape = shapes[0];
 			btAlignedObjectArray<btScalar> vertices;
 			btAlignedObjectArray<int> indices;
-			for (int i = 0; i < shape.mesh.positions.size(); i++)
+			for (int i = 0; i < attribute.vertices.size(); i++)
 			{
-				vertices.push_back(shape.mesh.positions[i]);
+				vertices.push_back(attribute.vertices[i]);
 			}
 			for (int i = 0; i < shape.mesh.indices.size(); i++)
 			{
-				indices.push_back(shape.mesh.indices[i]);
+				indices.push_back(shape.mesh.indices[i].vertex_index);
 			}
-			int numTris = indices.size() / 3;
+			int numTris = shape.mesh.indices.size() / 3;
 			if (numTris > 0)
 			{
 				btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
@@ -8041,9 +8043,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 				//turn on softbody vs softbody collision
 				psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
 				psb->randomizeConstraints();
+				psb->scale(btVector3(scale, scale, scale));
 				psb->rotate(initialOrn);
 				psb->translate(initialPos);
-				psb->scale(btVector3(scale, scale, scale));
 
 				psb->setTotalMass(mass, true);
 				psb->getCollisionShape()->setMargin(collisionMargin);
diff --git a/examples/ThirdPartyLibs/Wavefront/README.md b/examples/ThirdPartyLibs/Wavefront/README.md
index 6f911e523..de226acd3 100644
--- a/examples/ThirdPartyLibs/Wavefront/README.md
+++ b/examples/ThirdPartyLibs/Wavefront/README.md
@@ -38,51 +38,102 @@ Licensed under 2 clause BSD.
 
 Usage
 -----
+Data format
+attrib_t contains single and linear array of vertex data(position, normal and texcoord).
 
-    std::string inputfile = "cornell_box.obj";
-    std::vector<tinyobj::shape_t> shapes;
-  
-    std::string err = tinyobj::LoadObj(shapes, inputfile.c_str());
-  
-    if (!err.empty()) {
-      std::cerr << err << std::endl;
-      exit(1);
-    }
-  
-    std::cout << "# of shapes : " << shapes.size() << std::endl;
-  
-    for (size_t i = 0; i < shapes.size(); i++) {
-      printf("shape[%ld].name = %s\n", i, shapes[i].name.c_str());
-      printf("shape[%ld].indices: %ld\n", i, shapes[i].mesh.indices.size());
-      assert((shapes[i].mesh.indices.size() % 3) == 0);
-      for (size_t f = 0; f < shapes[i].mesh.indices.size(); f++) {
-        printf("  idx[%ld] = %d\n", f, shapes[i].mesh.indices[f]);
-      }
-  
-      printf("shape[%ld].vertices: %ld\n", i, shapes[i].mesh.positions.size());
-      assert((shapes[i].mesh.positions.size() % 3) == 0);
-      for (size_t v = 0; v < shapes[i].mesh.positions.size() / 3; v++) {
-        printf("  v[%ld] = (%f, %f, %f)\n", v,
-          shapes[i].mesh.positions[3*v+0],
-          shapes[i].mesh.positions[3*v+1],
-          shapes[i].mesh.positions[3*v+2]);
-      }
-    
-      printf("shape[%ld].material.name = %s\n", i, shapes[i].material.name.c_str());
-      printf("  material.Ka = (%f, %f ,%f)\n", shapes[i].material.ambient[0], shapes[i].material.ambient[1], shapes[i].material.ambient[2]);
-      printf("  material.Kd = (%f, %f ,%f)\n", shapes[i].material.diffuse[0], shapes[i].material.diffuse[1], shapes[i].material.diffuse[2]);
-      printf("  material.Ks = (%f, %f ,%f)\n", shapes[i].material.specular[0], shapes[i].material.specular[1], shapes[i].material.specular[2]);
-      printf("  material.Tr = (%f, %f ,%f)\n", shapes[i].material.transmittance[0], shapes[i].material.transmittance[1], shapes[i].material.transmittance[2]);
-      printf("  material.Ke = (%f, %f ,%f)\n", shapes[i].material.emission[0], shapes[i].material.emission[1], shapes[i].material.emission[2]);
-      printf("  material.Ns = %f\n", shapes[i].material.shininess);
-      printf("  material.map_Ka = %s\n", shapes[i].material.ambient_texname.c_str());
-      printf("  material.map_Kd = %s\n", shapes[i].material.diffuse_texname.c_str());
-      printf("  material.map_Ks = %s\n", shapes[i].material.specular_texname.c_str());
-      printf("  material.map_Ns = %s\n", shapes[i].material.normal_texname.c_str());
-      std::map<std::string, std::string>::iterator it(shapes[i].material.unknown_parameter.begin());
-      std::map<std::string, std::string>::iterator itEnd(shapes[i].material.unknown_parameter.end());
-      for (; it != itEnd; it++) {
-        printf("  material.%s = %s\n", it->first.c_str(), it->second.c_str());
-      }
-      printf("\n");
+attrib_t::vertices => 3 floats per vertex
+
+       v[0]        v[1]        v[2]        v[3]               v[n-1]
+  +-----------+-----------+-----------+-----------+      +-----------+
+  | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+  +-----------+-----------+-----------+-----------+      +-----------+
+
+attrib_t::normals => 3 floats per vertex
+
+       n[0]        n[1]        n[2]        n[3]               n[n-1]
+  +-----------+-----------+-----------+-----------+      +-----------+
+  | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+  +-----------+-----------+-----------+-----------+      +-----------+
+
+attrib_t::texcoords => 2 floats per vertex
+
+       t[0]        t[1]        t[2]        t[3]               t[n-1]
+  +-----------+-----------+-----------+-----------+      +-----------+
+  |  u  |  v  |  u  |  v  |  u  |  v  |  u  |  v  | .... |  u  |  v  |
+  +-----------+-----------+-----------+-----------+      +-----------+
+
+attrib_t::colors => 3 floats per vertex(vertex color. optional)
+
+       c[0]        c[1]        c[2]        c[3]               c[n-1]
+  +-----------+-----------+-----------+-----------+      +-----------+
+  | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z |
+  +-----------+-----------+-----------+-----------+      +-----------+
+
+Each shape_t::mesh_t does not contain vertex data but contains array index to attrib_t. See loader_example.cc for more details.
+
+
+mesh_t::indices => array of vertex indices.
+
+  +----+----+----+----+----+----+----+----+----+----+     +--------+
+  | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-1) |
+  +----+----+----+----+----+----+----+----+----+----+     +--------+
+
+Each index has an array index to attrib_t::vertices, attrib_t::normals and attrib_t::texcoords.
+
+mesh_t::num_face_vertices => array of the number of vertices per face(e.g. 3 = triangle, 4 = quad , 5 or more = N-gons).
+
+
+  +---+---+---+        +---+
+  | 3 | 4 | 3 | ...... | 3 |
+  +---+---+---+        +---+
+    |   |   |            |
+    |   |   |            +-----------------------------------------+
+    |   |   |                                                      |
+    |   |   +------------------------------+                       |
+    |   |                                  |                       |
+    |   +------------------+               |                       |
+    |                      |               |                       |
+    |/                     |/              |/                      |/
+
+ mesh_t::indices
+
+  |    face[0]   |       face[1]     |    face[2]   |     |      face[n-1]           |
+  +----+----+----+----+----+----+----+----+----+----+     +--------+--------+--------+
+  | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-3) | i(n-2) | i(n-1) |
+  +----+----+----+----+----+----+----+----+----+----+     +--------+--------+--------+
+
+```c++
+#define TINYOBJLOADER_IMPLEMENTATION // define this in only *one* .cc
+#include "tiny_obj_loader.h"
+
+std::string inputfile = "cornell_box.obj";
+tinyobj::attrib_t attrib;
+std::vector<tinyobj::shape_t> shapes;
+
+LoadObj(attrib, shapes, inputfile.c_str());
+
+// Loop over shapes
+for (size_t s = 0; s < shapes.size(); s++) {
+  // Loop over faces(polygon)
+  size_t index_offset = 0;
+  for (size_t f = 0; f < shapes[s].mesh.indices.size(); f++) {
+    int fv = 3;
+    // Loop over vertices in the face.
+    for (size_t v = 0; v < fv; v++) {
+      // access to vertex
+      tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v];
+      tinyobj::real_t vx = attrib.vertices[3*idx.vertex_index+0];
+      tinyobj::real_t vy = attrib.vertices[3*idx.vertex_index+1];
+      tinyobj::real_t vz = attrib.vertices[3*idx.vertex_index+2];
+      tinyobj::real_t nx = attrib.normals[3*idx.normal_index+0];
+      tinyobj::real_t ny = attrib.normals[3*idx.normal_index+1];
+      tinyobj::real_t nz = attrib.normals[3*idx.normal_index+2];
+      tinyobj::real_t tx = attrib.texcoords[2*idx.texcoord_index+0];
+      tinyobj::real_t ty = attrib.texcoords[2*idx.texcoord_index+1];
     }
+    index_offset += fv;
+  }
+}
+
+```
+
diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
index 9cd2f2092..d1b9c6362 100644
--- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
@@ -70,25 +70,6 @@ std::istream& safeGetline(std::istream& is, std::string& t)
 	}
 }
 #endif
-struct vertex_index
-{
-	int v_idx, vt_idx, vn_idx, dummy;
-};
-struct MyIndices
-{
-	int m_offset;
-	int m_numIndices;
-};
-
-// for std::map
-static inline bool operator<(const vertex_index& a, const vertex_index& b)
-{
-	if (a.v_idx != b.v_idx) return (a.v_idx < b.v_idx);
-	if (a.vn_idx != b.vn_idx) return (a.vn_idx < b.vn_idx);
-	if (a.vt_idx != b.vt_idx) return (a.vt_idx < b.vt_idx);
-
-	return false;
-}
 
 static inline bool isSpace(const char c)
 {
@@ -101,25 +82,33 @@ static inline bool isNewLine(const char c)
 }
 
 // Make index zero-base, and also support relative index.
-static inline int fixIndex(int idx, int n)
+static inline bool fixIndex(int idx, int n, int* ret)
 {
-	int i;
+	if (!ret)
+	{
+		return false;
+	}
 
 	if (idx > 0)
 	{
-		i = idx - 1;
+		(*ret) = idx - 1;
+		return true;
 	}
-	else if (idx == 0)
+
+	if (idx == 0)
 	{
-		i = 0;
+		// zero is not allowed according to the spec.
+		return false;
 	}
-	else
-	{  // negative value = relative
-		i = n + idx;
+
+	if (idx < 0)
+	{
+		(*ret) = n + idx;  // negative value = relative
+		return true;
 	}
-	return i;
-}
 
+	return false;  // never reach here.
+}
 static inline std::string parseString(const char*& token)
 {
 	std::string s;
@@ -156,170 +145,184 @@ static inline void parseFloat3(
 	z = parseFloat(token);
 }
 
-// Parse triples: i, i/j/k, i//k, i/j
-static vertex_index parseTriple(
-	const char*& token,
-	int vsize,
-	int vnsize,
-	int vtsize)
+// Parse triples with index offsets: i, i/j/k, i//k, i/j
+static bool parseTriple(const char** token, int vsize, int vnsize, int vtsize,
+						vertex_index_t* ret)
 {
-	vertex_index vi;
-	vi.vn_idx = -1;
-	vi.vt_idx = -1;
-	vi.v_idx = -1;
-
-	vi.v_idx = fixIndex(atoi(token), vsize);
-	token += strcspn(token, "/ \t\r");
-	if (token[0] != '/')
+	if (!ret)
 	{
-		return vi;
+		return false;
 	}
-	token++;
 
-	// i//k
-	if (token[0] == '/')
+	vertex_index_t vi(-1);
+
+	if (!fixIndex(atoi((*token)), vsize, &(vi.v_idx)))
 	{
-		token++;
-		vi.vn_idx = fixIndex(atoi(token), vnsize);
-		token += strcspn(token, "/ \t\r");
-		return vi;
+		return false;
 	}
 
-	// i/j/k or i/j
-	vi.vt_idx = fixIndex(atoi(token), vtsize);
-	token += strcspn(token, "/ \t\r");
-	if (token[0] != '/')
+	(*token) += strcspn((*token), "/ \t\r");
+	if ((*token)[0] != '/')
 	{
-		return vi;
+		(*ret) = vi;
+		return true;
 	}
+	(*token)++;
 
-	// i/j/k
-	token++;  // skip '/'
-	vi.vn_idx = fixIndex(atoi(token), vnsize);
-	token += strcspn(token, "/ \t\r");
-	return vi;
-}
-
-static unsigned int
-updateVertex(
-	std::map<vertex_index, unsigned int>& vertexCache,
-	std::vector<float>& positions,
-	std::vector<float>& normals,
-	std::vector<float>& texcoords,
-	const std::vector<float>& in_positions,
-	const std::vector<float>& in_normals,
-	const std::vector<float>& in_texcoords,
-	const vertex_index& i)
-{
-	const std::map<vertex_index, unsigned int>::iterator it = vertexCache.find(i);
-
-	if (it != vertexCache.end())
+	// i//k
+	if ((*token)[0] == '/')
 	{
-		// found cache
-		return it->second;
+		(*token)++;
+		if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx)))
+		{
+			return false;
+		}
+		(*token) += strcspn((*token), "/ \t\r");
+		(*ret) = vi;
+		return true;
 	}
 
-	assert(static_cast<int>(in_positions.size()) > (3 * i.v_idx + 2));
-
-	positions.push_back(in_positions[3 * i.v_idx + 0]);
-	positions.push_back(in_positions[3 * i.v_idx + 1]);
-	positions.push_back(in_positions[3 * i.v_idx + 2]);
-
-	if (i.vn_idx >= 0 && ((3 * i.vn_idx + 2) < in_normals.size()))
+	// i/j/k or i/j
+	if (!fixIndex(atoi((*token)), vtsize, &(vi.vt_idx)))
 	{
-		normals.push_back(in_normals[3 * i.vn_idx + 0]);
-		normals.push_back(in_normals[3 * i.vn_idx + 1]);
-		normals.push_back(in_normals[3 * i.vn_idx + 2]);
+		return false;
 	}
 
-	if (i.vt_idx >= 0)
+	(*token) += strcspn((*token), "/ \t\r");
+	if ((*token)[0] != '/')
 	{
-		int numTexCoords = in_texcoords.size();
-		int index0 = 2 * i.vt_idx + 0;
-		int index1 = 2 * i.vt_idx + 1;
+		(*ret) = vi;
+		return true;
+	}
 
-		if (index0 >= 0 && (index0) < numTexCoords)
-		{
-			texcoords.push_back(in_texcoords[index0]);
-		}
-		if (index1 >= 0 && (index1) < numTexCoords)
-		{
-			texcoords.push_back(in_texcoords[index1]);
-		}
+	// i/j/k
+	(*token)++;  // skip '/'
+	if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx)))
+	{
+		return false;
 	}
+	(*token) += strcspn((*token), "/ \t\r");
 
-	unsigned int idx = positions.size() / 3 - 1;
-	vertexCache[i] = idx;
+	(*ret) = vi;
 
-	return idx;
+	return true;
 }
 
-static bool
-exportFaceGroupToShape(
-	shape_t& shape,
-	const std::vector<float>& in_positions,
-	const std::vector<float>& in_normals,
-	const std::vector<float>& in_texcoords,
-	const std::vector<MyIndices>& faceGroup,
-	const material_t material,
-	const std::string name,
-	std::vector<vertex_index>& allIndices)
+static bool exportFaceGroupToShape(shape_t* shape, const std::vector<face_t>& face_group,
+								   const material_t material, const std::string& name,
+								   const std::vector<float>& v)
 {
-	if (faceGroup.empty())
+	if (face_group.empty())
 	{
 		return false;
 	}
 
+	shape->name = name;
 	// Flattened version of vertex data
-	std::vector<float> positions;
-	std::vector<float> normals;
-	std::vector<float> texcoords;
-	std::map<vertex_index, unsigned int> vertexCache;
-	std::vector<unsigned int> indices;
 
 	// Flatten vertices and indices
-	for (size_t i = 0; i < faceGroup.size(); i++)
+	for (size_t i = 0; i < face_group.size(); i++)
 	{
-		const MyIndices& face = faceGroup[i];
+		const face_t& face = face_group[i];
+		size_t npolys = face.size();
+
+		if (npolys < 3)
+		{
+			// Face must have 3+ vertices.
+			continue;
+		}
+		vertex_index_t i0 = face[0];
+		vertex_index_t i1(-1);
+		vertex_index_t i2 = face[1];
 
-		vertex_index i0 = allIndices[face.m_offset];
-		vertex_index i1;
-		i1.vn_idx = -1;
-		i1.vt_idx = -1;
-		i1.v_idx = -1;
-		vertex_index i2 = allIndices[face.m_offset + 1];
+		face_t remainingFace = face;  // copy
+		size_t guess_vert = 0;
+		vertex_index_t ind[3];
 
-		size_t npolys = face.m_numIndices;  //.size();
+		// How many iterations can we do without decreasing the remaining
+		// vertices.
+		size_t remainingIterations = face.size();
+		size_t previousRemainingVertices = remainingFace.size();
 
+		while (remainingFace.size() > 3 && remainingIterations > 0)
 		{
-			// Polygon -> triangle fan conversion
-			for (size_t k = 2; k < npolys; k++)
+			npolys = remainingFace.size();
+			if (guess_vert >= npolys)
 			{
-				i1 = i2;
-				i2 = allIndices[face.m_offset + k];
-
-				unsigned int v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0);
-				unsigned int v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1);
-				unsigned int v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2);
+				guess_vert -= npolys;
+			}
 
-				indices.push_back(v0);
-				indices.push_back(v1);
-				indices.push_back(v2);
+			if (previousRemainingVertices != npolys)
+			{
+				// The number of remaining vertices decreased. Reset counters.
+				previousRemainingVertices = npolys;
+				remainingIterations = npolys;
+			}
+			else
+			{
+				// We didn't consume a vertex on previous iteration, reduce the
+				// available iterations.
+				remainingIterations--;
 			}
-		}
-	}
 
-	//
-	// Construct shape.
-	//
-	shape.name = name;
-	shape.mesh.positions.swap(positions);
-	shape.mesh.normals.swap(normals);
-	shape.mesh.texcoords.swap(texcoords);
-	shape.mesh.indices.swap(indices);
+			for (size_t k = 0; k < 3; k++)
+			{
+				ind[k] = remainingFace[(guess_vert + k) % npolys];
+				size_t vi = size_t(ind[k].v_idx);
+			}
+			// this triangle is an ear
+			{
+				index_t idx0, idx1, idx2;
+				idx0.vertex_index = ind[0].v_idx;
+				idx0.normal_index = ind[0].vn_idx;
+				idx0.texcoord_index = ind[0].vt_idx;
+				idx1.vertex_index = ind[1].v_idx;
+				idx1.normal_index = ind[1].vn_idx;
+				idx1.texcoord_index = ind[1].vt_idx;
+				idx2.vertex_index = ind[2].v_idx;
+				idx2.normal_index = ind[2].vn_idx;
+				idx2.texcoord_index = ind[2].vt_idx;
+
+				shape->mesh.indices.push_back(idx0);
+				shape->mesh.indices.push_back(idx1);
+				shape->mesh.indices.push_back(idx2);
+			}
 
-	shape.material = material;
+			// remove v1 from the list
+			size_t removed_vert_index = (guess_vert + 1) % npolys;
+			while (removed_vert_index + 1 < npolys)
+			{
+				remainingFace[removed_vert_index] =
+					remainingFace[removed_vert_index + 1];
+				removed_vert_index += 1;
+			}
+			remainingFace.pop_back();
+		}
 
+		if (remainingFace.size() == 3)
+		{
+			i0 = remainingFace[0];
+			i1 = remainingFace[1];
+			i2 = remainingFace[2];
+			{
+				index_t idx0, idx1, idx2;
+				idx0.vertex_index = i0.v_idx;
+				idx0.normal_index = i0.vn_idx;
+				idx0.texcoord_index = i0.vt_idx;
+				idx1.vertex_index = i1.v_idx;
+				idx1.normal_index = i1.vn_idx;
+				idx1.texcoord_index = i1.vt_idx;
+				idx2.vertex_index = i2.v_idx;
+				idx2.normal_index = i2.vn_idx;
+				idx2.texcoord_index = i2.vt_idx;
+
+				shape->mesh.indices.push_back(idx0);
+				shape->mesh.indices.push_back(idx1);
+				shape->mesh.indices.push_back(idx2);
+			}
+		}
+	}
+	shape->material = material;
 	return true;
 }
 
@@ -329,7 +332,6 @@ void InitMaterial(material_t& material)
 	material.ambient_texname = "";
 	material.diffuse_texname = "";
 	material.specular_texname = "";
-	material.normal_texname = "";
 	for (int i = 0; i < 3; i++)
 	{
 		material.ambient[i] = 0.f;
@@ -369,8 +371,8 @@ std::string LoadMtl(
 		return err.str();
 	}
 #else
-	int fileHandle = fileIO->fileOpen(filepath.c_str(),"r");
-	if (fileHandle<0)
+	int fileHandle = fileIO->fileOpen(filepath.c_str(), "r");
+	if (fileHandle < 0)
 	{
 		err << "Cannot open file [" << filepath << "]" << std::endl;
 		return err.str();
@@ -396,7 +398,7 @@ std::string LoadMtl(
 		line = fileIO->readLine(fileHandle, tmpBuf, 1024);
 		if (line)
 		{
-			linebuf=line;
+			linebuf = line;
 		}
 #endif
 
@@ -420,7 +422,7 @@ std::string LoadMtl(
 		// Skip leading space.
 		const char* token = linebuf.c_str();
 		token += strspn(token, " \t");
-		
+
 		assert(token);
 		if (token[0] == '\0') continue;  // empty line
 
@@ -574,12 +576,13 @@ std::string LoadMtl(
 		}
 	}
 #ifndef USE_STREAM
-	while (line);
+	while (line)
+		;
 #endif
 	// flush last material.
 	material_map.insert(std::pair<std::string, material_t>(material.name, material));
 
-	if (fileHandle>=0)
+	if (fileHandle >= 0)
 	{
 		fileIO->fileClose(fileHandle);
 	}
@@ -588,11 +591,16 @@ std::string LoadMtl(
 
 std::string
 LoadObj(
+	attrib_t& attrib,
 	std::vector<shape_t>& shapes,
 	const char* filename,
 	const char* mtl_basepath,
 	CommonFileIOInterface* fileIO)
 {
+	attrib.vertices.clear();
+	attrib.normals.clear();
+	attrib.texcoords.clear();
+	shapes.clear();
 	std::string tmp = filename;
 	if (!mtl_basepath)
 	{
@@ -604,13 +612,6 @@ LoadObj(
 		mtl_basepath = tmp.c_str();
 		//fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename);
 	}
-
-	shapes.resize(0);
-	std::vector<vertex_index> allIndices;
-	allIndices.reserve(1024 * 1024);
-
-	MyIndices face;
-
 	std::stringstream err;
 #ifdef USE_STREAM
 	std::ifstream ifs(filename);
@@ -620,8 +621,8 @@ LoadObj(
 		return err.str();
 	}
 #else
-	int fileHandle = fileIO->fileOpen(filename,"r");
-	if (fileHandle<0)
+	int fileHandle = fileIO->fileOpen(filename, "r");
+	if (fileHandle < 0)
 	{
 		err << "Cannot open file [" << filename << "]" << std::endl;
 		return err.str();
@@ -629,16 +630,15 @@ LoadObj(
 #endif
 
 	std::vector<float> v;
-	v.reserve(1024 * 1024);
 	std::vector<float> vn;
-	vn.reserve(1024 * 1024);
 	std::vector<float> vt;
-	vt.reserve(1024 * 1024);
-	//std::vector<std::vector<vertex_index> > faceGroup;
-	std::vector<MyIndices> faceGroup;
-	faceGroup.reserve(1024 * 1024);
 	std::string name;
 
+	int greatest_v_idx = -1;
+	int greatest_vn_idx = -1;
+	int greatest_vt_idx = -1;
+
+	std::vector<face_t> faceGroup;
 	// material
 	std::map<std::string, material_t> material_map;
 	material_t material;
@@ -664,10 +664,9 @@ LoadObj(
 		line = fileIO->readLine(fileHandle, tmpBuf, 1024);
 		if (line)
 		{
-			linebuf=line;
+			linebuf = line;
 		}
 #endif
-
 		// Trim newline '\r\n' or '\r'
 		if (linebuf.size() > 0)
 		{
@@ -734,23 +733,34 @@ LoadObj(
 			token += 2;
 			token += strspn(token, " \t");
 
-			face.m_offset = allIndices.size();
-			face.m_numIndices = 0;
+			face_t face;
+
+			face.reserve(3);
 
 			while (!isNewLine(token[0]))
 			{
-				vertex_index vi = parseTriple(token, v.size() / 3, vn.size() / 3, vt.size() / 2);
-				allIndices.push_back(vi);
-				face.m_numIndices++;
-				int n = strspn(token, " \t\r");
+				vertex_index_t vi;
+				if (!parseTriple(&token, static_cast<int>(v.size() / 3),
+								 static_cast<int>(vn.size() / 3),
+								 static_cast<int>(vt.size() / 2), &vi))
+				{
+					err << "Failed parse `f' line(e.g. zero value for face index.";
+					return err.str();
+				}
+
+				greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx;
+				greatest_vn_idx =
+					greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx;
+				greatest_vt_idx =
+					greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx;
+
+				face.push_back(vi);
+				size_t n = strspn(token, " \t\r");
 				token += n;
 			}
-
 			faceGroup.push_back(face);
-
 			continue;
 		}
-
 		// use mtl
 		if ((0 == strncmp(token, "usemtl", 6)) && isSpace((token[6])))
 		{
@@ -777,10 +787,10 @@ LoadObj(
 			token += 7;
 			sscanf(token, "%s", namebuf);
 
-			std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO);
+			std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath, fileIO);
 			if (!err_mtl.empty())
 			{
-				//faceGroup.resize(0);  // for safety
+				//face_group.resize(0);  // for safety
 				//return err_mtl;
 			}
 			continue;
@@ -791,7 +801,7 @@ LoadObj(
 		{
 			// flush previous face group.
 			shape_t shape;
-			bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
+			bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
 			if (ret)
 			{
 				shapes.push_back(shape);
@@ -827,7 +837,7 @@ LoadObj(
 		{
 			// flush previous face group.
 			shape_t shape;
-			bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
+			bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
 			if (ret)
 			{
 				shapes.push_back(shape);
@@ -847,18 +857,23 @@ LoadObj(
 		// Ignore unknown command.
 	}
 #ifndef USE_STREAM
-	while (line);
+	while (line)
+		;
 #endif
 
 	shape_t shape;
-	bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices);
+	bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v);
 	if (ret)
 	{
 		shapes.push_back(shape);
 	}
 	faceGroup.resize(0);  // for safety
 
-	if (fileHandle>=0)
+	attrib.vertices.swap(v);
+	attrib.normals.swap(vn);
+	attrib.texcoords.swap(vt);
+
+	if (fileHandle >= 0)
 	{
 		fileIO->fileClose(fileHandle);
 	}
diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
index 0319b7c0d..1fb15b34a 100644
--- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
@@ -14,6 +14,17 @@ struct CommonFileIOInterface;
 
 namespace tinyobj
 {
+struct vertex_index_t
+{
+	int v_idx, vt_idx, vn_idx;
+	vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {}
+	explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {}
+	vertex_index_t(int vidx, int vtidx, int vnidx)
+		: v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {}
+};
+
+typedef std::vector<vertex_index_t> face_t;
+
 typedef struct
 {
 	std::string name;
@@ -24,21 +35,27 @@ typedef struct
 	float transmittance[3];
 	float emission[3];
 	float shininess;
-	float transparency;
+	float transparency;  // 1 == opaque; 0 == fully transparent
 
-	std::string ambient_texname;
-	std::string diffuse_texname;
-	std::string specular_texname;
+	std::string ambient_texname;   // map_Ka
+	std::string diffuse_texname;   // map_Kd
+	std::string specular_texname;  // map_Ks
 	std::string normal_texname;
 	std::map<std::string, std::string> unknown_parameter;
 } material_t;
 
+// Index struct to support different indices for vtx/normal/texcoord.
+// -1 means not used.
 typedef struct
 {
-	std::vector<float> positions;
-	std::vector<float> normals;
-	std::vector<float> texcoords;
-	std::vector<unsigned int> indices;
+	int vertex_index;
+	int normal_index;
+	int texcoord_index;
+} index_t;
+
+typedef struct
+{
+	std::vector<index_t> indices;
 } mesh_t;
 
 typedef struct
@@ -48,6 +65,14 @@ typedef struct
 	mesh_t mesh;
 } shape_t;
 
+// Vertex attributes
+struct attrib_t
+{
+	std::vector<float> vertices;   // 'v'(xyz)
+	std::vector<float> normals;    // 'vn'
+	std::vector<float> texcoords;  // 'vt'(uv)
+	attrib_t() {}
+};
 /// Loads .obj from a file.
 /// 'shapes' will be filled with parsed shape data
 /// The function returns error string.
@@ -55,12 +80,14 @@ typedef struct
 /// 'mtl_basepath' is optional, and used for base path for .mtl file.
 #ifdef USE_STREAM
 std::string LoadObj(
+	attrib_t& attrib,
 	std::vector<shape_t>& shapes,  // [output]
 	const char* filename,
 	const char* mtl_basepath = NULL);
 #else
 std::string
 LoadObj(
+	attrib_t& attrib,
 	std::vector<shape_t>& shapes,
 	const char* filename,
 	const char* mtl_basepath,
diff --git a/src/.DS_Store b/src/.DS_Store
new file mode 100644
index 000000000..ee616d543
--- /dev/null
+++ b/src/.DS_Store
diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index e5097ccbb..57e1bb494 100644
--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 				//				printf("error in island management\n");
 			}
 
-			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
 				if (colObj0->getActivationState() == ACTIVE_TAG ||
@@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
@@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
 
 				if (colObj0->getIslandTag() == islandId)
 				{
diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 7fe961921..73607c61f 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -17,7 +17,6 @@ subject to the following restrictions:
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
 #include "btDynamicsWorld.h"
-
 class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
@@ -26,6 +25,7 @@ class btTypedConstraint;
 class btActionInterface;
 class btPersistentManifold;
 class btIDebugDraw;
+
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
@@ -76,7 +76,7 @@ protected:
 
 	virtual void calculateSimulationIslands();
 
-	virtual void solveConstraints(btContactSolverInfo & solverInfo);
+	
 
 	virtual void updateActivationState(btScalar timeStep);
 
@@ -95,7 +95,7 @@ protected:
 	void serializeRigidBodies(btSerializer * serializer);
 
 	void serializeDynamicsWorldInfo(btSerializer * serializer);
-
+    
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
@@ -107,6 +107,8 @@ public:
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
 	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
+    virtual void solveConstraints(btContactSolverInfo & solverInfo);
+    
 	virtual void synchronizeMotionStates();
 
 	///this can be useful to synchronize a single rigid body -> graphics object
@@ -227,6 +229,16 @@ public:
 	{
 		return m_latencyMotionStateInterpolation;
 	}
+    
+    btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
+    {
+        return m_nonStaticRigidBodies;
+    }
+    
+    const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
+    {
+        return m_nonStaticRigidBodies;
+    }
 };
 
 #endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index eadd8c12e..3c55234a8 100644
--- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -34,7 +34,8 @@ enum btDynamicsWorldType
 	BT_CONTINUOUS_DYNAMICS_WORLD = 3,
 	BT_SOFT_RIGID_DYNAMICS_WORLD = 4,
 	BT_GPU_DYNAMICS_WORLD = 5,
-	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6
+	BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6,
+    BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 3e210d752..4d634b699 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -100,6 +100,8 @@ btMultiBody::btMultiBody(int n_links,
 	  m_baseName(0),
 	  m_basePos(0, 0, 0),
 	  m_baseQuat(0, 0, 0, 1),
+      m_basePos_interpolate(0, 0, 0),
+      m_baseQuat_interpolate(0, 0, 0, 1),
 	  m_baseMass(mass),
 	  m_baseInertia(inertia),
 
@@ -449,6 +451,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
 	return m_links[i].m_cachedRotParentToThis;
 }
 
+const btVector3 &btMultiBody::getInterpolateRVector(int i) const
+{
+    return m_links[i].m_cachedRVector_interpolate;
+}
+
+const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
+{
+    return m_links[i].m_cachedRotParentToThis_interpolate;
+}
+
 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
 {
 	btAssert(i >= -1);
@@ -1581,6 +1593,158 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 	//printf("]\n");
 	/////////////////
 }
+void btMultiBody::predictPositionsMultiDof(btScalar dt)
+{
+    int num_links = getNumLinks();
+    // step position by adding dt * velocity
+    //btVector3 v = getBaseVel();
+    //m_basePos += dt * v;
+    //
+    btScalar *pBasePos;
+    btScalar *pBaseVel = &m_realBuf[3];  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
+    // reset to current position
+    for (int i = 0; i < 3; ++i)
+    {
+        m_basePos_interpolate[i] = m_basePos[i];
+    }
+    pBasePos = m_basePos_interpolate;
+    
+    pBasePos[0] += dt * pBaseVel[0];
+    pBasePos[1] += dt * pBaseVel[1];
+    pBasePos[2] += dt * pBaseVel[2];
+    
+    ///////////////////////////////
+    //local functor for quaternion integration (to avoid error prone redundancy)
+    struct
+    {
+        //"exponential map" based on btTransformUtil::integrateTransform(..)
+        void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
+        {
+            //baseBody    =>    quat is alias and omega is global coor
+            //!baseBody    =>    quat is alibi and omega is local coor
+            
+            btVector3 axis;
+            btVector3 angvel;
+            
+            if (!baseBody)
+                angvel = quatRotate(quat, omega);  //if quat is not m_baseQuat, it is alibi => ok
+            else
+                angvel = omega;
+            
+            btScalar fAngle = angvel.length();
+            //limit the angular motion
+            if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
+            {
+                fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
+            }
+            
+            if (fAngle < btScalar(0.001))
+            {
+                // use Taylor's expansions of sync function
+                axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
+            }
+            else
+            {
+                // sync(fAngle) = sin(c*fAngle)/t
+                axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
+            }
+            
+            if (!baseBody)
+                quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
+            else
+                quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
+            //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
+            
+            quat.normalize();
+        }
+    } pQuatUpdateFun;
+    ///////////////////////////////
+    
+    //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
+    //
+    btScalar *pBaseQuat;
+
+    // reset to current orientation
+    for (int i = 0; i < 4; ++i)
+    {
+        m_baseQuat_interpolate[i] = m_baseQuat[i];
+    }
+    pBaseQuat = m_baseQuat_interpolate;
+
+    btScalar *pBaseOmega = &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
+    //
+    btQuaternion baseQuat;
+    baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
+    btVector3 baseOmega;
+    baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
+    pQuatUpdateFun(baseOmega, baseQuat, true, dt);
+    pBaseQuat[0] = baseQuat.x();
+    pBaseQuat[1] = baseQuat.y();
+    pBaseQuat[2] = baseQuat.z();
+    pBaseQuat[3] = baseQuat.w();
+
+    // Finally we can update m_jointPos for each of the m_links
+    for (int i = 0; i < num_links; ++i)
+    {
+        btScalar *pJointPos;
+        pJointPos = &m_links[i].m_jointPos_interpolate[0];
+        
+        btScalar *pJointVel = getJointVelMultiDof(i);
+        
+        switch (m_links[i].m_jointType)
+        {
+            case btMultibodyLink::ePrismatic:
+            case btMultibodyLink::eRevolute:
+            {
+                //reset to current pos
+                pJointPos[0] = m_links[i].m_jointPos[0];
+                btScalar jointVel = pJointVel[0];
+                pJointPos[0] += dt * jointVel;
+                break;
+            }
+            case btMultibodyLink::eSpherical:
+            {
+                //reset to current pos
+
+                for (int i = 0; i < 4; ++i)
+                {
+                    pJointPos[i] = m_links[i].m_jointPos[i];
+                }
+                
+                btVector3 jointVel;
+                jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
+                btQuaternion jointOri;
+                jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
+                pQuatUpdateFun(jointVel, jointOri, false, dt);
+                pJointPos[0] = jointOri.x();
+                pJointPos[1] = jointOri.y();
+                pJointPos[2] = jointOri.z();
+                pJointPos[3] = jointOri.w();
+                break;
+            }
+            case btMultibodyLink::ePlanar:
+            {
+                for (int i = 0; i < 3; ++i)
+                {
+                    pJointPos[i] = m_links[i].m_jointPos[i];
+                }
+                pJointPos[0] += dt * getJointVelMultiDof(i)[0];
+                
+                btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
+                btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
+                pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
+                pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
+                break;
+            }
+            default:
+            {
+            }
+        }
+        
+        m_links[i].updateInterpolationCacheMultiDof();
+    }
+}
 
 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
 {
@@ -1589,9 +1753,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	//btVector3 v = getBaseVel();
 	//m_basePos += dt * v;
 	//
-	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
-	btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
-	//
+    btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
+    btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);  //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
+    
 	pBasePos[0] += dt * pBaseVel[0];
 	pBasePos[1] += dt * pBaseVel[1];
 	pBasePos[2] += dt * pBaseVel[2];
@@ -1645,7 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 
 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
 	//
-	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
+    btScalar *pBaseQuat = pq ? pq : m_baseQuat;
 	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];  //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
 	//
 	btQuaternion baseQuat;
@@ -1670,7 +1834,9 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 	// Finally we can update m_jointPos for each of the m_links
 	for (int i = 0; i < num_links; ++i)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
+        btScalar *pJointPos;
+        pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
+        
 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
 
 		switch (m_links[i].m_jointType)
@@ -1678,12 +1844,14 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
 			case btMultibodyLink::ePrismatic:
 			case btMultibodyLink::eRevolute:
 			{
+                //reset to current pos
 				btScalar jointVel = pJointVel[0];
 				pJointPos[0] += dt * jointVel;
 				break;
 			}
 			case btMultibodyLink::eSpherical:
 			{
+                //reset to current pos
 				btVector3 jointVel;
 				jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
 				btQuaternion jointOri;
@@ -2006,6 +2174,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
 	}
 }
 
+void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
+{
+    world_to_local.resize(getNumLinks() + 1);
+    local_origin.resize(getNumLinks() + 1);
+    
+    world_to_local[0] = getInterpolateWorldToBaseRot();
+    local_origin[0] = getInterpolateBasePos();
+    
+    if (getBaseCollider())
+    {
+        btVector3 posr = local_origin[0];
+        //    float pos[4]={posr.x(),posr.y(),posr.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()};
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
+        getBaseCollider()->setInterpolationWorldTransform(tr);
+    }
+    
+    for (int k = 0; k < getNumLinks(); k++)
+    {
+        const int parent = getParent(k);
+        world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
+        local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
+    }
+    
+    for (int m = 0; m < getNumLinks(); m++)
+    {
+        btMultiBodyLinkCollider *col = getLink(m).m_collider;
+        if (col)
+        {
+            int link = col->m_link;
+            btAssert(link == m);
+            
+            int index = link + 1;
+            
+            btVector3 posr = local_origin[index];
+            //            float pos[4]={posr.x(),posr.y(),posr.z(),1};
+            btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            
+            col->setInterpolationWorldTransform(tr);
+        }
+    }
+}
+
 int btMultiBody::calculateSerializeBufferSize() const
 {
 	int sz = sizeof(btMultiBodyData);
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index c0b0d003b..91b5c3edb 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -193,12 +193,24 @@ public:
 	const btQuaternion &getWorldToBaseRot() const
 	{
 		return m_baseQuat;
-	}                                                                                               // rotates world vectors into base frame
+	}
+    
+    const btVector3 &getInterpolateBasePos() const
+    {
+        return m_basePos_interpolate;
+    }  // in world frame
+    const btQuaternion &getInterpolateWorldToBaseRot() const
+    {
+        return m_baseQuat_interpolate;
+    }
+    
+    // rotates world vectors into base frame
 	btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); }  // in world frame
 
 	void setBasePos(const btVector3 &pos)
 	{
 		m_basePos = pos;
+        m_basePos_interpolate = pos;
 	}
 
 	void setBaseWorldTransform(const btTransform &tr)
@@ -224,6 +236,7 @@ public:
 	void setWorldToBaseRot(const btQuaternion &rot)
 	{
 		m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
+        m_baseQuat_interpolate = rot;
 	}
 	void setBaseOmega(const btVector3 &omega)
 	{
@@ -273,6 +286,8 @@ public:
 
 	const btVector3 &getRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
 	const btQuaternion &getParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
+    const btVector3 &getInterpolateRVector(int i) const;              // vector from COM(parent(i)) to COM(i), in frame i's coords
+    const btQuaternion &getInterpolateParentToLocalRot(int i) const;  // rotates vectors in frame parent(i) to vectors in frame i.
 
 	//
 	// transform vectors in local frame of link i to world frame (or vice versa)
@@ -421,6 +436,9 @@ public:
 
 	// timestep the positions (given current velocities).
 	void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
+    
+    // predict the positions
+    void predictPositionsMultiDof(btScalar dt);
 
 	//
 	// contacts
@@ -581,6 +599,7 @@ public:
 	void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
 
 	void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
+    void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
 
 	virtual int calculateSerializeBufferSize() const;
 
@@ -664,7 +683,9 @@ private:
 	const char *m_baseName;                   //memory needs to be manager by user!
 
 	btVector3 m_basePos;      // position of COM of base (world frame)
+    btVector3 m_basePos_interpolate;      // position of interpolated COM of base (world frame)
 	btQuaternion m_baseQuat;  // rotates world points into base frame
+    btQuaternion m_baseQuat_interpolate;  
 
 	btScalar m_baseMass;      // mass of the base
 	btVector3 m_baseInertia;  // inertia of the base (in local frame; diagonal)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 1131e5378..1be76ad86 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 	m_multiBodies.remove(body);
 }
 
+void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    predictMultiBodyTransforms(timeStep);
+    
+}
 void btMultiBodyDynamicsWorld::calculateSimulationIslands()
 {
 	BT_PROFILE("calculateSimulationIslands");
@@ -421,292 +427,19 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 }
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
-	forwardKinematics();
-
-	BT_PROFILE("solveConstraints");
-
-	clearMultiBodyConstraintForces();
-
-	m_sortedConstraints.resize(m_constraints.size());
-	int i;
-	for (i = 0; i < getNumConstraints(); i++)
-	{
-		m_sortedConstraints[i] = m_constraints[i];
-	}
-	m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
-	btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
-
-	m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
-	for (i = 0; i < m_multiBodyConstraints.size(); i++)
-	{
-		m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
-	}
-	m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
-
-	btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
-
-	m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
-
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-	{
-		BT_PROFILE("btMultiBody addForce");
-		for (int i = 0; i < this->m_multiBodies.size(); i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			}
-			for (int b = 0; b < bod->getNumLinks(); b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-					isSleeping = true;
-			}
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-				m_scratch_v.resize(bod->getNumLinks() + 1);
-				m_scratch_m.resize(bod->getNumLinks() + 1);
-
-				bod->addBaseForce(m_gravity * bod->getBaseMass());
-
-				for (int j = 0; j < bod->getNumLinks(); ++j)
-				{
-					bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
-				}
-			}  //if (!isSleeping)
-		}
-	}
-#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-
-	{
-		BT_PROFILE("btMultiBody stepVelocities");
-		for (int i = 0; i < this->m_multiBodies.size(); i++)
-		{
-			btMultiBody* bod = m_multiBodies[i];
-
-			bool isSleeping = false;
-
-			if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
-			{
-				isSleeping = true;
-			}
-			for (int b = 0; b < bod->getNumLinks(); b++)
-			{
-				if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
-					isSleeping = true;
-			}
-
-			if (!isSleeping)
-			{
-				//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
-				m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
-				m_scratch_v.resize(bod->getNumLinks() + 1);
-				m_scratch_m.resize(bod->getNumLinks() + 1);
-				bool doNotUpdatePos = false;
-                bool isConstraintPass = false;
-				{
-					if (!bod->isUsingRK4Integration())
-					{
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
-						m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
-						getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-					}
-					else
-					{
-						//
-						int numDofs = bod->getNumDofs() + 6;
-						int numPosVars = bod->getNumPosVars() + 7;
-						btAlignedObjectArray<btScalar> scratch_r2;
-						scratch_r2.resize(2 * numPosVars + 8 * numDofs);
-						//convenience
-						btScalar* pMem = &scratch_r2[0];
-						btScalar* scratch_q0 = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qx = pMem;
-						pMem += numPosVars;
-						btScalar* scratch_qd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qd3 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd0 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd1 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd2 = pMem;
-						pMem += numDofs;
-						btScalar* scratch_qdd3 = pMem;
-						pMem += numDofs;
-						btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
-
-						/////
-						//copy q0 to scratch_q0 and qd0 to scratch_qd0
-						scratch_q0[0] = bod->getWorldToBaseRot().x();
-						scratch_q0[1] = bod->getWorldToBaseRot().y();
-						scratch_q0[2] = bod->getWorldToBaseRot().z();
-						scratch_q0[3] = bod->getWorldToBaseRot().w();
-						scratch_q0[4] = bod->getBasePos().x();
-						scratch_q0[5] = bod->getBasePos().y();
-						scratch_q0[6] = bod->getBasePos().z();
-						//
-						for (int link = 0; link < bod->getNumLinks(); ++link)
-						{
-							for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
-								scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
-						}
-						//
-						for (int dof = 0; dof < numDofs; ++dof)
-							scratch_qd0[dof] = bod->getVelocityVector()[dof];
-						////
-						struct
-						{
-							btMultiBody* bod;
-							btScalar *scratch_qx, *scratch_q0;
-
-							void operator()()
-							{
-								for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
-									scratch_qx[dof] = scratch_q0[dof];
-							}
-						} pResetQx = {bod, scratch_qx, scratch_q0};
-						//
-						struct
-						{
-							void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pVal[i] = pCurVal[i] + dt * pDer[i];
-							}
-
-						} pEulerIntegrate;
-						//
-						struct
-						{
-							void operator()(btMultiBody* pBody, const btScalar* pData)
-							{
-								btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
-
-								for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
-									pVel[i] = pData[i];
-							}
-						} pCopyToVelocityVector;
-						//
-						struct
-						{
-							void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
-							{
-								for (int i = 0; i < size; ++i)
-									pDst[i] = pSrc[start + i];
-							}
-						} pCopy;
-						//
-
-						btScalar h = solverInfo.m_timeStep;
-#define output &m_scratch_r[bod->getNumDofs()]
-						//calc qdd0 from: q0 & qd0
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd0, 0, numDofs);
-						//calc q1 = q0 + h/2 * qd0
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
-						//calc qd1 = qd0 + h/2 * qdd0
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
-						//
-						//calc qdd1 from: q1 & qd1
-						pCopyToVelocityVector(bod, scratch_qd1);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd1, 0, numDofs);
-						//calc q2 = q0 + h/2 * qd1
-						pResetQx();
-						bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
-						//calc qd2 = qd0 + h/2 * qdd1
-						pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
-						//
-						//calc qdd2 from: q2 & qd2
-						pCopyToVelocityVector(bod, scratch_qd2);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd2, 0, numDofs);
-						//calc q3 = q0 + h * qd2
-						pResetQx();
-						bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
-						//calc qd3 = qd0 + h * qdd2
-						pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
-						//
-						//calc qdd3 from: q3 & qd3
-						pCopyToVelocityVector(bod, scratch_qd3);
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
-						isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
-						pCopy(output, scratch_qdd3, 0, numDofs);
-
-						//
-						//calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
-						//calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
-						btAlignedObjectArray<btScalar> delta_q;
-						delta_q.resize(numDofs);
-						btAlignedObjectArray<btScalar> delta_qd;
-						delta_qd.resize(numDofs);
-						for (int i = 0; i < numDofs; ++i)
-						{
-							delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
-							delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
-							//delta_q[i] = h*scratch_qd0[i];
-							//delta_qd[i] = h*scratch_qdd0[i];
-						}
-						//
-						pCopyToVelocityVector(bod, scratch_qd0);
-						bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
-						//
-						if (!doNotUpdatePos)
-						{
-							btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
-							pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
-
-							for (int i = 0; i < numDofs; ++i)
-								pRealBuf[i] = delta_q[i];
-
-							//bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
-							bod->setPosUpdated(true);
-						}
-
-						//ugly hack which resets the cached data to t0 (needed for constraint solver)
-						{
-							for (int link = 0; link < bod->getNumLinks(); ++link)
-								bod->getLink(link).updateCacheMultiDof();
-							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
-							isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
-							getSolverInfo().m_jointFeedbackInJointFrame);
-						}
-					}
-				}
+    solveExternalForces(solverInfo);
+    buildIslands();
+    solveInternalConstraints(solverInfo);
+}
 
-#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-				bod->clearForcesAndTorques();
-#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
-			}  //if (!isSleeping)
-		}
-	}
+void btMultiBodyDynamicsWorld::buildIslands()
+{
+    m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
+}
 
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
 	/// solve all the constraints for this island
-	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
-
 	m_solverMultiBodyIslandCallback->processConstraints();
 
 	m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
@@ -760,11 +493,306 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 	}
 }
 
+void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo)
+{
+    forwardKinematics();
+    
+    BT_PROFILE("solveConstraints");
+    
+    clearMultiBodyConstraintForces();
+    
+    m_sortedConstraints.resize(m_constraints.size());
+    int i;
+    for (i = 0; i < getNumConstraints(); i++)
+    {
+        m_sortedConstraints[i] = m_constraints[i];
+    }
+    m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
+    btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
+    
+    m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
+    for (i = 0; i < m_multiBodyConstraints.size(); i++)
+    {
+        m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
+    }
+    m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
+    
+    btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
+    
+    m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
+    m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
+    
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    {
+        BT_PROFILE("btMultiBody addForce");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                
+                bod->addBaseForce(m_gravity * bod->getBaseMass());
+                
+                for (int j = 0; j < bod->getNumLinks(); ++j)
+                {
+                    bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
+                }
+            }  //if (!isSleeping)
+        }
+    }
+#endif  //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+    
+    {
+        BT_PROFILE("btMultiBody stepVelocities");
+        for (int i = 0; i < this->m_multiBodies.size(); i++)
+        {
+            btMultiBody* bod = m_multiBodies[i];
+            
+            bool isSleeping = false;
+            
+            if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+            {
+                isSleeping = true;
+            }
+            for (int b = 0; b < bod->getNumLinks(); b++)
+            {
+                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                    isSleeping = true;
+            }
+            
+            if (!isSleeping)
+            {
+                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
+                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
+                m_scratch_v.resize(bod->getNumLinks() + 1);
+                m_scratch_m.resize(bod->getNumLinks() + 1);
+                bool doNotUpdatePos = false;
+                bool isConstraintPass = false;
+                {
+                    if (!bod->isUsingRK4Integration())
+                    {
+                        const btScalar linearDamp = bod->getLinearDamping();
+//                        const btScalar angularDamp = bod->getAngularDamping();
+                        bod->setLinearDamping(0);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep,
+                                                                                  m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass,
+                                                                                  getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        bod->setLinearDamping(linearDamp);
+//                        bod->setAngularDamping(angularDamp);
+                    }
+                    else
+                    {
+                        //
+                        int numDofs = bod->getNumDofs() + 6;
+                        int numPosVars = bod->getNumPosVars() + 7;
+                        btAlignedObjectArray<btScalar> scratch_r2;
+                        scratch_r2.resize(2 * numPosVars + 8 * numDofs);
+                        //convenience
+                        btScalar* pMem = &scratch_r2[0];
+                        btScalar* scratch_q0 = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qx = pMem;
+                        pMem += numPosVars;
+                        btScalar* scratch_qd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qd3 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd0 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd1 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd2 = pMem;
+                        pMem += numDofs;
+                        btScalar* scratch_qdd3 = pMem;
+                        pMem += numDofs;
+                        btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]);
+                        
+                        /////
+                        //copy q0 to scratch_q0 and qd0 to scratch_qd0
+                        scratch_q0[0] = bod->getWorldToBaseRot().x();
+                        scratch_q0[1] = bod->getWorldToBaseRot().y();
+                        scratch_q0[2] = bod->getWorldToBaseRot().z();
+                        scratch_q0[3] = bod->getWorldToBaseRot().w();
+                        scratch_q0[4] = bod->getBasePos().x();
+                        scratch_q0[5] = bod->getBasePos().y();
+                        scratch_q0[6] = bod->getBasePos().z();
+                        //
+                        for (int link = 0; link < bod->getNumLinks(); ++link)
+                        {
+                            for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof)
+                                scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof];
+                        }
+                        //
+                        for (int dof = 0; dof < numDofs; ++dof)
+                            scratch_qd0[dof] = bod->getVelocityVector()[dof];
+                        ////
+                        struct
+                        {
+                            btMultiBody* bod;
+                            btScalar *scratch_qx, *scratch_q0;
+                            
+                            void operator()()
+                            {
+                                for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof)
+                                    scratch_qx[dof] = scratch_q0[dof];
+                            }
+                        } pResetQx = {bod, scratch_qx, scratch_q0};
+                        //
+                        struct
+                        {
+                            void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pVal[i] = pCurVal[i] + dt * pDer[i];
+                            }
+                            
+                        } pEulerIntegrate;
+                        //
+                        struct
+                        {
+                            void operator()(btMultiBody* pBody, const btScalar* pData)
+                            {
+                                btScalar* pVel = const_cast<btScalar*>(pBody->getVelocityVector());
+                                
+                                for (int i = 0; i < pBody->getNumDofs() + 6; ++i)
+                                    pVel[i] = pData[i];
+                            }
+                        } pCopyToVelocityVector;
+                        //
+                        struct
+                        {
+                            void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size)
+                            {
+                                for (int i = 0; i < size; ++i)
+                                    pDst[i] = pSrc[start + i];
+                            }
+                        } pCopy;
+                        //
+                        
+                        btScalar h = solverInfo.m_timeStep;
+#define output &m_scratch_r[bod->getNumDofs()]
+                        //calc qdd0 from: q0 & qd0
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd0, 0, numDofs);
+                        //calc q1 = q0 + h/2 * qd0
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0);
+                        //calc qd1 = qd0 + h/2 * qdd0
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs);
+                        //
+                        //calc qdd1 from: q1 & qd1
+                        pCopyToVelocityVector(bod, scratch_qd1);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd1, 0, numDofs);
+                        //calc q2 = q0 + h/2 * qd1
+                        pResetQx();
+                        bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1);
+                        //calc qd2 = qd0 + h/2 * qdd1
+                        pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs);
+                        //
+                        //calc qdd2 from: q2 & qd2
+                        pCopyToVelocityVector(bod, scratch_qd2);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd2, 0, numDofs);
+                        //calc q3 = q0 + h * qd2
+                        pResetQx();
+                        bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2);
+                        //calc qd3 = qd0 + h * qdd2
+                        pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs);
+                        //
+                        //calc qdd3 from: q3 & qd3
+                        pCopyToVelocityVector(bod, scratch_qd3);
+                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                  isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                  getSolverInfo().m_jointFeedbackInJointFrame);
+                        pCopy(output, scratch_qdd3, 0, numDofs);
+                        
+                        //
+                        //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3)
+                        //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3)
+                        btAlignedObjectArray<btScalar> delta_q;
+                        delta_q.resize(numDofs);
+                        btAlignedObjectArray<btScalar> delta_qd;
+                        delta_qd.resize(numDofs);
+                        for (int i = 0; i < numDofs; ++i)
+                        {
+                            delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]);
+                            delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]);
+                            //delta_q[i] = h*scratch_qd0[i];
+                            //delta_qd[i] = h*scratch_qdd0[i];
+                        }
+                        //
+                        pCopyToVelocityVector(bod, scratch_qd0);
+                        bod->applyDeltaVeeMultiDof(&delta_qd[0], 1);
+                        //
+                        if (!doNotUpdatePos)
+                        {
+                            btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                            pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                            
+                            for (int i = 0; i < numDofs; ++i)
+                                pRealBuf[i] = delta_q[i];
+                            
+                            //bod->stepPositionsMultiDof(1, 0, &delta_q[0]);
+                            bod->setPosUpdated(true);
+                        }
+                        
+                        //ugly hack which resets the cached data to t0 (needed for constraint solver)
+                        {
+                            for (int link = 0; link < bod->getNumLinks(); ++link)
+                                bod->getLink(link).updateCacheMultiDof();
+                            bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m,
+                                                                                      isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace,
+                                                                                      getSolverInfo().m_jointFeedbackInJointFrame);
+                        }
+                    }
+                }
+                
+#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+                bod->clearForcesAndTorques();
+#endif         //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY
+            }  //if (!isSleeping)
+        }
+    }
+}
+
+
 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
 	btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    integrateMultiBodyTransforms(timeStep);
+}
 
-	{
+void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
+{
 		BT_PROFILE("btMultiBody stepPositions");
 		//integrate and update the Featherstone hierarchies
 
@@ -787,31 +815,61 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
+                if (!bod->isPosUpdated())
+                    bod->stepPositionsMultiDof(timeStep);
+                else
+                {
+                    btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
+                    pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
 
-				{
-					if (!bod->isPosUpdated())
-						bod->stepPositionsMultiDof(timeStep);
-					else
-					{
-						btScalar* pRealBuf = const_cast<btScalar*>(bod->getVelocityVector());
-						pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs();
+                    bod->stepPositionsMultiDof(1, 0, pRealBuf);
+                    bod->setPosUpdated(false);
+                }
 
-						bod->stepPositionsMultiDof(1, 0, pRealBuf);
-						bod->setPosUpdated(false);
-					}
-				}
 
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
-
-				bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+                bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
 			}
 			else
 			{
 				bod->clearVelocities();
 			}
 		}
-	}
+}
+
+void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep)
+{
+    BT_PROFILE("btMultiBody stepPositions");
+    //integrate and update the Featherstone hierarchies
+    
+    for (int b = 0; b < m_multiBodies.size(); b++)
+    {
+        btMultiBody* bod = m_multiBodies[b];
+        bool isSleeping = false;
+        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
+        {
+            isSleeping = true;
+        }
+        for (int b = 0; b < bod->getNumLinks(); b++)
+        {
+            if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
+                isSleeping = true;
+        }
+        
+        if (!isSleeping)
+        {
+            int nLinks = bod->getNumLinks();
+            bod->predictPositionsMultiDof(timeStep);
+            m_scratch_world_to_local.resize(nLinks + 1);
+            m_scratch_local_origin.resize(nLinks + 1);
+            bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+        }
+        else
+        {
+            bod->clearVelocities();
+        }
+    }
 }
 
 void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index e36c2f7aa..653ec36ca 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -47,7 +47,7 @@ protected:
 
 	virtual void calculateSimulationIslands();
 	virtual void updateActivationState(btScalar timeStep);
-	virtual void solveConstraints(btContactSolverInfo& solverInfo);
+	
 
 	virtual void serializeMultiBodies(btSerializer* serializer);
 
@@ -55,7 +55,8 @@ public:
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btMultiBodyDynamicsWorld();
-
+    
+    virtual void solveConstraints(btContactSolverInfo& solverInfo);
 	virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter);
 
 	virtual void removeMultiBody(btMultiBody* body);
@@ -95,7 +96,10 @@ public:
 	virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
 
 	virtual void integrateTransforms(btScalar timeStep);
-
+    void integrateMultiBodyTransforms(btScalar timeStep);
+    void predictMultiBodyTransforms(btScalar timeStep);
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
 	virtual void debugDrawWorld();
 
 	virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
@@ -110,6 +114,10 @@ public:
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
 	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+    
+    virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
+    void buildIslands();
 
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index 92d41dfac..1cd6b8c07 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -111,6 +111,10 @@ struct btMultibodyLink
 
 	btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
 	btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
+    
+    // predicted verstion
+    btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
+    btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 
 	btVector3 m_appliedForce;   // In WORLD frame
 	btVector3 m_appliedTorque;  // In WORLD frame
@@ -119,6 +123,7 @@ struct btMultibodyLink
 	btVector3 m_appliedConstraintTorque;  // In WORLD frame
 
 	btScalar m_jointPos[7];
+    btScalar m_jointPos_interpolate[7];
 
 	//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
 	//It gets set to zero after each internal stepSimulation call
@@ -188,42 +193,43 @@ struct btMultibodyLink
 	// routine to update m_cachedRotParentToThis and m_cachedRVector
 	void updateCacheMultiDof(btScalar *pq = 0)
 	{
-		btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
-
+        btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
+        btQuaternion& cachedRot = m_cachedRotParentToThis;
+        btVector3& cachedVector =m_cachedRVector;
 		switch (m_jointType)
 		{
 			case eRevolute:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 
 				break;
 			}
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+				cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
 			case eSpherical:
 			{
-				m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case ePlanar:
 			{
-				m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+				cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
 			case eFixed:
 			{
-				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+				cachedRot = m_zeroRotParentToThis;
+				cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 
 				break;
 			}
@@ -234,6 +240,57 @@ struct btMultibodyLink
 			}
 		}
 	}
+    
+    void updateInterpolationCacheMultiDof()
+    {
+        btScalar *pJointPos = &m_jointPos_interpolate[0];
+        
+        btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
+        btVector3& cachedVector = m_cachedRVector_interpolate;
+        switch (m_jointType)
+        {
+            case eRevolute:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
+                
+                break;
+            }
+            case ePrismatic:
+            {
+                // m_cachedRotParentToThis never changes, so no need to update
+                cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
+                
+                break;
+            }
+            case eSpherical:
+            {
+                cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case ePlanar:
+            {
+                cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
+                cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            case eFixed:
+            {
+                cachedRot = m_zeroRotParentToThis;
+                cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
+                
+                break;
+            }
+            default:
+            {
+                //invalid type
+                btAssert(0);
+            }
+        }
+    }
 };
 
 #endif  //BT_MULTIBODY_LINK_H
diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt
index d43df1c67..40155786e 100644
--- a/src/BulletSoftBody/CMakeLists.txt
+++ b/src/BulletSoftBody/CMakeLists.txt
@@ -17,6 +17,11 @@ SET(BulletSoftBody_SRCS
 	btSoftSoftCollisionAlgorithm.cpp
 	btDefaultSoftBodySolver.cpp
 
+	btDeformableBackwardEulerObjective.cpp
+	btDeformableBodySolver.cpp
+	btDeformableContactProjection.cpp
+	btDeformableRigidDynamicsWorld.cpp
+
 )
 
 SET(BulletSoftBody_HDRS
@@ -33,6 +38,18 @@ SET(BulletSoftBody_HDRS
 
 	btSoftBodySolvers.h
 	btDefaultSoftBodySolver.h
+	
+	btCGProjection.h
+	btConjugateGradient.h
+	btDeformableGravityForce.h
+	btDeformableMassSpringForce.h
+	btDeformableLagrangianForce.h
+	btPreconditioner.h
+
+	btDeformableBackwardEulerObjective.h
+	btDeformableBodySolver.h
+	btDeformableContactProjection.h
+	btDeformableRigidDynamicsWorld.h
 
 	btSoftBodySolverVertexBuffer.h
 )
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
new file mode 100644
index 000000000..f38fa545c
--- /dev/null
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -0,0 +1,148 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CG_PROJECTION_H
+#define BT_CG_PROJECTION_H
+
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+
+class btDeformableRigidDynamicsWorld;
+
+struct DeformableContactConstraint
+{
+    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;
+    
+    DeformableContactConstraint(const btSoftBody::RContact& rcontact)
+    {
+        append(rcontact);
+    }
+    
+    DeformableContactConstraint(const btVector3 dir)
+    {
+        m_contact.push_back(NULL);
+        m_direction.push_back(dir);
+        m_value.push_back(0);
+        m_accumulated_normal_impulse.push_back(0);
+    }
+    
+    DeformableContactConstraint()
+    {
+        m_contact.push_back(NULL);
+        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);
+    }
+    
+    ~DeformableContactConstraint()
+    {
+    }
+};
+
+struct DeformableFrictionConstraint
+{
+    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; // whether the contact is released
+
+    
+    // the total impulse the node applied to the rb in the tangential direction in the cg solve
+    btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
+    
+    DeformableFrictionConstraint()
+    {
+        append();
+    }
+    
+    void append()
+    {
+        m_static.push_back(false);
+        m_static_prev.push_back(false);
+        
+        m_direction_prev.push_back(btVector3(0,0,0));
+        m_direction.push_back(btVector3(0,0,0));
+        
+        m_impulse.push_back(0);
+        m_impulse_prev.push_back(0);
+        
+        m_dv.push_back(0);
+        m_dv_prev.push_back(0);
+        
+        m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
+        m_released.push_back(false);
+    }
+};
+
+class btCGProjection
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
+    typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
+    btAlignedObjectArray<btSoftBody *>& m_softBodies;
+    btDeformableRigidDynamicsWorld* m_world;
+//    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+    const btScalar& m_dt;
+    
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    : m_softBodies(softBodies)
+    , m_dt(dt)
+    {
+    }
+    
+    virtual ~btCGProjection()
+    {
+    }
+    
+    // apply the constraints
+    virtual void project(TVStack& x) = 0;
+    
+    virtual void setConstraints() = 0;
+    
+    // update the constraints
+    virtual void update() = 0;
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+    }
+};
+
+
+#endif /* btCGProjection_h */
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
new file mode 100644
index 000000000..22a9f3ada
--- /dev/null
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -0,0 +1,146 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONJUGATE_GRADIENT_H
+#define BT_CONJUGATE_GRADIENT_H
+#include <iostream>
+#include <cmath>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <LinearMath/btVector3.h>
+#include "LinearMath/btQuickprof.h"
+template <class MatrixX>
+class btConjugateGradient
+{
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    TVStack r,p,z,temp;
+    int max_iterations;
+public:
+    btConjugateGradient(const int max_it_in)
+    : max_iterations(max_it_in)
+    {
+    }
+    
+    virtual ~btConjugateGradient(){}
+    
+    // return the number of iterations taken
+    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance)
+    {
+        BT_PROFILE("CGSolve");
+        btAssert(x.size() == b.size());
+        reinitialize(b);
+        
+        // r = b - A * x --with assigned dof zeroed out
+        A.multiply(x, temp);
+        r = sub(b, temp);
+        A.project(r);
+        A.enforceConstraint(x);
+        
+        btScalar r_norm = std::sqrt(squaredNorm(r));
+        if (r_norm < tolerance) {
+            std::cout << "Iteration = 0" << std::endl;
+            std::cout << "Two norm of the residual = " << r_norm << std::endl;
+            return 0;
+        }
+        
+        // z = M^(-1) * r
+        A.precondition(r, z);
+        p = z;
+        // temp = A*p
+        A.multiply(p, temp);
+        
+        btScalar r_dot_z = dot(z,r), r_dot_z_new;
+        // alpha = r^T * z / (p^T * A * p)
+        btScalar alpha = r_dot_z / dot(p, temp), beta;
+        
+        for (int k = 1; k < max_iterations; k++) {
+            //  x += alpha * p;
+            //  r -= alpha * temp;
+            multAndAddTo(alpha, p, x);
+            multAndAddTo(-alpha, temp, r);
+            // zero out the dofs of r
+            A.project(r);
+            A.enforceConstraint(x);
+            r_norm = std::sqrt(squaredNorm(r));
+            
+            if (r_norm < tolerance) {
+                std::cout << "ConjugateGradient iterations " << k << std::endl;
+                return k;
+            }
+            
+            // z = M^(-1) * r
+            A.precondition(r, z);
+            r_dot_z_new = dot(r,z);
+            beta = r_dot_z_new/ r_dot_z;
+            r_dot_z = r_dot_z_new;
+            // p = z + beta * p;
+            p = multAndAdd(beta, p, z);
+            // temp = A * p;
+            A.multiply(p, temp);
+            // alpha = r^T * z / (p^T * A * p)
+            alpha = r_dot_z / dot(p, temp);
+        }
+        std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
+        return max_iterations;
+    }
+    
+    void reinitialize(const TVStack& b)
+    {
+        r.resize(b.size());
+        p.resize(b.size());
+        z.resize(b.size());
+        temp.resize(b.size());
+    }
+    
+    TVStack sub(const TVStack& a, const TVStack& b)
+    {
+        // c = a-b
+        btAssert(a.size() == b.size())
+        TVStack c;
+        c.resize(a.size());
+        for (int i = 0; i < a.size(); ++i)
+            c[i] = a[i] - b[i];
+        return c;
+    }
+    
+    btScalar squaredNorm(const TVStack& a)
+    {
+        return dot(a,a);
+    }
+    
+    btScalar dot(const TVStack& a, const TVStack& b)
+    {
+        btScalar ans(0);
+        for (int i = 0; i < a.size(); ++i)
+            ans += a[i].dot(b[i]);
+        return ans;
+    }
+    
+    void multAndAddTo(btScalar s, const TVStack& a, TVStack& result)
+    {
+//        result += s*a
+        btAssert(a.size() == result.size())
+        for (int i = 0; i < a.size(); ++i)
+            result[i] += s * a[i];
+    }
+    
+    TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b)
+    {
+        // result = a*s + b
+        TVStack result;
+        result.resize(a.size());
+        for (int i = 0; i < a.size(); ++i)
+            result[i] = s * a[i] + b[i];
+        return result;
+    }
+};
+#endif /* btConjugateGradient_h */
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
new file mode 100644
index 000000000..746048562
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -0,0 +1,147 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableBackwardEulerObjective.h"
+#include "LinearMath/btQuickprof.h"
+
+btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+: m_softBodies(softBodies)
+, projection(m_softBodies, m_dt)
+, m_backupVelocity(backup_v)
+{
+    m_preconditioner = new DefaultPreconditioner();
+}
+
+void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
+{
+    BT_PROFILE("reinitialize");
+    setDt(dt);
+    if(nodeUpdated)
+    {
+        updateId();
+    }
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->reinitialize(nodeUpdated);
+    }
+    projection.reinitialize(nodeUpdated);
+    m_preconditioner->reinitialize(nodeUpdated);
+}
+
+void btDeformableBackwardEulerObjective::setDt(btScalar dt)
+{
+    m_dt = dt;
+}
+
+void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
+{
+    BT_PROFILE("multiply");
+    // add in the mass term
+    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)
+        {
+            const btSoftBody::Node& node = psb->m_nodes[j];
+            b[counter] = (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
+            ++counter;
+        }
+    }
+
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        // add damping matrix
+        m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
+    }
+}
+
+void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
+{
+    // only the velocity of the constrained nodes needs to be updated during CG solve
+    for (int i = 0; i < projection.m_constraints.size(); ++i)
+    {
+        int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
+        m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
+    }
+}
+
+void btDeformableBackwardEulerObjective::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 btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
+{
+    BT_PROFILE("computeResidual");
+    // add implicit force
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->addScaledImplicitForce(dt, residual);
+    }
+}
+
+btScalar btDeformableBackwardEulerObjective::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 btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
+{
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->addScaledExplicitForce(m_dt, force);
+    }
+    applyForce(force, true);
+}
+
+void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
+{
+    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)
+        {
+            dv[counter] = psb->m_nodes[j].m_im * residual[counter];
+            ++counter;
+        }
+    }
+}
+
+//set constraints as projections
+void btDeformableBackwardEulerObjective::setConstraints()
+{
+    // build islands for multibody solve
+    m_world->btMultiBodyDynamicsWorld::buildIslands();
+    projection.setConstraints();
+}
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
new file mode 100644
index 000000000..f87f7f509
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -0,0 +1,126 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
+#define BT_BACKWARD_EULER_OBJECTIVE_H
+#include "btConjugateGradient.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableGravityForce.h"
+#include "btDeformableContactProjection.h"
+#include "btPreconditioner.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+class btDeformableRigidDynamicsWorld;
+class btDeformableBackwardEulerObjective
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btScalar m_dt;
+    btDeformableRigidDynamicsWorld* m_world;
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
+    btAlignedObjectArray<btSoftBody *>& m_softBodies;
+    Preconditioner* m_preconditioner;
+    btDeformableContactProjection projection;
+    const TVStack& m_backupVelocity;
+    btAlignedObjectArray<btSoftBody::Node* > m_nodes;
+    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
+    
+    virtual ~btDeformableBackwardEulerObjective() {}
+    
+    void initialize(){}
+    
+    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+    void computeResidual(btScalar dt, TVStack& residual) const;
+    
+    // add explicit force to the velocity
+    void applyExplicitForce(TVStack& force);
+    
+    // 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;
+    
+    // set initial guess for CG solve
+    void initialGuess(TVStack& dv, const TVStack& residual);
+    
+    // reset data structure and reset dt
+    void reinitialize(bool nodeUpdated, btScalar dt);
+    
+    void setDt(btScalar dt);
+    
+    // enforce constraints in CG solve
+    void enforceConstraint(TVStack& x)
+    {
+        BT_PROFILE("enforceConstraint");
+        projection.enforceConstraint(x);
+        updateVelocity(x);
+    }
+    
+    // add dv to velocity
+    void updateVelocity(const TVStack& dv);
+    
+    //set constraints as projections
+    void setConstraints();
+    
+    // update the projections and project the residual
+    void project(TVStack& r)
+    {
+        BT_PROFILE("project");
+        projection.update();
+        projection.project(r);
+    }
+    
+    // perform precondition M^(-1) x = b
+    void precondition(const TVStack& x, TVStack& b)
+    {
+        m_preconditioner->operator()(x,b);
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+        projection.setWorld(world);
+    }
+    
+    virtual void updateId()
+    {
+        size_t id = 0;
+        m_nodes.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)
+            {
+                psb->m_nodes[j].index = id;
+                m_nodes.push_back(&psb->m_nodes[j]);
+                ++id;
+            }
+        }
+    }
+    
+    const btAlignedObjectArray<btSoftBody::Node*>* getIndices() const
+    {
+        return &m_nodes;
+    }
+};
+
+#endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
new file mode 100644
index 000000000..eb8e62237
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -0,0 +1,204 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include <stdio.h>
+#include <limits>
+#include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
+
+btDeformableBodySolver::btDeformableBodySolver()
+: m_numNodes(0)
+, m_cg(10)
+{
+    m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+}
+
+btDeformableBodySolver::~btDeformableBodySolver()
+{
+    delete m_objective;
+}
+
+void btDeformableBodySolver::solveConstraints(float solverdt)
+{
+    BT_PROFILE("solveConstraints");
+    // add constraints to the solver
+    setConstraints();
+    
+    // save v_{n+1}^* velocity after explicit forces
+    backupVelocity();
+    
+    m_objective->computeResidual(solverdt, m_residual);
+    
+    computeStep(m_dv, m_residual);
+    updateVelocity();
+}
+
+void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
+{
+    btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
+    m_cg.solve(*m_objective, dv, residual, tolerance);
+}
+
+void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
+{
+    m_softBodySet.copyFromArray(softBodies);
+    bool nodeUpdated = updateNodes();
+    
+    if (nodeUpdated)
+    {
+        m_dv.resize(m_numNodes, btVector3(0,0,0));
+        m_residual.resize(m_numNodes, btVector3(0,0,0));
+        m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
+    }
+    
+    // need to setZero here as resize only set value for newly allocated items
+    for (int i = 0; i < m_numNodes; ++i)
+    {
+        m_dv[i].setZero();
+        m_residual[i].setZero();
+    }
+    
+    m_objective->reinitialize(nodeUpdated, dt);
+}
+
+void btDeformableBodySolver::setConstraints()
+{
+    BT_PROFILE("setConstraint");
+    m_objective->setConstraints();
+}
+
+void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
+{
+    m_objective->setWorld(world);
+}
+
+void btDeformableBodySolver::updateVelocity()
+{
+    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)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
+            ++counter;
+        }
+    }
+}
+
+void btDeformableBodySolver::backupVelocity()
+{
+    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 btDeformableBodySolver::revertVelocity()
+{
+    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)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter++];
+        }
+    }
+}
+
+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;
+        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())
+        {
+            // apply explicit forces to velocity
+            m_objective->applyExplicitForce(m_residual);
+            // predict motion for collision detection
+            predictDeformableMotion(psb, solverdt);
+        }
+    }
+}
+
+void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
+{
+    int i, ni;
+    
+    /* Prepare                */
+    psb->m_sst.sdt = dt * psb->m_cfg.timescale;
+    psb->m_sst.isdt = 1 / psb->m_sst.sdt;
+    psb->m_sst.velmrg = psb->m_sst.sdt * 3;
+    psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
+    psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
+    /* Integrate            */
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        n.m_q = n.m_x;
+        n.m_x += n.m_v * dt;
+    }
+    /* Bounds                */
+    psb->updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg);
+        psb->m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * psb->m_sst.velmrg,
+                       psb->m_sst.updmrg);
+    }
+
+    /* Clear contacts        */
+    psb->m_rcontacts.resize(0);
+    psb->m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    psb->m_ndbvt.optimizeIncremental(1);
+}
+
+void btDeformableBodySolver::updateSoftBodies()
+{
+    for (int i = 0; i < m_softBodySet.size(); i++)
+    {
+        btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
+        if (psb->isActive())
+        {
+            psb->updateNormals(); // normal is updated here
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
new file mode 100644
index 000000000..94102afa9
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -0,0 +1,94 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_BODY_SOLVERS_H
+#define BT_DEFORMABLE_BODY_SOLVERS_H
+
+
+#include "btSoftBodySolvers.h"
+#include "btDeformableBackwardEulerObjective.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+
+struct btCollisionObjectWrapper;
+class btDeformableBackwardEulerObjective;
+class btDeformableRigidDynamicsWorld;
+
+class btDeformableBodySolver : public btSoftBodySolver
+{
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+protected:
+    int m_numNodes;
+    TVStack m_dv;
+    TVStack m_residual;
+    btAlignedObjectArray<btSoftBody *> m_softBodySet;
+   
+    btAlignedObjectArray<btVector3> m_backupVelocity;
+    btScalar m_dt;
+    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
+    
+    
+public:
+    btDeformableBackwardEulerObjective* m_objective;
+    
+    btDeformableBodySolver();
+    
+    virtual ~btDeformableBodySolver();
+    
+    virtual SolverTypes getSolverType() const
+    {
+        return DEFORMABLE_SOLVER;
+    }
+
+    virtual void updateSoftBodies();
+
+    virtual void copyBackToSoftBodies(bool bMove = true) {}
+
+    void extracted(float solverdt);
+    
+    virtual void solveConstraints(float solverdt);
+    
+    void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
+    
+    void setConstraints();
+    
+    void predictDeformableMotion(btSoftBody* psb, btScalar dt);
+    
+    void backupVelocity();
+    void revertVelocity();
+    void updateVelocity();
+    
+    bool updateNodes();
+    
+    void computeStep(TVStack& dv, const TVStack& residual);
+                     
+    virtual void predictMotion(float solverdt);
+
+    virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
+
+    virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
+    {
+        softBody->defaultCollisionHandler(collisionObjectWrap);
+    }
+
+    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
+        softBody->defaultCollisionHandler(otherSoftBody);
+    }
+    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+    virtual bool checkInitialized(){return true;}
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world);
+};
+
+#endif /* btDeformableBodySolver_h */
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
new file mode 100644
index 000000000..84d35c062
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -0,0 +1,478 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "btDeformableContactProjection.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include <algorithm>
+#include <cmath>
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    jacobianData.m_jacobians.resize(ndof);
+    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+    btScalar* jac = &jacobianData.m_jacobians[0];
+    
+    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+}
+
+static btVector3 generateUnitOrthogonalVector(const btVector3& u)
+{
+    btScalar ux = u.getX();
+    btScalar uy = u.getY();
+    btScalar uz = u.getZ();
+    btScalar ax = std::abs(ux);
+    btScalar ay = std::abs(uy);
+    btScalar az = std::abs(uz);
+    btVector3 v;
+    if (ax <= ay && ax <= az)
+        v = btVector3(0, -uz, uy);
+    else if (ay <= ax && ay <= az)
+        v = btVector3(-uz, 0, ux);
+    else
+        v = btVector3(-uy, ux, 0);
+    v.normalize();
+    return v;
+}
+
+void btDeformableContactProjection::update()
+{
+    ///solve rigid body constraints
+    m_world->getSolverInfo().m_numIterations = 1;
+    m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
+
+    // loop through constraints to set constrained values
+    for (int index = 0; index < m_constraints.size(); ++index)
+    {
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        for (int i = 0; i < constraints.size(); ++i)
+        {
+            DeformableContactConstraint& constraint = constraints[i];
+            DeformableFrictionConstraint& friction = frictions[i];
+            for (int j = 0; j < constraint.m_contact.size(); ++j)
+            {
+                if (constraint.m_contact[j] == NULL)
+                {
+                    // nothing needs to be done for dirichelet constraints
+                    continue;
+                }
+                const btSoftBody::RContact* c = constraint.m_contact[j];
+                const btSoftBody::sCti& cti = c->m_cti;
+                
+                if (cti.m_colObj->hasContactResponse())
+                {
+                    btVector3 va(0, 0, 0);
+                    btRigidBody* rigidCol = 0;
+                    btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                    const btScalar* deltaV_normal;
+                    
+                    // grab the velocity of the rigid body
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    {
+                        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                        va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                            const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
+                            const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
+                            const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
+                            const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                            deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                            // add in the normal component of the va
+                            btScalar vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += local_v[k] * J_n[k];
+                            }
+                            va = cti.m_normal * vel * m_dt;
+
+                            // add in the tangential components of the va
+                            vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += local_v[k] * J_t1[k];
+                            }
+                            va += c->t1 * vel * m_dt;
+                            vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += local_v[k] * J_t2[k];
+                            }
+                            va += c->t2 * vel * m_dt;
+                        }
+                    }
+                    
+                    const btVector3 vb = c->m_node->m_v * m_dt;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, cti.m_normal);
+                    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
+                    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();
+  
+                    if (accumulated_normal < 0)
+                    {
+                        friction.m_direction[j] = -local_tangent_dir;
+                        // 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;
+                        }
+                        else
+                        {
+                            friction.m_static[j] = true;
+                            friction.m_impulse[j] = tangent_norm;
+                        }
+                    }
+                    else
+                    {
+                        friction.m_released[j] = true;
+                        friction.m_static[j] = false;
+                        friction.m_impulse[j] = 0;
+                        friction.m_direction[j] = btVector3(0,0,0);
+                    }
+                    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]);
+                    
+
+                    // dv = new_impulse + accumulated velocity change in previous CG iterations
+                    // so we have the invariant node->m_v = backupVelocity + dv;
+
+                    btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
+                    
+                    // the following is equivalent
+                    /*
+                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
+                        btScalar dvn = dv.dot(cti.m_normal);
+                     */
+                    
+                    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 + incremental_tangent;
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    {
+                        if (rigidCol)
+                            rigidCol->applyImpulse(impulse, c->m_c1);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        if (multibodyLinkCol)
+                        {
+                            // apply normal component of the impulse
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
+                            if (incremental_tangent.norm() > SIMD_EPSILON)
+                            {
+                                // apply tangential component of the impulse
+                               const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
+                                const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2));
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::setConstraints()
+{
+    BT_PROFILE("setConstraints");
+    // set Dirichlet constraint
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            if (psb->m_nodes[j].m_im == 0)
+            {
+                btAlignedObjectArray<DeformableContactConstraint> c;
+                c.reserve(3);
+                c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
+                m_constraints.insert(psb->m_nodes[j].index, c);
+                
+                btAlignedObjectArray<DeformableFrictionConstraint> f;
+                f.reserve(3);
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
+                m_frictions.insert(psb->m_nodes[j].index, f);
+            }
+        }
+    }
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_complementary;
+        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
+        {
+            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            // skip anchor points
+            if (c.m_node->m_im == 0)
+            {
+                continue;
+            }
+            
+            const btSoftBody::sCti& cti = c.m_cti;
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btVector3 va(0, 0, 0);
+                btRigidBody* rigidCol = 0;
+                btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                {
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
+                }
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                {
+                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                    if (multibodyLinkCol)
+                    {
+                        btScalar vel = 0.0;
+                        const btScalar* jac = &c.jacobianData_normal.m_jacobians[0];
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                        for (int j = 0; j < ndof; ++j)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                        }
+                        va = cti.m_normal * vel * m_dt;
+                    }
+                }
+                
+                const btVector3 vb = c.m_node->m_v * m_dt;
+                const btVector3 vr = vb - va;
+                const btScalar dn = btDot(vr, cti.m_normal);
+                if (dn < SIMD_EPSILON)
+                {
+                    
+                    if (m_constraints.find(c.m_node->index) == NULL)
+                    {
+                        btAlignedObjectArray<DeformableContactConstraint> constraints;
+                        constraints.push_back(DeformableContactConstraint(c));
+                        m_constraints.insert(c.m_node->index,constraints);
+                        btAlignedObjectArray<DeformableFrictionConstraint> frictions;
+                        frictions.push_back(DeformableFrictionConstraint());
+                        m_frictions.insert(c.m_node->index,frictions);
+                    }
+                    else
+                    {
+                        // group colinear constraints into one
+                        const btScalar angle_epsilon = 0.015192247; // less than 10 degree
+                        bool merged = false;
+                        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints[c.m_node->index];
+                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[c.m_node->index];
+                        for (int j = 0; j < constraints.size(); ++j)
+                        {
+                            const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
+                            btScalar dot_prod = dirs[0].dot(cti.m_normal);
+                            if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
+                            {
+                                // group the constraints
+                                constraints[j].append(c);
+                                // push in an empty friction
+                                frictions[j].append();
+                                merged = true;
+                                break;
+                            }
+                        }
+                        const int dim = 3;
+                        // hard coded no more than 3 constraint directions
+                        if (!merged && constraints.size() < dim)
+                        {
+                            constraints.push_back(DeformableContactConstraint(c));
+                            frictions.push_back(DeformableFrictionConstraint());
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::enforceConstraint(TVStack& x)
+{
+    const int dim = 3;
+    for (int index = 0; index < m_constraints.size(); ++index)
+    {
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        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 DeformableFrictionConstraint& friction= frictions[f];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
+                {
+                    // add the friction constraint
+                    if (friction.m_static[j] == true)
+                    {
+                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
+                        x[i] += friction.m_direction[j] * friction.m_dv[j];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::project(TVStack& x)
+{
+    const int dim = 3;
+    for (int index = 0; index < m_constraints.size(); ++index)
+    {
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_constraints.getAtIndex(index);
+        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)];
+        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)
+            {
+                DeformableFrictionConstraint& 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)
+            {
+                DeformableFrictionConstraint& 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)
+                    {
+                        if (!has_static_constraint)
+                            x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                    }
+                    else
+                    {
+                        // otherwise clear the constraint in the friction direction
+                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::reinitialize(bool nodeUpdated)
+{
+    btCGProjection::reinitialize(nodeUpdated);
+    m_constraints.clear();
+    m_frictions.clear();
+}
+
+
+
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
new file mode 100644
index 000000000..64d448b5e
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -0,0 +1,50 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_CONTACT_PROJECTION_H
+#define BT_CONTACT_PROJECTION_H
+#include "btCGProjection.h"
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "LinearMath/btHashMap.h"
+class btDeformableContactProjection : public btCGProjection
+{
+public:
+    // map from node index to constraints
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+    btHashMap<btHashInt, btAlignedObjectArray<DeformableFrictionConstraint> >m_frictions;
+    
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    : btCGProjection(softBodies, dt)
+    {
+    }
+    
+    virtual ~btDeformableContactProjection()
+    {
+    }
+    
+    // apply the constraints to the rhs
+    virtual void project(TVStack& x);
+    
+    // apply constraints to x in Ax=b
+    virtual void enforceConstraint(TVStack& x);
+    
+    // update the constraints
+    virtual void update();
+    
+    virtual void setConstraints();
+    
+    virtual void reinitialize(bool nodeUpdated);
+};
+#endif /* btDeformableContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
new file mode 100644
index 000000000..270222b7e
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -0,0 +1,68 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H
+#define BT_DEFORMABLE_GRAVITY_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableGravityForce : public btDeformableLagrangianForce
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btVector3 m_gravity;
+    
+    btDeformableGravityForce(const btVector3& g) : m_gravity(g)
+    {
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledGravityForce(scale, force);
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        
+    }
+    
+    virtual void addScaledGravityForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                btSoftBody::Node& n = psb->m_nodes[j];
+                size_t id = n.index;
+                btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
+                btVector3 scaled_force = scale * m_gravity * mass;
+                force[id] += scaled_force;
+            }
+        }
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_GRAVITY_FORCE;
+    }
+    
+    
+};
+#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */
diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
new file mode 100644
index 000000000..eb4d032bf
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -0,0 +1,72 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+
+#include "btSoftBody.h"
+#include <LinearMath/btHashMap.h>
+
+enum btDeformableLagrangianForceType
+{
+    BT_GRAVITY_FORCE = 1,
+    BT_MASSSPRING_FORCE = 2
+};
+
+class btDeformableLagrangianForce
+{
+public:
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;
+    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
+    
+    btDeformableLagrangianForce()
+    {
+    }
+    
+    virtual ~btDeformableLagrangianForce(){}
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
+    
+    virtual btDeformableLagrangianForceType getForceType() = 0;
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+    }
+    
+    virtual int getNumNodes()
+    {
+        int numNodes = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            numNodes += m_softBodies[i]->m_nodes.size();
+        }
+        return numNodes;
+    }
+    
+    virtual void addSoftBody(btSoftBody* psb)
+    {
+        m_softBodies.push_back(psb);
+    }
+    
+    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+    {
+        m_nodes = nodes;
+    }
+};
+#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
new file mode 100644
index 000000000..f97ef0a03
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -0,0 +1,119 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_MASS_SPRING_H
+#define BT_MASS_SPRING_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMassSpringForce : public btDeformableLagrangianForce
+{
+public:
+//    using TVStack = btDeformableLagrangianForce::TVStack;
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btDeformableMassSpringForce()
+    {
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledDampingForce(scale, force);
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                
+                // damping force
+                btVector3 v_diff = (node2->m_v - node1->m_v);
+                btScalar k_damp = psb->m_dampingCoefficient;
+                btVector3 scaled_force = scale * v_diff * k_damp;
+                force[id1] += scaled_force;
+                force[id2] -= scaled_force;
+            }
+        }
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                btScalar kLST = link.Feature::m_material->m_kLST;
+                btScalar r = link.m_rl;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                
+                // elastic force
+                // explicit elastic force
+                btVector3 dir = (node2->m_q - node1->m_q);
+                btVector3 dir_normalized = dir.normalized();
+                btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
+                force[id1] += scaled_force;
+                force[id2] -= scaled_force;
+            }
+        }
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        // implicit damping force differential
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            btScalar scaled_k_damp = psb->m_dampingCoefficient * scale;
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
+                df[id1] += local_scaled_df;
+                df[id2] -= local_scaled_df;
+            }
+        }
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_MASSSPRING_FORCE;
+    }
+    
+};
+
+#endif /* btMassSpring_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
new file mode 100644
index 000000000..3b3b62edc
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -0,0 +1,266 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/* ====== Overview of the Deformable Algorithm ====== */
+
+/*
+A single step of the deformable body simulation contains the following main components:
+1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
+2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
+3. Then velocities of deformable bodies v_{n+1} are solved in
+        M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
+   by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
+4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
+5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
+6. Apply position correction to prevent numerical drift.
+
+The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
+ */
+
+#include <stdio.h>
+#include "btDeformableRigidDynamicsWorld.h"
+#include "btDeformableBodySolver.h"
+#include "LinearMath/btQuickprof.h"
+
+void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
+{
+    BT_PROFILE("internalSingleStepSimulation");
+    reinitialize(timeStep);
+    // add gravity to velocity of rigid and multi bodys
+    applyRigidBodyGravity(timeStep);
+    
+    ///apply gravity and explicit force to velocity, predict motion
+    predictUnconstraintMotion(timeStep);
+    
+    ///perform collision detection
+    btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
+    
+    btMultiBodyDynamicsWorld::calculateSimulationIslands();
+    
+    beforeSolverCallbacks(timeStep);
+    
+    ///solve deformable bodies constraints
+    solveDeformableBodiesConstraints(timeStep);
+    
+    afterSolverCallbacks(timeStep);
+    
+    integrateTransforms(timeStep);
+    
+    ///update vehicle simulation
+    btMultiBodyDynamicsWorld::updateActions(timeStep);
+    
+    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+    // End solver-wise simulation step
+    // ///////////////////////////////
+}
+
+void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
+{
+    // perform position correction for all constraints 
+    BT_PROFILE("positionCorrection");
+    for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
+    {
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
+        for (int i = 0; i < constraints.size(); ++i)
+        {
+            DeformableContactConstraint& constraint = constraints[i];
+            DeformableFrictionConstraint& friction = frictions[i];
+            for (int j = 0; j < constraint.m_contact.size(); ++j)
+            {
+                const btSoftBody::RContact* c = constraint.m_contact[j];
+                // skip anchor points
+                if (c == NULL || c->m_node->m_im == 0)
+                    continue;
+                const btSoftBody::sCti& cti = c->m_cti;
+                btVector3 va(0, 0, 0);
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                {
+                    btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
+                }
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                {
+                    btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                    if (multibodyLinkCol)
+                    {
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                        const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
+                        const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
+                        const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
+                        const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                        // add in the normal component of the va
+                        btScalar vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += local_v[k] * J_n[k];
+                        }
+                        va = cti.m_normal * vel;
+                        
+                        vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += local_v[k] * J_t1[k];
+                        }
+                        va += c->t1 * vel;
+                        vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += local_v[k] * J_t2[k];
+                        }
+                        va += c->t2 * vel;
+                    }
+                }
+                else
+                {
+                    // The object interacting with deformable node is not supported for position correction
+                    btAssert(false);
+                }
+                
+                if (cti.m_colObj->hasContactResponse())
+                {
+                    btScalar dp = cti.m_offset;
+                    
+                    // only perform position correction when penetrating
+                    if (dp < 0)
+                    {
+                        if (friction.m_static[j] == true)
+                        {
+                            c->m_node->m_v = va;
+                        }
+                        c->m_node->m_v -= dp * cti.m_normal / dt;
+                    }
+                }
+            }
+        }
+    }
+}
+
+
+void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
+{
+    BT_PROFILE("integrateTransforms");
+    m_deformableBodySolver->backupVelocity();
+    positionCorrection(dt);
+    btMultiBodyDynamicsWorld::integrateTransforms(dt);
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            btSoftBody::Node& node = psb->m_nodes[j];
+            node.m_x  =  node.m_q + dt * node.m_v;
+        }
+    }
+    m_deformableBodySolver->revertVelocity();
+}
+
+void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
+{
+    m_deformableBodySolver->solveConstraints(timeStep);
+}
+
+void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
+{
+    m_softBodies.push_back(body);
+    
+    // Set the soft body solver that will deal with this body
+    // to be the world's solver
+    body->setSoftBodySolver(m_deformableBodySolver);
+    
+    btCollisionWorld::addCollisionObject(body,
+                                         collisionFilterGroup,
+                                         collisionFilterMask);
+}
+
+void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    BT_PROFILE("predictUnconstraintMotion");
+    btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
+    m_deformableBodySolver->predictMotion(timeStep);
+}
+
+void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep)
+{
+    m_internalTime += timeStep;
+    m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
+    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
+    dispatchInfo.m_timeStep = timeStep;
+    dispatchInfo.m_stepCount = 0;
+    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
+    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+}
+
+void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
+{
+    // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
+    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
+    // when there are multiple substeps
+    clearForces();
+    clearMultiBodyForces();
+    btMultiBodyDynamicsWorld::applyGravity();
+    // integrate rigid body gravity
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+    {
+        btRigidBody* rb = m_nonStaticRigidBodies[i];
+        rb->integrateVelocities(timeStep);
+    }
+    // integrate multibody gravity
+    btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
+    clearForces();
+    clearMultiBodyForces();
+}
+
+void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
+{
+    if (0 != m_internalTickCallback)
+    {
+        (*m_internalTickCallback)(this, timeStep);
+    }
+    
+    if (0 != m_solverCallback)
+    {
+        (*m_solverCallback)(m_internalTime, this);
+    }
+}
+
+void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
+{
+    if (0 != m_solverCallback)
+    {
+        (*m_solverCallback)(m_internalTime, this);
+    }
+}
+
+void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+    bool added = false;
+    for (int i = 0; i < forces.size(); ++i)
+    {
+        if (forces[i]->getForceType() == force->getForceType())
+        {
+            forces[i]->addSoftBody(psb);
+            added = true;
+            break;
+        }
+    }
+    if (!added)
+    {
+        force->addSoftBody(psb);
+        force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+        forces.push_back(force);
+    }
+}
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
new file mode 100644
index 000000000..de5eb7ef4
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -0,0 +1,142 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
+#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
+
+#include "btSoftMultiBodyDynamicsWorld.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableBodySolver.h"
+#include "btSoftBodyHelpers.h"
+#include <functional>
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableBodySolver;
+class btDeformableLagrangianForce;
+typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+    typedef btAlignedObjectArray<btVector3> TVStack;
+//    using TVStack = btAlignedObjectArray<btVector3>;
+    ///Solver classes that encapsulate multiple deformable bodies for solving
+    btDeformableBodySolver* m_deformableBodySolver;
+    btSoftBodyArray m_softBodies;
+    int m_drawFlags;
+    bool m_drawNodeTree;
+    bool m_drawFaceTree;
+    bool m_drawClusterTree;
+    btSoftBodyWorldInfo m_sbi;
+    bool m_ownsSolver;
+    btScalar m_internalTime;
+    
+    typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world);
+    btSolverCallback m_solverCallback;
+    
+protected:
+    virtual void internalSingleStepSimulation(btScalar timeStep);
+    
+    virtual void integrateTransforms(btScalar timeStep);
+    
+    void positionCorrection(btScalar dt);
+    
+    void solveDeformableBodiesConstraints(btScalar timeStep);
+    
+public:
+    btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
+    : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
+    m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
+    {
+        m_drawFlags = fDrawFlags::Std;
+        m_drawNodeTree = true;
+        m_drawFaceTree = false;
+        m_drawClusterTree = false;
+        m_sbi.m_broadphase = pairCache;
+        m_sbi.m_dispatcher = dispatcher;
+        m_sbi.m_sparsesdf.Initialize();
+        m_sbi.m_sparsesdf.Reset();
+        
+        m_sbi.air_density = (btScalar)1.2;
+        m_sbi.water_density = 0;
+        m_sbi.water_offset = 0;
+        m_sbi.water_normal = btVector3(0, 0, 0);
+        m_sbi.m_gravity.setValue(0, -10, 0);
+        
+        m_sbi.m_sparsesdf.Initialize();
+        m_internalTime = 0.0;
+    }
+
+    void setSolverCallback(btSolverCallback cb)
+    {
+        m_solverCallback = cb;
+    }
+    
+    virtual ~btDeformableRigidDynamicsWorld()
+    {
+    }
+    
+    virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld()
+    {
+        return (btMultiBodyDynamicsWorld*)(this);
+    }
+    
+    virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const
+    {
+        return (const btMultiBodyDynamicsWorld*)(this);
+    }
+    
+    virtual btDynamicsWorldType getWorldType() const
+    {
+        return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD;
+    }
+    
+    virtual void predictUnconstraintMotion(btScalar timeStep);
+    
+    virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
+    
+    btSoftBodyArray& getSoftBodyArray()
+    {
+        return m_softBodies;
+    }
+    
+    const btSoftBodyArray& getSoftBodyArray() const
+    {
+        return m_softBodies;
+    }
+    
+    btSoftBodyWorldInfo& getWorldInfo()
+    {
+        return m_sbi;
+    }
+    
+    const btSoftBodyWorldInfo& getWorldInfo() const
+    {
+        return m_sbi;
+    }
+    
+    void reinitialize(btScalar timeStep);
+    
+    void applyRigidBodyGravity(btScalar timeStep);
+    
+    void beforeSolverCallbacks(btScalar timeStep);
+    
+    void afterSolverCallbacks(btScalar timeStep);
+    
+    void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+    
+    int getDrawFlags() const { return (m_drawFlags); }
+    void setDrawFlags(int f) { m_drawFlags = f; }
+};
+
+#endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h
new file mode 100644
index 000000000..663731a58
--- /dev/null
+++ b/src/BulletSoftBody/btPreconditioner.h
@@ -0,0 +1,74 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef BT_PRECONDITIONER_H
+#define BT_PRECONDITIONER_H
+
+class Preconditioner
+{
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+//    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 */
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 58796a88d..86a48bf62 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -110,6 +110,7 @@ void btSoftBody::initDefaults()
 
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
+    m_dampingCoefficient = 1;
 }
 
 //
@@ -1757,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
 	}
 }
 
-//
 void btSoftBody::predictMotion(btScalar dt)
 {
-	int i, ni;
-
-	/* Update				*/
-	if (m_bUpdateRtCst)
-	{
-		m_bUpdateRtCst = false;
-		updateConstants();
-		m_fdbvt.clear();
-		if (m_cfg.collisions & fCollision::VF_SS)
-		{
-			initializeFaceTree();
-		}
-	}
-
-	/* Prepare				*/
-	m_sst.sdt = dt * m_cfg.timescale;
-	m_sst.isdt = 1 / m_sst.sdt;
-	m_sst.velmrg = m_sst.sdt * 3;
-	m_sst.radmrg = getCollisionShape()->getMargin();
-	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
-	/* Forces				*/
-	addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
-	applyForces();
-	/* Integrate			*/
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		n.m_q = n.m_x;
-		btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
-		{
-			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
-			for (int c = 0; c < 3; c++)
-			{
-				if (deltaV[c] > clampDeltaV)
-				{
-					deltaV[c] = clampDeltaV;
-				}
-				if (deltaV[c] < -clampDeltaV)
-				{
-					deltaV[c] = -clampDeltaV;
-				}
-			}
-		}
-		n.m_v += deltaV;
-		n.m_x += n.m_v * m_sst.sdt;
-		n.m_f = btVector3(0, 0, 0);
-	}
-	/* Clusters				*/
-	updateClusters();
-	/* Bounds				*/
-	updateBounds();
-	/* Nodes				*/
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)
-	vol;
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
-		m_ndbvt.update(n.m_leaf,
-					   vol,
-					   n.m_v * m_sst.velmrg,
-					   m_sst.updmrg);
-	}
-	/* Faces				*/
-	if (!m_fdbvt.empty())
-	{
-		for (int i = 0; i < m_faces.size(); ++i)
-		{
-			Face& f = m_faces[i];
-			const btVector3 v = (f.m_n[0]->m_v +
-								 f.m_n[1]->m_v +
-								 f.m_n[2]->m_v) /
-								3;
-			vol = VolumeOf(f, m_sst.radmrg);
-			m_fdbvt.update(f.m_leaf,
-						   vol,
-						   v * m_sst.velmrg,
-						   m_sst.updmrg);
-		}
-	}
-	/* Pose					*/
-	updatePose();
-	/* Match				*/
-	if (m_pose.m_bframe && (m_cfg.kMT > 0))
-	{
-		const btMatrix3x3 posetrs = m_pose.m_rot;
-		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
-		{
-			Node& n = m_nodes[i];
-			if (n.m_im > 0)
-			{
-				const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
-				n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
-			}
-		}
-	}
-	/* Clear contacts		*/
-	m_rcontacts.resize(0);
-	m_scontacts.resize(0);
-	/* Optimize dbvt's		*/
-	m_ndbvt.optimizeIncremental(1);
-	m_fdbvt.optimizeIncremental(1);
-	m_cdbvt.optimizeIncremental(1);
+    int i, ni;
+    
+    /* Update                */
+    if (m_bUpdateRtCst)
+    {
+        m_bUpdateRtCst = false;
+        updateConstants();
+        m_fdbvt.clear();
+        if (m_cfg.collisions & fCollision::VF_SS)
+        {
+            initializeFaceTree();
+        }
+    }
+    
+    /* Prepare                */
+    m_sst.sdt = dt * m_cfg.timescale;
+    m_sst.isdt = 1 / m_sst.sdt;
+    m_sst.velmrg = m_sst.sdt * 3;
+    m_sst.radmrg = getCollisionShape()->getMargin();
+    m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
+    /* Forces                */
+    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+    applyForces();
+    /* Integrate            */
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        n.m_q = n.m_x;
+        btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
+        {
+            btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
+            btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
+            for (int c = 0; c < 3; c++)
+            {
+                if (deltaV[c] > clampDeltaV)
+                {
+                    deltaV[c] = clampDeltaV;
+                }
+                if (deltaV[c] < -clampDeltaV)
+                {
+                    deltaV[c] = -clampDeltaV;
+                }
+            }
+        }
+        n.m_v += deltaV;
+        n.m_x += n.m_v * m_sst.sdt;
+        n.m_f = btVector3(0, 0, 0);
+    }
+    /* Clusters                */
+    updateClusters();
+    /* Bounds                */
+    updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
+        m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * m_sst.velmrg,
+                       m_sst.updmrg);
+    }
+    /* Faces                */
+    if (!m_fdbvt.empty())
+    {
+        for (int i = 0; i < m_faces.size(); ++i)
+        {
+            Face& f = m_faces[i];
+            const btVector3 v = (f.m_n[0]->m_v +
+                                 f.m_n[1]->m_v +
+                                 f.m_n[2]->m_v) /
+            3;
+            vol = VolumeOf(f, m_sst.radmrg);
+            m_fdbvt.update(f.m_leaf,
+                           vol,
+                           v * m_sst.velmrg,
+                           m_sst.updmrg);
+        }
+    }
+    /* Pose                    */
+    updatePose();
+    /* Match                */
+    if (m_pose.m_bframe && (m_cfg.kMT > 0))
+    {
+        const btMatrix3x3 posetrs = m_pose.m_rot;
+        for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
+        {
+            Node& n = m_nodes[i];
+            if (n.m_im > 0)
+            {
+                const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
+                n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
+            }
+        }
+    }
+    /* Clear contacts        */
+    m_rcontacts.resize(0);
+    m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    m_ndbvt.optimizeIncremental(1);
+    m_fdbvt.optimizeIncremental(1);
+    m_cdbvt.optimizeIncremental(1);
 }
 
+
 //
 void btSoftBody::solveConstraints()
 {
@@ -2261,18 +2262,45 @@ btVector3 btSoftBody::evaluateCom() const
 	return (com);
 }
 
-//
 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
+                              const btVector3& x,
+                              btScalar margin,
+                              btSoftBody::sCti& cti) const
+{
+    btVector3 nrm;
+    const btCollisionShape* shp = colObjWrap->getCollisionShape();
+    //    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+    //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
+    const btTransform& wtr = colObjWrap->getWorldTransform();
+    //todo: check which transform is needed here
+    
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(x),
+                                      shp,
+                                      nrm,
+                                      margin);
+    if (dst < 0)
+    {
+        cti.m_colObj = colObjWrap->getCollisionObject();
+        cti.m_normal = wtr.getBasis() * nrm;
+        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+        return (true);
+    }
+    return (false);
+}
+//
+bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
 							  const btVector3& x,
 							  btScalar margin,
-							  btSoftBody::sCti& cti) const
+							  btSoftBody::sCti& cti, bool predict) const
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-	//	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
-	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
-	const btTransform& wtr = colObjWrap->getWorldTransform();
-	//todo: check which transform is needed here
+    const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+    // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+    // but resolve contact at x_n
+    const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
 
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
@@ -2280,13 +2308,14 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 			shp,
 			nrm,
 			margin);
-	if (dst < 0)
+	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
-		cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
-		return (true);
+        cti.m_offset = dst;
 	}
+    if (dst < 0)
+        return true;
 	return (false);
 }
 
@@ -2774,6 +2803,14 @@ void btSoftBody::dampClusters()
 	}
 }
 
+void btSoftBody::setSpringStiffness(btScalar k)
+{
+    for (int i = 0; i < m_links.size(); ++i)
+    {
+        m_links[i].Feature::m_material->m_kLST = k;
+    }
+}
+
 //
 void btSoftBody::Joint::Prepare(btScalar dt, int)
 {
@@ -3252,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 			collider.ProcessColObj(this, pcoWrap);
 		}
 		break;
+        case fCollision::SDF_RD:
+        {
+            btSoftColliders::CollideSDF_RD docollide;
+            btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+            btTransform wtr = pcoWrap->getWorldTransform();
+            
+            const btTransform ctr = pcoWrap->getWorldTransform();
+            const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+            const btScalar basemargin = getCollisionShape()->getMargin();
+            btVector3 mins;
+            btVector3 maxs;
+            ATTRIBUTE_ALIGNED16(btDbvtVolume)
+            volume;
+            pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
+                                                  mins,
+                                                  maxs);
+            volume = btDbvtVolume::FromMM(mins, maxs);
+            volume.Expand(btVector3(basemargin, basemargin, basemargin));
+            docollide.psb = this;
+            docollide.m_colObj1Wrap = pcoWrap;
+            docollide.m_rigidBody = prb1;
+            
+            docollide.dynmargin = basemargin + timemargin;
+            docollide.stamargin = basemargin;
+            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
+        }
+        break;
 	}
 }
 
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 9b35b799d..f2934c94a 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -26,7 +26,8 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h"
 #include "btSparseSDF.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
-
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 //#ifdef BT_USE_DOUBLE_PRECISION
 //#define btRigidBodyData	btRigidBodyDoubleData
 //#define btRigidBodyDataName	"btRigidBodyDoubleData"
@@ -159,6 +160,7 @@ public:
 			RVSmask = 0x000f,  ///Rigid versus soft mask
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
+            SDF_RD = 0x0003,   ///DF based rigid vs deformable
 
 			SVSmask = 0x0030,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
@@ -257,6 +259,7 @@ public:
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
 		int m_battach : 1;   // Attached
+        int index;
 	};
 	/* Link			*/
 	ATTRIBUTE_ALIGNED16(struct)
@@ -300,6 +303,13 @@ public:
 		btScalar m_c2;     // ima*dt
 		btScalar m_c3;     // Friction
 		btScalar m_c4;     // Hardness
+        
+        // jacobians and unit impulse responses for multibody
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_t1;
+        btMultiBodyJacobianData jacobianData_t2;
+        btVector3 t1;
+        btVector3 t2;
 	};
 	/* SContact		*/
 	struct SContact
@@ -704,6 +714,7 @@ public:
 	btDbvt m_fdbvt;                    // Faces tree
 	btDbvt m_cdbvt;                    // Clusters tree
 	tClusterArray m_clusters;          // Clusters
+    btScalar m_dampingCoefficient;     // Damping Coefficient
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
@@ -735,6 +746,11 @@ public:
 	{
 		return m_worldInfo;
 	}
+    
+    void setDampingCoefficient(btScalar damping_coeff)
+    {
+        m_dampingCoefficient = damping_coeff;
+    }
 
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
@@ -991,7 +1007,8 @@ public:
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
-	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
+	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+    bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
 	void updateNormals();
 	void updateBounds();
 	void updatePose();
@@ -1005,6 +1022,7 @@ public:
 	void solveClusters(btScalar sor);
 	void applyClusters(bool drift);
 	void dampClusters();
+    void setSpringStiffness(btScalar k);
 	void applyForces();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
@@ -1015,7 +1033,7 @@ public:
 	static vsolver_t getSolver(eVSolver::_ solver);
 
 	virtual int calculateSerializeBufferSize() const;
-
+  
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
 
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 7efe514f3..80545ebad 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -25,7 +25,41 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 #include "BulletCollision/CollisionShapes/btConvexInternalShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <string.h>  //for memset
+#include <cmath>
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    jacobianData.m_jacobians.resize(ndof);
+    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+    btScalar* jac = &jacobianData.m_jacobians[0];
+    
+    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+}
+static btVector3 generateUnitOrthogonalVector(const btVector3& u)
+{
+    btScalar ux = u.getX();
+    btScalar uy = u.getY();
+    btScalar uz = u.getZ();
+    btScalar ax = std::abs(ux);
+    btScalar ay = std::abs(uy);
+    btScalar az = std::abs(uz);
+    btVector3 v;
+    if (ax <= ay && ax <= az)
+        v = btVector3(0, -uz, uy);
+    else if (ay <= ax && ay <= az)
+        v = btVector3(-uz, 0, ux);
+    else
+        v = btVector3(-uy, ux, 0);
+    v.normalize();
+    return v;
+}
 //
 // btSymMatrix
 //
@@ -298,6 +332,46 @@ static inline btMatrix3x3 Diagonal(btScalar x)
 	m[2] = btVector3(0, 0, x);
 	return (m);
 }
+
+static inline btMatrix3x3 Diagonal(const btVector3& v)
+{
+    btMatrix3x3 m;
+    m[0] = btVector3(v.getX(), 0, 0);
+    m[1] = btVector3(0, v.getY(), 0);
+    m[2] = btVector3(0, 0, v.getZ());
+    return (m);
+}
+
+static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof)
+{
+    btScalar result = 0;
+    for (int i = 0; i < ndof; ++i)
+        result += a[i] * b[i];
+    return result;
+}
+
+static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3,
+                                       const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof)
+{
+    btMatrix3x3 m;
+    btScalar a11 = Dot(v1,u1,ndof);
+    btScalar a12 = Dot(v1,u2,ndof);
+    btScalar a13 = Dot(v1,u3,ndof);
+    
+    btScalar a21 = Dot(v2,u1,ndof);
+    btScalar a22 = Dot(v2,u2,ndof);
+    btScalar a23 = Dot(v2,u3,ndof);
+    
+    btScalar a31 = Dot(v3,u1,ndof);
+    btScalar a32 = Dot(v3,u2,ndof);
+    btScalar a33 = Dot(v3,u3,ndof);
+    m[0] = btVector3(a11, a12, a13);
+    m[1] = btVector3(a21, a22, a23);
+    m[2] = btVector3(a31, a32, a33);
+    return (m);
+}
+
+
 //
 static inline btMatrix3x3 Add(const btMatrix3x3& a,
 							  const btMatrix3x3& b)
@@ -854,10 +928,62 @@ struct btSoftColliders
 			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
 		}
 	};
+    //
+    // CollideSDF_RS
+    //
+    struct CollideSDF_RS : btDbvt::ICollide
+    {
+        void Process(const btDbvtNode* leaf)
+        {
+            btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
+            DoNode(*node);
+        }
+        void DoNode(btSoftBody::Node& n) const
+        {
+            const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+            btSoftBody::RContact c;
+            
+            if ((!n.m_battach) &&
+                psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+            {
+                const btScalar ima = n.m_im;
+                const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                const btScalar ms = ima + imb;
+                if (ms > 0)
+                {
+                    const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                    const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                    const btVector3 ra = n.m_x - wtr.getOrigin();
+                    const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+                    const btVector3 vb = n.m_x - n.m_q;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, c.m_cti.m_normal);
+                    const btVector3 fv = vr - c.m_cti.m_normal * dn;
+                    const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                    c.m_node = &n;
+                    c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                    c.m_c1 = ra;
+                    c.m_c2 = ima * psb->m_sst.sdt;
+                    c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+                    c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                    psb->m_rcontacts.push_back(c);
+                    if (m_rigidBody)
+                        m_rigidBody->activate();
+                }
+            }
+        }
+        btSoftBody* psb;
+        const btCollisionObjectWrapper* m_colObj1Wrap;
+        btRigidBody* m_rigidBody;
+        btScalar dynmargin;
+        btScalar stamargin;
+    };
+
 	//
-	// CollideSDF_RS
+	// CollideSDF_RD
 	//
-	struct CollideSDF_RS : btDbvt::ICollide
+	struct CollideSDF_RD : btDbvt::ICollide
 	{
 		void Process(const btDbvtNode* leaf)
 		{
@@ -869,34 +995,75 @@ struct btSoftColliders
 			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
 			btSoftBody::RContact c;
 
-			if ((!n.m_battach) &&
-				psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
-			{
-				const btScalar ima = n.m_im;
-				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
-				const btScalar ms = ima + imb;
-				if (ms > 0)
-				{
-					const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-					static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-					const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-					const btVector3 ra = n.m_x - wtr.getOrigin();
-					const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
-					const btVector3 vb = n.m_x - n.m_q;
-					const btVector3 vr = vb - va;
-					const btScalar dn = btDot(vr, c.m_cti.m_normal);
-					const btVector3 fv = vr - c.m_cti.m_normal * dn;
-					const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
-					c.m_node = &n;
-					c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
-					c.m_c1 = ra;
-					c.m_c2 = ima * psb->m_sst.sdt;
-					c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
-					c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
-					psb->m_rcontacts.push_back(c);
-					if (m_rigidBody)
-						m_rigidBody->activate();
-				}
+			if (!n.m_battach)
+            {
+                // check for collision at x_{n+1}^*
+                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
+                {
+                    const btScalar ima = n.m_im;
+                    const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                    const btScalar ms = ima + imb;
+                    if (ms > 0)
+                    {
+                        // resolve contact at x_n
+                        psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
+                        btSoftBody::sCti& cti = c.m_cti;
+                        c.m_node = &n;
+                        const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                        c.m_c2 = ima * psb->m_sst.sdt;
+                        c.m_c3 = fc;
+                        c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                        
+                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                        {
+                            const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                            static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                            const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                            const btVector3 ra = n.m_q - wtr.getOrigin();
+                            
+                            c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                            c.m_c1 = ra;
+                            if (m_rigidBody)
+                                m_rigidBody->activate();
+                        }
+                        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                        {
+                            btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                            if (multibodyLinkCol)
+                            {
+                                btVector3 normal = cti.m_normal;
+                                btVector3 t1 = generateUnitOrthogonalVector(normal);
+                                btVector3 t2 = btCross(normal, t1);
+                                btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+                                findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
+                                findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
+                                findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
+                                
+                                btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+                                btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+                                btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+                                
+                                btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                                btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                                btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                                
+                                btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+                                                t1.getX(), t1.getY(), t1.getZ(),
+                                                t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame
+                                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                                btScalar dt = psb->m_sst.sdt;
+                                btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+                                c.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
+                                c.jacobianData_normal = jacobianData_normal;
+                                c.jacobianData_t1 = jacobianData_t1;
+                                c.jacobianData_t2 = jacobianData_t2;
+                                c.t1 = t1;
+                                c.t2 = t2;
+                            }
+                        }
+                        psb->m_rcontacts.push_back(c);
+                    }
+                }
 			}
 		}
 		btSoftBody* psb;
diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h
index dcf508265..405475529 100644
--- a/src/BulletSoftBody/btSoftBodySolvers.h
+++ b/src/BulletSoftBody/btSoftBodySolvers.h
@@ -35,7 +35,8 @@ public:
 		CL_SOLVER,
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
-		DX_SIMD_SOLVER
+		DX_SIMD_SOLVER,
+        DEFORMABLE_SOLVER
 	};
 
 protected: