add correct impulse matrix to multibody-deformable contact

14 files changed, 618 insertions, 594 deletions

diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp
index a43fc04ca..f51978ae3 100644
--- a/examples/DeformableContact/DeformableContact.cpp
+++ b/examples/DeformableContact/DeformableContact.cpp
@@ -69,10 +69,10 @@ public:
 
 	void resetCamera()
 	{
-        float dist = 15;
+        float dist = 30;
         float pitch = -30;
         float yaw = 100;
-        float targetPos[3] = {0, -3, 0};
+        float targetPos[3] = {0, -10, 0};
 		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
     
@@ -140,7 +140,7 @@ void DeformableContact::initPhysics()
         btTransform groundTransform;
         groundTransform.setIdentity();
         groundTransform.setOrigin(btVector3(0, -40, 0));
-        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1));
+        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.);
 
@@ -163,16 +163,16 @@ void DeformableContact::initPhysics()
 
     
     {
-        bool damping = false;
+        bool damping = true;
         bool gyro = false;
-        int numLinks = 0;
-        bool spherical = true;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        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, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+        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);
@@ -180,13 +180,13 @@ void DeformableContact::initPhysics()
         //
         if (!damping)
         {
-            mbC->setLinearDamping(0.f);
-            mbC->setAngularDamping(0.f);
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
         }
         else
         {
-            mbC->setLinearDamping(0.1f);
-            mbC->setAngularDamping(0.9f);
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
         }
 
         if (numLinks > 0)
@@ -209,22 +209,24 @@ void DeformableContact::initPhysics()
     
     // create a patch of cloth
     {
+        btScalar h = 0;
         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),
-//                                                         20,20,
-                                                         3,3,
+        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->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 = 0;
+        psb->m_cfg.kDF = .1;
         getDeformableDynamicsWorld()->addSoftBody(psb);
     }
 
@@ -271,7 +273,7 @@ void DeformableContact::exitPhysics()
 void DeformableContact::stepSimulation(float deltaTime)
 {
 //    getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime);
-    m_dynamicsWorld->stepSimulation(deltaTime, 4, 1./240.);
+    m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.);
 }
 
 
@@ -279,7 +281,7 @@ btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMulti
 {
     //init the base
     btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
-    float baseMass = .05f;
+    float baseMass = .1f;
     
     if (baseMass)
     {
@@ -300,7 +302,7 @@ btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMulti
     
     //init the links
     btVector3 hingeJointAxis(1, 0, 0);
-    float linkMass = .05f;
+    float linkMass = .1f;
     btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
     
     btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index f29cef2ba..1bbadd941 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -50,6 +50,7 @@
 #include "../DeformableDemo/DeformableDemo.h"
 #include "../Pinch/Pinch.h"
 #include "../DeformableContact/DeformableContact.h"
+#include "../MultiBodyBaseline/MultiBodyBaseline.h"
 #include "../VolumetricDeformable/VolumetricDeformable.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
@@ -126,6 +127,7 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(0, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
         ExampleEntry(0, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
         ExampleEntry(0, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableContactCreateFunc),
+        ExampleEntry(0, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
         
 		ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
 					 AllConstraintCreateFunc),
diff --git a/examples/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/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp
index 9bcf63ab2..b30e9bc24 100644
--- a/examples/VolumetricDeformable/VolumetricDeformable.cpp
+++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp
@@ -77,8 +77,8 @@ public:
     void stepSimulation(float deltaTime)
     {
         //use a smaller internal timestep, there are stability issues
-        float internalTimeStep = 1. / 480.f;
-        m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep);
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
     }
     
     void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 718106e77..d4a9a754f 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -422,7 +422,11 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
     solveExternalForces(solverInfo);
+    solveInternalConstraints(solverInfo);
+}
 
+void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo)
+{
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 019d1bd87..4f48f07d2 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -113,6 +113,7 @@ public:
 	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
     
     virtual void solveExternalForces(btContactSolverInfo& solverInfo);
+    virtual void solveInternalConstraints(btContactSolverInfo& solverInfo);
 
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 09055c7d1..928ee46f8 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -21,11 +21,10 @@ struct DeformableContactConstraint
     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;
-    btAlignedObjectArray<btMultiBodyJacobianData> m_normal_jacobian;
     
-    DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
+    DeformableContactConstraint(const btSoftBody::RContact& rcontact)
     {
-        append(rcontact, jacobian);
+        append(rcontact);
     }
     
     DeformableContactConstraint(const btVector3 dir)
@@ -34,8 +33,6 @@ struct DeformableContactConstraint
         m_direction.push_back(dir);
         m_value.push_back(0);
         m_accumulated_normal_impulse.push_back(0);
-        btMultiBodyJacobianData j;
-        m_normal_jacobian.push_back(j);
     }
     
     DeformableContactConstraint()
@@ -44,17 +41,14 @@ struct DeformableContactConstraint
         m_direction.push_back(btVector3(0,0,0));
         m_value.push_back(0);
         m_accumulated_normal_impulse.push_back(0);
-        btMultiBodyJacobianData j;
-        m_normal_jacobian.push_back(j);
     }
     
-    void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
+    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);
-        m_normal_jacobian.push_back(jacobian);
     }
     
     ~DeformableContactConstraint()
@@ -77,8 +71,6 @@ struct DeformableFrictionConstraint
     btAlignedObjectArray<btVector3> m_direction_prev;
     
     btAlignedObjectArray<bool> m_released; // whether the contact is released
-    btAlignedObjectArray<btMultiBodyJacobianData> m_complementary_jacobian;
-    btAlignedObjectArray<btVector3> m_complementaryDirection;
 
     
     // the total impulse the node applied to the rb in the tangential direction in the cg solve
@@ -89,12 +81,6 @@ struct DeformableFrictionConstraint
         append();
     }
     
-    DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
-    {
-        append();
-        addJacobian(complementaryDir, jacobian);
-    }
-    
     void append()
     {
         m_static.push_back(false);
@@ -112,13 +98,6 @@ struct DeformableFrictionConstraint
         m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
         m_released.push_back(false);
     }
-    
-    void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
-    {
-        m_complementary_jacobian.push_back(jacobian);
-        m_complementaryDirection.push_back(complementaryDir);
-    }
-    
 };
 
 class btCGProjection
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
deleted file mode 100644
index e81cb2c50..000000000
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ /dev/null
@@ -1,488 +0,0 @@
-//
-//  btDeformableContactProjection.cpp
-//  BulletSoftBody
-//
-//  Created by Xuchen Han on 7/4/19.
-//
-
-#include "btDeformableContactProjection.h"
-#include "btDeformableRigidDynamicsWorld.h"
-#include <algorithm>
-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 = 10;
-    m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
-
-    // loop through constraints to set constrained values
-    for (auto& it : m_constraints)
-    {
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
-        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        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] == nullptr)
-                {
-                    // nothing needs to be done for dirichelet constraints
-                    continue;
-                }
-                const btSoftBody::RContact* c = constraint.m_contact[j];
-                const btSoftBody::sCti& cti = c->m_cti;
-                
-                // normal jacobian is precompute but tangent jacobian is not
-                const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
-                const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
-                
-                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* jac_normal = &jacobianData_normal.m_jacobians[0];
-                            deltaV_normal = &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 += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
-                            }
-                            va = cti.m_normal * vel * m_dt;
-                            
-                            // add in complementary direction of va
-                            const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
-                            vel = 0.0;
-                            for (int k = 0; k < ndof; ++k)
-                            {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
-                            }
-                            va += friction.m_complementaryDirection[j] * 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]);
-                    
-                    // TODO cleanup
-                    if (1) // in the same CG solve, the set of constraits doesn't change
-                    {
-                        // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
-                        
-                        // dv = new_impulse + accumulated velocity change in previous CG iterations
-                        // so we have the invariant node->m_v = backupVelocity + dv;
-
-                        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[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)
-                            {
-                                double multiplier = 1;
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
-                                
-                                if (incremental_tangent.norm() > SIMD_EPSILON)
-                                {
-                                    btMultiBodyJacobianData jacobian_tangent;
-                                    btVector3 tangent = incremental_tangent.normalized();
-                                    findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
-                                    const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
-                                    multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
-void btDeformableContactProjection::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.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[&(psb->m_nodes[j])] = c;
-                
-                btAlignedObjectArray<DeformableFrictionConstraint> f;
-                f.push_back(DeformableFrictionConstraint());
-                f.push_back(DeformableFrictionConstraint());
-                f.push_back(DeformableFrictionConstraint());
-                m_frictions[&(psb->m_nodes[j])] = 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)
-                    {
-                        findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
-                        btScalar vel = 0.0;
-                        const btScalar* jac = &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];
-                            std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
-                            std::cout << jac[j] << std::endl;
-                        }
-                        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)
-                {
-                    // find complementary jacobian
-                    btVector3 complementaryDirection;
-                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                    {
-                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                        if (multibodyLinkCol)
-                        {
-                            complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
-                            findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
-                        }
-                    }
-                    
-                    if (m_constraints.find(c.m_node) == m_constraints.end())
-                    {
-                        btAlignedObjectArray<DeformableContactConstraint> constraints;
-                        constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
-                        m_constraints[c.m_node] = constraints;
-                        btAlignedObjectArray<DeformableFrictionConstraint> frictions;
-                        frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
-                        m_frictions[c.m_node] = 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];
-                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
-                        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, jacobianData_normal);
-                                // push in an empty friction
-                                frictions[j].append();
-                                frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
-                                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, jacobianData_normal));
-                            frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
-void btDeformableContactProjection::enforceConstraint(TVStack& x)
-{
-    const int dim = 3;
-    for (auto& it : m_constraints)
-    {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices[it.first];
-        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
-        btAssert(constraints.size() <= dim);
-        btAssert(constraints.size() > 0);
-        if (constraints.size() == 1)
-        {
-            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
-            for (int j = 0; j < constraints[0].m_direction.size(); ++j)
-                x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
-        }
-        else if (constraints.size() == 2)
-        {
-            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
-            btAssert(free_dir.norm() > SIMD_EPSILON)
-            free_dir.normalize();
-            x[i] = x[i].dot(free_dir) * free_dir;
-            for (int j = 0; j < constraints.size(); ++j)
-            {
-                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                {
-                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                }
-            }
-            
-        }
-        else
-        {
-            x[i].setZero();
-            for (int j = 0; j < constraints.size(); ++j)
-            {
-                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
-                {
-                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
-                }
-            }
-        }
-        
-        // apply friction if the node is not constrained in all directions
-        if (constraints.size() < 3)
-        {
-            for (int f = 0; f < frictions.size(); ++f)
-            {
-                const DeformableFrictionConstraint& friction= frictions[f];
-                for (int j = 0; j < friction.m_direction.size(); ++j)
-                {
-                    // clear the old constraint
-                    if (friction.m_static_prev[j] == true)
-                    {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
-                    }
-                    // add the new constraint
-                    if (friction.m_static[j] == true)
-                    {
-                        x[i] += friction.m_direction[j] * friction.m_dv[j];
-                    }
-                }
-            }
-        }
-    }
-}
-
-void btDeformableContactProjection::project(TVStack& x)
-{
-    const int dim = 3;
-    for (auto& it : m_constraints)
-    {
-        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices[it.first];
-        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
-        btAssert(constraints.size() <= dim);
-        btAssert(constraints.size() > 0);
-        if (constraints.size() == 1)
-        {
-            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
-        }
-        else if (constraints.size() == 2)
-        {
-            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
-            btAssert(free_dir.norm() > SIMD_EPSILON)
-            free_dir.normalize();
-            x[i] = x[i].dot(free_dir) * free_dir;
-        }
-        else
-            x[i].setZero();
-        
-        // apply friction if the node is not constrained in all directions
-        if (constraints.size() < 3)
-        {
-            bool has_static_constraint = false;
-            for (int f = 0; f < frictions.size(); ++f)
-            {
-                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 && !has_static_constraint)
-                    {
-                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
-                    }
-                }
-            }
-        }
-    }
-}
-
-void btDeformableContactProjection::reinitialize(bool nodeUpdated)
-{
-    btCGProjection::reinitialize(nodeUpdated);
-    m_constraints.clear();
-    m_frictions.clear();
-}
-
-
-
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 25f667711..1a9cad120 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -45,7 +45,7 @@ void btDeformableContactProjection::update()
 {
     ///solve rigid body constraints
     m_world->getSolverInfo().m_numIterations = 10;
-    m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
+    m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
 
     // loop through constraints to set constrained values
     for (auto& it : m_constraints)
@@ -66,10 +66,6 @@ void btDeformableContactProjection::update()
                 const btSoftBody::RContact* c = constraint.m_contact[j];
                 const btSoftBody::sCti& cti = c->m_cti;
                 
-                // normal jacobian is precompute but tangent jacobian is not
-                const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
-                const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
-                
                 if (cti.m_colObj->hasContactResponse())
                 {
                     btVector3 va(0, 0, 0);
@@ -89,25 +85,31 @@ void btDeformableContactProjection::update()
                         if (multibodyLinkCol)
                         {
                             const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                            const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
-                            deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                            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];
+                            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 += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k];
                             }
                             va = cti.m_normal * vel * m_dt;
-                            
-                            // add in complementary direction of va
-                            const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
+
+                            vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
+                            }
+                            va += c->t1 * vel * m_dt;
                             vel = 0.0;
                             for (int k = 0; k < ndof; ++k)
                             {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
                             }
-                            va += friction.m_complementaryDirection[j] * vel * m_dt;
+                            va += c->t2 * vel * m_dt;
                         }
                     }
                     
@@ -143,8 +145,6 @@ void btDeformableContactProjection::update()
                         friction.m_direction[j] = -local_tangent_dir;
                         // do not allow switching from static friction to dynamic friction
                         // it causes cg to explode
-                        btScalar comp1 = -accumulated_normal*c->m_c3;
-                        btScalar comp2 = tangent_norm;
                         if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
                         {
                             friction.m_static[j] = false;
@@ -194,19 +194,15 @@ void btDeformableContactProjection::update()
                     }
                     else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                     {
-                        
                         if (multibodyLinkCol)
                         {
-                            double multiplier = 1;
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
-                            
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
                             if (incremental_tangent.norm() > SIMD_EPSILON)
                             {
-                                btMultiBodyJacobianData jacobian_tangent;
-                                btVector3 tangent = incremental_tangent.normalized();
-                                findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
-                                const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
+                               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));
                             }
                         }
                     }
@@ -274,15 +270,12 @@ void btDeformableContactProjection::setConstraints()
                     multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
                     if (multibodyLinkCol)
                     {
-                        findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
                         btScalar vel = 0.0;
-                        const btScalar* jac = &jacobianData_normal.m_jacobians[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];
-                            std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
-                            std::cout << jac[j] << std::endl;
                         }
                         va = cti.m_normal * vel * m_dt;
                     }
@@ -293,25 +286,13 @@ void btDeformableContactProjection::setConstraints()
                 const btScalar dn = btDot(vr, cti.m_normal);
                 if (dn < SIMD_EPSILON)
                 {
-                    // find complementary jacobian
-                    btVector3 complementaryDirection;
-                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                    {
-                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                        if (multibodyLinkCol)
-                        {
-                            complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
-                            findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
-                        }
-                    }
-                    
                     if (m_constraints.find(c.m_node) == m_constraints.end())
                     {
                         btAlignedObjectArray<DeformableContactConstraint> constraints;
-                        constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
+                        constraints.push_back(DeformableContactConstraint(c));
                         m_constraints[c.m_node] = constraints;
                         btAlignedObjectArray<DeformableFrictionConstraint> frictions;
-                        frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
+                        frictions.push_back(DeformableFrictionConstraint());
                         m_frictions[c.m_node] = frictions;
                     }
                     else
@@ -328,10 +309,9 @@ void btDeformableContactProjection::setConstraints()
                             if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
                             {
                                 // group the constraints
-                                constraints[j].append(c, jacobianData_normal);
+                                constraints[j].append(c);
                                 // push in an empty friction
                                 frictions[j].append();
-                                frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
                                 merged = true;
                                 break;
                             }
@@ -340,8 +320,8 @@ void btDeformableContactProjection::setConstraints()
                         // hard coded no more than 3 constraint directions
                         if (!merged && constraints.size() < dim)
                         {
-                            constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
-                            frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
+                            constraints.push_back(DeformableContactConstraint(c));
+                            frictions.push_back(DeformableFrictionConstraint());
                         }
                     }
                 }
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index aa7af70fb..e73d8faa4 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -40,4 +40,4 @@ public:
     
     virtual void reinitialize(bool nodeUpdated);
 };
-#endif /* btContactProjection_h */
+#endif /* btDeformableContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 216c1a24d..f9c9968c9 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -69,6 +69,43 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
                     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];
+                        
+                        // add in the normal component of the va
+                        btScalar vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k];
+                        }
+                        va = cti.m_normal * vel;
+                        
+                        vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
+                        }
+                        va += c->t1 * vel;
+                        vel = 0.0;
+                        for (int k = 0; k < ndof; ++k)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[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())
                 {
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index e8fd3da39..95249b093 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"
@@ -300,6 +301,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
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 9b3e45ea4..8cf0ed3c0 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 <iostream>
+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)
@@ -879,21 +953,68 @@ struct btSoftColliders
                     if (ms > 0)
                     {
                         psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
-                        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();
-                        const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                        auto& cti = c.m_cti;
                         c.m_node = &n;
-                        c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
-                        c.m_c1 = ra;
+                        const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
                         c.m_c2 = ima * psb->m_sst.sdt;
-    //                    c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
                         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, t1, t2); // world frame to local frame
+                                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                                btVector3 u_dot_J(0,0,0);
+                                for (int i = 0; i < ndof; ++i)
+                                {
+                                    u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]);
+                                }
+                                btVector3 impulse_matrix_diag;
+                                btScalar dt = psb->m_sst.sdt;
+                                impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt));
+                                impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt));
+                                impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt));
+                                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);
-                        if (m_rigidBody)
-                            m_rigidBody->activate();
                     }
                 }
 			}