diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp
new file mode 100644
index 000000000..49fd3184b
--- /dev/null
+++ b/examples/DeformableDemo/DeformableClothAnchor.cpp
@@ -0,0 +1,236 @@
+/*
+ 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 "DeformableClothAnchor.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.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 DeformableClothAnchor shows contact between deformable objects and rigid objects.
+class DeformableClothAnchor : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+    DeformableClothAnchor(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase(helper)
+    {
+    }
+    virtual ~DeformableClothAnchor()
+    {
+    }
+    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);
+    }
+    
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* 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 DeformableClothAnchor::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)
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
+    m_solver = sol;
+    
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(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, -50, 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
+    {
+        const btScalar s = 4;
+        const btScalar h = 6;
+        const int r = 9;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s), r, r, 4 + 8, true);
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(1);
+        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);
+        
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,1, true);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        m_forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        m_forces.push_back(gravity_force);
+        
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0, h, -(s + 3.5)));
+        btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3)));
+        psb->appendDeformableAnchor(0, body);
+        psb->appendDeformableAnchor(r - 1, body);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableClothAnchor::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 forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    //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* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options)
+{
+    return new DeformableClothAnchor(options.m_guiHelper);
+}
+
+
diff --git a/examples/DeformableDemo/DeformableClothAnchor.h b/examples/DeformableDemo/DeformableClothAnchor.h
new file mode 100644
index 000000000..4e8388647
--- /dev/null
+++ b/examples/DeformableDemo/DeformableClothAnchor.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_CLOTH_ANCHOR_H
+#define DEFORMABLE_CLOTH_ANCHOR_H
+
+class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //DEFORMABLE_CLOTH_ANCHOR_H
diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp
index 12a81543b..3efa8e470 100644
--- a/examples/DeformableDemo/DeformableContact.cpp
+++ b/examples/DeformableDemo/DeformableContact.cpp
@@ -196,7 +196,7 @@ void DeformableContact::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb2, gravity_force2);
         m_forces.push_back(gravity_force2);
     }
-    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setImplicit(false);
     getDeformableDynamicsWorld()->setLineSearch(false);
     m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp
index d7f254960..7b9137cc9 100644
--- a/examples/DeformableDemo/DeformableMultibody.cpp
+++ b/examples/DeformableDemo/DeformableMultibody.cpp
@@ -145,7 +145,6 @@ void DeformableMultibody::initPhysics()
         m_dynamicsWorld->addRigidBody(body,1,1+2);
     }
 
-    
     {
         bool damping = true;
         bool gyro = false;
diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp
index 1bd437a49..748f1f4c8 100644
--- a/examples/DeformableDemo/DeformableRigid.cpp
+++ b/examples/DeformableDemo/DeformableRigid.cpp
@@ -231,7 +231,7 @@ void DeformableRigid::initPhysics()
         // add a few rigid bodies
         Ctor_RbUpStack(1);
     }
-    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setImplicit(false);
     getDeformableDynamicsWorld()->setLineSearch(false);
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp
index be09af17c..cb4f405c2 100644
--- a/examples/DeformableDemo/DeformableSelfCollision.cpp
+++ b/examples/DeformableDemo/DeformableSelfCollision.cpp
@@ -175,7 +175,7 @@ void DeformableSelfCollision::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
     }
-    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setImplicit(false);
     getDeformableDynamicsWorld()->setLineSearch(false);
     m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp
index 41f75a4cf..ddb47954b 100644
--- a/examples/DeformableDemo/GraspDeformable.cpp
+++ b/examples/DeformableDemo/GraspDeformable.cpp
@@ -190,6 +190,7 @@ void GraspDeformable::initPhysics()
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     // build a gripper
+    if(1)
     {
         bool damping = true;
         bool gyro = false;
@@ -269,17 +270,18 @@ void GraspDeformable::initPhysics()
     }
 
     // create a soft block
+    if(1)
     {
         char relative_path[1024];
 //        b3FileUtils::findFile("banana.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("ball.vtk", relative_path, 1024);
-//        b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
-        b3FileUtils::findFile("tube.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("tube.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("torus.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("bread.vtk", relative_path, 1024);
-//        b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
+        b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("boot.vtk", relative_path, 1024);
 //        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
 //                                                                  TetraCube::getElements(),
@@ -289,12 +291,13 @@ void GraspDeformable::initPhysics()
         btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path);
 
 //        psb->scale(btVector3(30, 30, 30)); // for banana
-//        psb->scale(btVector3(.2, .2, .2));
+        psb->scale(btVector3(.7, .7, .7));
 //        psb->scale(btVector3(2, 2, 2));
-        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
-//        psb->scale(btVector3(.1, .1, .1));  // for ditto
-//        psb->translate(btVector3(.25, 2, 0.4));
-        psb->getCollisionShape()->setMargin(0.01);
+//        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
+        psb->scale(btVector3(.1, .1, .1));  // for ditto
+//        psb->translate(btVector3(.25, 10, 0.4));
+        psb->getCollisionShape()->setMargin(0.0005);
+        psb->setMaxStress(50);
         psb->setTotalMass(.01);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
@@ -306,44 +309,49 @@ void GraspDeformable::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
 
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(10,20, 0.01);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
     }
-    
-//    // 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),
-//                                                         20,20,
-//                                                                  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.02);
-//        psb->generateBendingConstraints(2);
-//        psb->setTotalMass(.01);
-//        psb->setSpringStiffness(5);
-//        psb->setDampingCoefficient(0.05);
-//        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(.2,0.02, true));
-//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
-//    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+
+    // create a piece of cloth
+    if(0)
+    {
+        bool onGround = false;
+        const btScalar s = .1;
+        const btScalar h = 1;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s),
+                                                         20,20,
+                                                                  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.005);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(.01);
+        psb->setSpringStiffness(10);
+        psb->setDampingCoefficient(0.05);
+        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;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true));
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.05, false));
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+    }
     
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
     
diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp
new file mode 100644
index 000000000..eeeb4d33c
--- /dev/null
+++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp
@@ -0,0 +1,395 @@
+/*
+ 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 "MultibodyClothAnchor.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.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 MultibodyClothAnchor shows contact between deformable objects and rigid objects.
+class MultibodyClothAnchor : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+    MultibodyClothAnchor(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase(helper)
+    {
+    }
+    virtual ~MultibodyClothAnchor()
+    {
+    }
+    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);
+    }
+    
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        ///just make it a btSoftRigidDynamicsWorld please
+        ///or we will add type checking
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonRigidBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* 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());
+            }
+        }
+    }
+    
+    btMultiBody* createMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+    
+    void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+};
+
+void MultibodyClothAnchor::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)
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(deformableBodySolver);
+    m_solver = sol;
+    
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(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, -20, 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, -35, 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,1,1+2);
+    }
+    
+    // create a piece of cloth
+    {
+        const btScalar s = 4;
+        const btScalar h = 6;
+        const int r = 9;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s), r, r, 4 + 8, true);
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->generateBendingConstraints(2);
+        psb->setTotalMass(1);
+        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);
+        
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
+        getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+        m_forces.push_back(mass_spring);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        m_forces.push_back(gravity_force);
+        
+        bool damping = true;
+        bool gyro = false;
+        int numLinks = 5;
+        bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(1.5, .5, .5);
+        btVector3 baseHalfExtents(1.5, .5, .5);
+        
+        btMultiBody* mbC = createMultiBody(getDeformableDynamicsWorld(), numLinks, btVector3(s+3.5f, h, -s-0.6f), linkHalfExtents, baseHalfExtents, spherical, true);
+        
+        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(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
+
+        // quick hack: advance time to populate the variables in multibody
+        m_dynamicsWorld->stepSimulation(SIMD_EPSILON, 0);
+        btAlignedObjectArray<btQuaternion> scratch_q;
+        btAlignedObjectArray<btVector3> scratch_m;
+        mbC->forwardKinematics(scratch_q, scratch_m);
+        psb->appendDeformableAnchor(0, mbC->getLink(3).m_collider);
+        psb->appendDeformableAnchor(r - 1, mbC->getLink(0).m_collider);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void MultibodyClothAnchor::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 forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    //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;
+}
+
+btMultiBody* MultibodyClothAnchor::createMultiBody(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 = 1.f;
+
+    if (baseMass)
+    {
+        btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+        pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+        delete pTempBox;
+    }
+    
+    bool canSleep = false;
+    
+    btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+    
+    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    pMultiBody->setBasePos(basePosition);
+    pMultiBody->setWorldToBaseRot(baseOriQuat);
+    btVector3 vel(0, 0, 0);
+
+    //init the links
+    btVector3 hingeJointAxis(0, 1, 0);
+    float linkMass = 1.f;
+    btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+    
+    btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+    pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+    delete pTempBox;
+    
+    //y-axis assumed up
+    btVector3 parentComToCurrentCom(-linkHalfExtents[0] * 2.f, 0, 0);                      //par body's COM to cur body's COM offset
+    btVector3 currentPivotToCurrentCom(-linkHalfExtents[0], 0, 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->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 MultibodyClothAnchor::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+    btAlignedObjectArray<btQuaternion> world_to_local;
+    world_to_local.resize(pMultiBody->getNumLinks() + 1);
+    
+    btAlignedObjectArray<btVector3> local_origin;
+    local_origin.resize(pMultiBody->getNumLinks() + 1);
+    world_to_local[0] = pMultiBody->getWorldToBaseRot();
+    local_origin[0] = pMultiBody->getBasePos();
+    
+    {
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+
+        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(1);
+        pMultiBody->setBaseCollider(col);
+    }
+
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        const int parent = pMultiBody->getParent(i);
+        world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+        local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+    }
+
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        btVector3 posr = local_origin[i + 1];
+        
+        btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
+        
+        btCollisionShape* box = new btBoxShape(linkHalfExtents);
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+        
+        col->setCollisionShape(box);
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        col->setWorldTransform(tr);
+        col->setFriction(1);
+        pWorld->addCollisionObject(col, 2, 1+2);
+        pMultiBody->getLink(i).m_collider = col;
+    }
+}
+
+class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options)
+{
+    return new MultibodyClothAnchor(options.m_guiHelper);
+}
+
+
diff --git a/examples/DeformableDemo/MultibodyClothAnchor.h b/examples/DeformableDemo/MultibodyClothAnchor.h
new file mode 100644
index 000000000..818e043c1
--- /dev/null
+++ b/examples/DeformableDemo/MultibodyClothAnchor.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 MULTIBODY_CLOTH_ANCHOR_H
+#define MULTIBODY_CLOTH_ANCHOR_H
+
+class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //MULTIBODY_CLOTH_ANCHOR_H
diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp
index 72e864ac3..1cbc7e43d 100644
--- a/examples/DeformableDemo/PinchFriction.cpp
+++ b/examples/DeformableDemo/PinchFriction.cpp
@@ -350,7 +350,7 @@ void PinchFriction::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb3, neohookean);
         m_forces.push_back(neohookean);
     }
-    
+    getDeformableDynamicsWorld()->setImplicit(false);
     // add a pair of grippers
     createGrip();
     m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp
index c1213d520..d3a37d5bf 100644
--- a/examples/DeformableDemo/VolumetricDeformable.cpp
+++ b/examples/DeformableDemo/VolumetricDeformable.cpp
@@ -228,7 +228,7 @@ void VolumetricDeformable::initPhysics()
         m_forces.push_back(neohookean);
         
     }
-    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setImplicit(false);
     getDeformableDynamicsWorld()->setLineSearch(false);
     // add a few rigid bodies
     Ctor_RbUpStack(4); 
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 7c31c9d84..56eecf328 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -371,6 +371,10 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/DeformableRigid.h
   ../DeformableDemo/VolumetricDeformable.cpp
   ../DeformableDemo/VolumetricDeformable.h
+  ../DeformableDemo/DeformableClothAnchor.cpp
+  ../DeformableDemo/DeformableClothAnchor.h
+  ../DeformableDemo/MultibodyClothAnchor.cpp
+  ../DeformableDemo/MultibodyClothAnchor.h
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index e705fd075..191d62e58 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -53,6 +53,8 @@
 #include "../DeformableDemo/VolumetricDeformable.h"
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
+#include "../DeformableDemo/DeformableClothAnchor.h"
+#include "../DeformableDemo/MultibodyClothAnchor.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -198,6 +200,8 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
+        ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
+        ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
         ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
         // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
         
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 1963c3473..085a0384b 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -348,12 +348,13 @@ B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle co
 	return 0;
 }
 
-B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda)
+B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
         command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu;
         command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda;
+        command->m_loadSoftBodyArguments.m_NeoHookeanDamping = NeoHookeanDamping;
 	command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE;
 	return 0;
 }
@@ -385,6 +386,15 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle
 	return 0;
 }
 
+B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
+        command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision;
+	command->m_updateFlags |= LOAD_SOFT_BODY_SET_SELF_COLLISION;
+	return 0;
+}
+
 B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 64ef41bc7..752789428 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -632,10 +632,11 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
 	B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
 	B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
-	B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda);
+	B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping);
 	B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
 	B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
 	B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
+	B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision);
 	B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
 	B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings);
 
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 78087a686..9e5799c36 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1631,6 +1631,7 @@ struct PhysicsServerCommandProcessorInternalData
 #ifndef SKIP_DEFORMABLE_BODY
 	btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld;
 	btDeformableBodySolver* m_deformablebodySolver;
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
 #else
 	btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
 	btSoftBodySolver* m_softbodySolver;
@@ -2790,6 +2791,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 			m_data->m_dynamicsWorld->removeMultiBody(mb);
 			delete mb;
 		}
+#ifndef SKIP_DEFORMABLE_BODY
+        for (int j = 0; j < m_data->m_lf.size(); j++)
+        {
+            btDeformableLagrangianForce* force = m_data->m_lf[j];
+            delete force;
+        }
+        m_data->m_lf.clear();
+#endif
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 		for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
 		{
@@ -8019,7 +8028,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	const std::string& error_message_prefix = "";
 	std::string out_found_filename;
 	std::string out_found_sim_filename;
-	int out_type, out_sim_type;
+	int out_type(0), out_sim_type(0);
 
         bool render_mesh_is_sim_mesh = true;
 
@@ -8066,7 +8075,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	    {
             spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
             spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
-            m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
+            btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false);
+            m_data->m_dynamicsWorld->addForce(psb, springForce);
+            m_data->m_lf.push_back(springForce);
 	    }
 #endif
         }
@@ -8077,23 +8088,30 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
             btScalar corotated_mu, corotated_lambda;
             if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
             {
-                    corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
-                    corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
-                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
+                corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
+                corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
+                btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
+                m_data->m_dynamicsWorld->addForce(psb, corotatedForce);
+                m_data->m_lf.push_back(corotatedForce);
             }
-            btScalar neohookean_mu, neohookean_lambda;
+            btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
             if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
             {
-                    neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
-                    neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
-                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
+                neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
+                neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
+                neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
+                btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
+                m_data->m_dynamicsWorld->addForce(psb, neohookeanForce);
+                m_data->m_lf.push_back(neohookeanForce);
             }
             btScalar spring_elastic_stiffness, spring_damping_stiffness;
             if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
             {
-                    spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
-                    spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
-                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
+                spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
+                spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
+                btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true);
+                m_data->m_dynamicsWorld->addForce(psb, springForce);
+                m_data->m_lf.push_back(springForce);
             }
 #endif
 		}
@@ -8112,10 +8130,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
                 psb->m_renderNodes.resize(0);
             }
             btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
-            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
-            {
-                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
-            }
+            btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
+            m_data->m_dynamicsWorld->addForce(psb, gravityForce);
+            m_data->m_lf.push_back(gravityForce);
             btScalar collision_hardness = 1;
             psb->m_cfg.kKHR = collision_hardness;
             psb->m_cfg.kCHR = collision_hardness;
@@ -8145,7 +8162,13 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
             // collion between deformable and deformable and self-collision
             psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
             psb->setCollisionFlags(0);
-	    psb->setTotalMass(mass);
+            psb->setTotalMass(mass);
+            bool use_self_collision = false;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_SELF_COLLISION)
+            {
+                    use_self_collision = loadSoftBodyArgs.m_useSelfCollision;
+            }
+            psb->setSelfCollision(use_self_collision);
 #else
 	    btSoftBody::Material* pm = psb->appendMaterial();
 	    pm->m_kLST = 0.5;
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 74e79537a..d23ac1594 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -501,6 +501,7 @@ enum EnumLoadSoftBodyUpdateFlags
         LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10,
         LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11,
         LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
+        LOAD_SOFT_BODY_SET_SELF_COLLISION = 1<<13,
 };
 
 enum EnumSimParamInternalSimFlags
@@ -525,9 +526,11 @@ struct LoadSoftBodyArgs
         double m_corotatedLambda;
         bool m_useBendingSprings;
         double m_collisionHardness;
+        double m_useSelfCollision;
         double m_frictionCoeff;
         double m_NeoHookeanMu;
         double m_NeoHookeanLambda;
+        double m_NeoHookeanDamping;
 };
 
 struct b3LoadSoftBodyResultArgs
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index a73199f90..bd51e584b 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -27,12 +27,12 @@ class btConjugateGradient
     typedef btAlignedObjectArray<btVector3> TVStack;
     TVStack r,p,z,temp;
     int max_iterations;
-    btScalar tolerance;
+    btScalar tolerance_squared;
 public:
     btConjugateGradient(const int max_it_in)
     : max_iterations(max_it_in)
     {
-       tolerance = 1024 * std::numeric_limits<btScalar>::epsilon();
+       tolerance_squared = 1e-5;
     }
     
     virtual ~btConjugateGradient(){}
@@ -51,8 +51,7 @@ public:
         A.precondition(r, z);
         A.project(z);
         btScalar r_dot_z = dot(z,r);
-        btScalar local_tolerance = tolerance;
-        if (std::sqrt(r_dot_z) <= local_tolerance) {
+        if (r_dot_z <= tolerance_squared) {
             if (verbose)
             {
                 std::cout << "Iteration = 0" << std::endl;
@@ -66,7 +65,6 @@ public:
             // temp = A*p
             A.multiply(p, temp);
             A.project(temp);
-            // alpha = r^T * z / (p^T * A * p)
             if (dot(p,temp) < SIMD_EPSILON)
             {
                 if (verbose)
@@ -77,6 +75,7 @@ public:
                 }
               return k;
             }
+            // alpha = r^T * z / (p^T * A * p)
             btScalar alpha = r_dot_z_new / dot(p, temp);
             //  x += alpha * p;
             multAndAddTo(alpha, p, x);
@@ -86,7 +85,7 @@ public:
             A.precondition(r, z);
             r_dot_z = r_dot_z_new;
             r_dot_z_new = dot(r,z);
-            if (std::sqrt(r_dot_z_new) < local_tolerance) {
+            if (r_dot_z_new < tolerance_squared) {
                 if (verbose)
                 {
                     std::cout << "ConjugateGradient iterations " << k << std::endl;
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 49232725a..fee353a60 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -101,6 +101,10 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
         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;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 10d3775d3..9af8e3f77 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -18,13 +18,13 @@
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyInternals.h"
 #include "LinearMath/btQuickprof.h"
-
+static const int kMaxConjugateGradientIterations  = 200;
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
-, m_cg(20)
+, m_cg(kMaxConjugateGradientIterations)
 , m_maxNewtonIterations(5)
 , m_newtonTolerance(1e-4)
-, m_lineSearch(true)
+, m_lineSearch(false)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
@@ -101,6 +101,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
                 m_residual[j].setZero();
             }
         }
+        updateVelocity();
     }
 }
 
@@ -199,7 +200,7 @@ void btDeformableBodySolver::updateDv(btScalar scale)
 
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
-    m_cg.solve(*m_objective, ddv, residual, false);
+    m_cg.solve(*m_objective, ddv, residual);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@@ -247,6 +248,11 @@ void btDeformableBodySolver::updateVelocity()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        psb->m_maxSpeedSquared = 0;
+        if (!psb->isActive())
+        {
+            continue;
+        }
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             // set NaN to zero;
@@ -255,6 +261,7 @@ void btDeformableBodySolver::updateVelocity()
                 m_dv[counter].setZero();
             }
             psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
+            psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
             ++counter;
         }
     }
@@ -266,6 +273,10 @@ void btDeformableBodySolver::updateTempPosition()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
@@ -294,13 +305,23 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             if (implicit)
             {
+                if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON)
+                    m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+                else
+                    m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn;
                 m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
             }
-            m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+            else
+                m_dv[counter] =  psb->m_nodes[j].m_v - m_backupVelocity[counter];
+            psb->m_nodes[j].m_v = m_backupVelocity[counter];
             ++counter;
         }
     }
@@ -335,14 +356,14 @@ bool btDeformableBodySolver::updateNodes()
 
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
+    // apply explicit forces to velocity
+    m_objective->applyExplicitForce(m_residual);
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody *psb = m_softBodies[i];
         
         if (psb->isActive())
         {
-            // apply explicit forces to velocity
-            m_objective->applyExplicitForce(m_residual);
             // predict motion for collision detection
             predictDeformableMotion(psb, solverdt);
         }
@@ -371,14 +392,26 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
     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;
+    /* Bounds                */
+    psb->updateBounds();
+    
     /* Integrate            */
+    // do not allow particles to move more than the bounding box size
+    btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt;
     for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
     {
         btSoftBody::Node& n = psb->m_nodes[i];
+        // apply drag
+        n.m_v *= (1 - psb->m_cfg.drag);
+        // scale velocity back
+        if (n.m_v.norm() > max_v)
+        {
+            n.m_v.safeNormalize();
+            n.m_v *= max_v;
+        }
         n.m_q = n.m_x + n.m_v * dt;
     }
-    /* Bounds                */
-    psb->updateBounds();
+
     /* Nodes                */
     ATTRIBUTE_ALIGNED16(btDbvtVolume)
     vol;
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index ce5018f2f..91a6c7d20 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -15,6 +15,125 @@
 
 #include "btDeformableContactConstraint.h"
 
+/* ================   Deformable Node Anchor   =================== */
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
+: m_anchor(&a)
+, btDeformableContactConstraint(a.m_cti.m_normal)
+{
+}
+
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
+: m_anchor(other.m_anchor)
+, btDeformableContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVa() const
+{
+    const btSoftBody::sCti& cti = m_anchor->m_cti;
+    btVector3 va(0, 0, 0);
+    if (cti.m_colObj->hasContactResponse())
+    {
+        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(m_anchor->m_c1)) : 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 = &m_anchor->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0];
+                const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+                // add in the normal component of the va
+                btScalar vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_n[k];
+                }
+                va = cti.m_normal * vel;
+                // add in the tangential components of the va
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t1[k];
+                }
+                va += m_anchor->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                }
+                va += m_anchor->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
+btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+{
+    const btSoftBody::sCti& cti = m_anchor->m_cti;
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = (vb - va);
+    // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
+    const btScalar dn = btDot(vr, cti.m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_anchor->m_c0 * vr;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    
+    // apply impulse to the rigid/multibodies involved and change their velocities
+    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+    {
+        btRigidBody* rigidCol = 0;
+        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+        if (rigidCol)
+        {
+            rigidCol->applyImpulse(impulse, m_anchor->m_c1);
+        }
+    }
+    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+    {
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+        if (multibodyLinkCol)
+        {
+            const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+            // apply tangential component of the impulse
+            const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
+            const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
+        }
+    }
+    return residualSquare;
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVb() const
+{
+    return m_anchor->m_node->m_v;
+}
+
+void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
+{
+    btVector3 dv = impulse * m_anchor->m_c2;
+    m_anchor->m_node->m_v -= dv;
+}
+
 /* ================   Deformable vs. Rigid   =================== */
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
 : m_contact(&c)
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h
index f79bc6065..da16c51e5 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.h
+++ b/src/BulletSoftBody/btDeformableContactConstraint.h
@@ -108,6 +108,32 @@ public:
     virtual void applyImpulse(const btVector3& impulse){}
 };
 
+//
+// Anchor Constraint between rigid and deformable node
+class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
+{
+public:
+    const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
+    
+    btDeformableNodeAnchorConstraint(){}
+    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
+    btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+    virtual ~btDeformableNodeAnchorConstraint()
+    {
+    }
+    virtual btScalar solveConstraint();
+    // object A is the rigid/multi body, and object B is the deformable node/face
+    virtual btVector3 getVa() const;
+    // get the velocity of the deformable node in contact
+    virtual btVector3 getVb() const;
+    virtual btVector3 getDv(const btSoftBody::Node* n) const
+    {
+        return btVector3(0,0,0);
+    }
+    virtual void applyImpulse(const btVector3& impulse);
+};
+
+
 //
 // Constraint between rigid/multi body and deformable objects
 class btDeformableRigidContactConstraint : public btDeformableContactConstraint
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index e340a3fd5..d438d6eec 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update()
         }
     }
     
+    // anchor constraints
+    for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
+    {
+        btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
+        btScalar localResidualSquare = constraint.solveConstraint();
+        residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+    
     // face constraints
     for (int index = 0; index < m_allFaceConstraints.size(); ++index)
     {
@@ -51,6 +59,10 @@ void btDeformableContactProjection::setConstraints()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             if (psb->m_nodes[j].m_im == 0)
@@ -61,10 +73,33 @@ void btDeformableContactProjection::setConstraints()
         }
     }
     
-    // set Deformable Node vs. Rigid constraint
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
+
+        // set up deformable anchors
+        for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+        {
+            btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
+            // skip fixed points
+            if (anchor.m_node->m_im == 0)
+            {
+                continue;
+            }
+            
+            if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
+            {
+                anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
+                btDeformableNodeAnchorConstraint constraint(anchor);
+                m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint);
+            }
+        }
+        
+        // set Deformable Node vs. Rigid constraint
         for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
         {
             const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
@@ -242,6 +277,10 @@ void btDeformableContactProjection::setProjection()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
+        if (!psb->isActive())
+        {
+            continue;
+        }
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             int index = psb->m_nodes[j].index;
@@ -249,7 +288,7 @@ void btDeformableContactProjection::setProjection()
             bool existStaticConstraint = false;
             btVector3 averagedNormal(0,0,0);
             btAlignedObjectArray<btVector3> normals;
-            if (m_staticConstraints.find(index) != NULL)
+            if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
             {
                 existStaticConstraint = true;
                 hasConstraint = true;
@@ -438,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
     m_staticConstraints.clear();
+    m_nodeAnchorConstraints.clear();
     m_nodeRigidConstraints.clear();
     m_faceRigidConstraints.clear();
     m_deformableConstraints.clear();
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index fa5eb662b..90386b0c9 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -36,6 +36,8 @@ public:
     btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
     // map from node index to deformable constraint
     btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
+    // map from node index to node anchor constraint
+    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
     
     // all constraints involving face
     btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
index d9bef8b07..33e5a8564 100644
--- a/src/BulletSoftBody/btDeformableGravityForce.h
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -57,6 +57,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 btSoftBody::Node& n = psb->m_nodes[j];
@@ -80,6 +84,10 @@ public:
         for (int i = 0; i<m_softBodies.size();++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 const btSoftBody::Node& node = psb->m_nodes[j];
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 9e7b3594c..44ac91c62 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -51,6 +51,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
@@ -83,6 +87,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
@@ -108,6 +116,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             btScalar scaled_k_damp = m_dampingStiffness * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
@@ -138,6 +150,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
@@ -160,6 +176,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 sz = btMax(sz, psb->m_nodes[j].index);
@@ -188,6 +208,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             btScalar scaled_k = m_elasticStiffness * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
index 362451c0e..1e0d53a41 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp
@@ -23,7 +23,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
         ///this is a special step to resolve penetrations (just for contacts)
         solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
         
-        m_maxOverrideNumSolverIterations = 50;
         int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
         for (int iteration = 0; iteration < maxIterations; iteration++)
         {
@@ -33,10 +32,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
             m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
             // solver body velocity -> rigid body velocity
             solverBodyWriteBack(infoGlobal);
-            
+            btScalar deformableResidual = m_deformableSolver->solveContactConstraints();
             // update rigid body velocity in rigid/deformable contact
-            m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints());
-            
+            m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
             // solver body velocity <- rigid body velocity
             writeToSolverBody(bodies, numBodies, infoGlobal);
             
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 595ed737c..9f8e13e20 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -33,7 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include "btDeformableBodySolver.h"
 #include "LinearMath/btQuickprof.h"
-
+#include "btSoftBodyInternals.h"
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     BT_PROFILE("internalSingleStepSimulation");
@@ -70,26 +70,52 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     ///update vehicle simulation
     btMultiBodyDynamicsWorld::updateActions(timeStep);
     
-    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+    updateActivationState(timeStep);
     // End solver-wise simulation step
     // ///////////////////////////////
 }
 
+void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
+{
+    for (int i = 0; i < m_softBodies.size(); i++)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        psb->updateDeactivation(timeStep);
+        if (psb->wantsSleeping())
+        {
+            if (psb->getActivationState() == ACTIVE_TAG)
+                psb->setActivationState(WANTS_DEACTIVATION);
+            if (psb->getActivationState() == ISLAND_SLEEPING)
+            {
+                psb->setZeroVelocity();
+            }
+        }
+        else
+        {
+            if (psb->getActivationState() != DISABLE_DEACTIVATION)
+                psb->setActivationState(ACTIVE_TAG);
+        }
+    }
+    btMultiBodyDynamicsWorld::updateActivationState(timeStep);
+}
+
+
 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
     m_deformableBodySolver->updateSoftBodies();
     for (int i = 0; i < m_softBodies.size(); i++)
     {
-        btSoftBody* psb = (btSoftBody*)m_softBodies[i];
-        psb->defaultCollisionHandler(psb);
+        btSoftBody* psb = m_softBodies[i];
+        if (psb->isActive())
+        {
+            psb->defaultCollisionHandler(psb);
+        }
     }
 }
 
 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
     BT_PROFILE("integrateTransforms");
-    //m_deformableBodySolver->backupVelocity();
-    //positionCorrection(timeStep);  // looks like position correction is no longer necessary
     btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
@@ -114,18 +140,66 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
             node.m_q = node.m_x;
             node.m_vn = node.m_v;
         }
+        // enforce anchor constraints
+        for (int j = 0; j < psb->m_deformableAnchors.size();++j)
+        {
+            btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+            btSoftBody::Node* n = a.m_node;
+            n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+            
+            // update multibody anchor info
+            if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+            {
+                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+                if (multibodyLinkCol)
+                {
+                    btVector3 nrm;
+                    const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+                    const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+                    psb->m_worldInfo->m_sparsesdf.Evaluate(
+                                                      wtr.invXform(n->m_x),
+                                                      shp,
+                                                      nrm,
+                                                      0);
+                    a.m_cti.m_normal = wtr.getBasis() * nrm;
+                    btVector3 normal = a.m_cti.m_normal;
+                    btVector3 t1 = generateUnitOrthogonalVector(normal);
+                    btVector3 t2 = btCross(normal, t1);
+                    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+                    findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+                    findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+                    findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, 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;
+                    btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+                    a.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
+                    a.jacobianData_normal = jacobianData_normal;
+                    a.jacobianData_t1 = jacobianData_t1;
+                    a.jacobianData_t2 = jacobianData_t2;
+                    a.t1 = t1;
+                    a.t2 = t2;
+                }
+            }
+        }
         psb->interpolateRenderMesh();
     }
-    //m_deformableBodySolver->revertVelocity();
 }
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-    if (!m_implicit)
-    {
-        // save v_{n+1}^* velocity after explicit forces
-        m_deformableBodySolver->backupVelocity();
-    }
+    // save v_{n+1}^* velocity after explicit forces
+    m_deformableBodySolver->backupVelocity();
     
     // set up constraints among multibodies and between multibodies and deformable bodies
     setupConstraints();
@@ -386,3 +460,12 @@ void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
     // force a reinitialize so that node indices get updated.
     m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
 }
+
+void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+    btSoftBody* body = btSoftBody::upcast(collisionObject);
+    if (body)
+        removeSoftBody(body);
+    else
+        btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
index e66e060f2..834c6c8b1 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
@@ -62,6 +62,8 @@ protected:
     
     void solveConstraints(btScalar timeStep);
     
+    void updateActivationState(btScalar timeStep);
+
 public:
     btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
     : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
@@ -83,7 +85,8 @@ public:
         m_sbi.water_normal = btVector3(0, 0, 0);
         m_sbi.m_gravity.setValue(0, -10, 0);
         m_internalTime = 0.0;
-        m_implicit = true;
+        m_implicit = false;
+        m_lineSearch = false;
         m_selfCollision = true;
     }
 
@@ -147,6 +150,8 @@ public:
     
     void removeSoftBody(btSoftBody* body);
     
+    void removeCollisionObject(btCollisionObject* collisionObject);
+    
     int getDrawFlags() const { return (m_drawFlags); }
     void setDrawFlags(int f) { m_drawFlags = f; }
     
diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
index c40aaf4a7..b113b4546 100644
--- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h
+++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
@@ -18,6 +18,7 @@ subject to the following restrictions:
 
 #include "btDeformableLagrangianForce.h"
 #include "LinearMath/btQuickprof.h"
+#include "LinearMath/btImplicitQRSVD.h"
 // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
 class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
 {
@@ -32,7 +33,7 @@ public:
         m_lambda_damp = damping * m_lambda;
     }
     
-    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda)
+    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
     {
         m_mu_damp = damping * m_mu;
         m_lambda_damp = damping * m_lambda;
@@ -60,6 +61,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_tetras.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
@@ -72,8 +77,10 @@ public:
                 size_t id2 = node2->index;
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
-                btMatrix3x3 dP;
-                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+                btMatrix3x3 I;
+                I.setIdentity();
+                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
+//                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
                 btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 
@@ -93,6 +100,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
@@ -111,6 +122,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 sz = btMax(sz, psb->m_nodes[j].index);
@@ -150,11 +165,35 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
+            btScalar max_p = psb->m_cfg.m_maxStress;
             for (int j = 0; j < psb->m_tetras.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
                 btMatrix3x3 P;
                 firstPiola(psb->m_tetraScratches[j],P);
+                
+                btMatrix3x3 U, V;
+                btVector3 sigma;
+                singularValueDecomposition(P, U, sigma, V);
+                if (max_p > 0)
+                {
+                    sigma[0] = btMin(sigma[0], max_p);
+                    sigma[1] = btMin(sigma[1], max_p);
+                    sigma[2] = btMin(sigma[2], max_p);
+                    sigma[0] = btMax(sigma[0], -max_p);
+                    sigma[1] = btMax(sigma[1], -max_p);
+                    sigma[2] = btMax(sigma[2], -max_p);
+                }
+                btMatrix3x3 Sigma;
+                Sigma.setIdentity();
+                Sigma[0][0] = sigma[0];
+                Sigma[1][1] = sigma[1];
+                Sigma[2][2] = sigma[2];
+                P = U * Sigma * V.transpose();
 //                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose();
                 btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col;
@@ -189,6 +228,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_tetras.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
@@ -201,8 +244,10 @@ public:
                 size_t id2 = node2->index;
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
-                btMatrix3x3 dP;
-                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
+                btMatrix3x3 I;
+                I.setIdentity();
+                btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp;
+//                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
 //                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
                 btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
@@ -225,6 +270,10 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
+            if (!psb->isActive())
+            {
+                continue;
+            }
             for (int j = 0; j < psb->m_tetras.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 0da85ff43..c46f7a3b8 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -89,6 +89,8 @@ void btSoftBody::initDefaults()
 	m_cfg.piterations = 1;
 	m_cfg.diterations = 0;
 	m_cfg.citerations = 4;
+    m_cfg.drag = 0;
+    m_cfg.m_maxStress = 0;
 	m_cfg.collisions = fCollision::Default;
     m_cfg.collisions |= fCollision::VF_DD;
 	m_pose.m_bvolume = false;
@@ -115,6 +117,9 @@ void btSoftBody::initDefaults()
 	m_windVelocity = btVector3(0, 0, 0);
 	m_restLengthScale = btScalar(1.0);
     m_dampingCoefficient = 1;
+    m_sleepingThreshold = 0.1;
+    m_useFaceContact = false;
+    m_collisionFlags = 0;
 }
 
 //
@@ -406,6 +411,98 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
 	m_anchors.push_back(a);
 }
 
+//
+void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
+{
+    DeformableNodeRigidAnchor c;
+    btSoftBody::Node& n = m_nodes[node];
+    const btScalar ima = n.m_im;
+    const btScalar imb = body->getInvMass();
+    btVector3 nrm;
+    const btCollisionShape* shp = body->getCollisionShape();
+    const btTransform& wtr = body->getWorldTransform();
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(m_nodes[node].m_x),
+                                      shp,
+                                      nrm,
+                                      0);
+
+    c.m_cti.m_colObj = body;
+    c.m_cti.m_normal = wtr.getBasis() * nrm;
+    c.m_cti.m_offset = dst;
+    c.m_node = &m_nodes[node];
+    const btScalar fc = m_cfg.kDF * body->getFriction();
+    c.m_c2 = ima;
+    c.m_c3 = fc;
+    c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
+    const btVector3 ra = n.m_x - wtr.getOrigin();
+
+    c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+    c.m_c1 = ra;
+    c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+    c.m_node->m_battach = 1;
+    m_deformableAnchors.push_back(c);
+}
+
+//
+void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
+{
+    DeformableNodeRigidAnchor c;
+    btSoftBody::Node& n = m_nodes[node];
+    const btScalar ima = n.m_im;
+    btVector3 nrm;
+    const btCollisionShape* shp = link->getCollisionShape();
+    const btTransform& wtr = link->getWorldTransform();
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(m_nodes[node].m_x),
+                                      shp,
+                                      nrm,
+                                      0);
+    c.m_cti.m_colObj = link;
+    c.m_cti.m_normal = wtr.getBasis() * nrm;
+    c.m_cti.m_offset = dst;
+    c.m_node = &m_nodes[node];
+    const btScalar fc = m_cfg.kDF * link->getFriction();
+    c.m_c2 = ima;
+    c.m_c3 = fc;
+    c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+    btVector3 normal = c.m_cti.m_normal;
+    btVector3 t1 = generateUnitOrthogonalVector(normal);
+    btVector3 t2 = btCross(normal, t1);
+    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+    findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
+    findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
+    findJacobian(link, jacobianData_t2, c.m_node->m_x, 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 = link->m_multiBody->getNumDofs() + 6;
+    btMatrix3x3 local_impulse_matrix = (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;
+    const btVector3 ra = n.m_x - wtr.getOrigin();
+    c.m_c1 = ra;
+    c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
+    c.m_node->m_battach = 1;
+    m_deformableAnchors.push_back(c);
+}
 //
 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
 {
@@ -2306,10 +2403,10 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
     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
-    btTransform wtr = (predict) ?
-    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-                 : colObjWrap->getWorldTransform();
-    //const btTransform& wtr = colObjWrap->getWorldTransform();
+//    btTransform wtr = (predict) ?
+//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+//                 : colObjWrap->getWorldTransform();
+    const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2427,31 +2524,63 @@ void btSoftBody::updateBounds()
 		m_bounds[1] = btVector3(1000, 1000, 1000);
 
 	} else {*/
-	if (m_ndbvt.m_root)
-	{
-		const btVector3& mins = m_ndbvt.m_root->volume.Mins();
-		const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
-		const btScalar csm = getCollisionShape()->getMargin();
-		const btVector3 mrg = btVector3(csm,
-										csm,
-										csm) *
-							  1;  // ??? to investigate...
-		m_bounds[0] = mins - mrg;
-		m_bounds[1] = maxs + mrg;
-		if (0 != getBroadphaseHandle())
-		{
-			m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
-											   m_bounds[0],
-											   m_bounds[1],
-											   m_worldInfo->m_dispatcher);
-		}
-	}
-	else
-	{
-		m_bounds[0] =
-			m_bounds[1] = btVector3(0, 0, 0);
-	}
-	//}
+//    if (m_ndbvt.m_root)
+//    {
+//        const btVector3& mins = m_ndbvt.m_root->volume.Mins();
+//        const btVector3& maxs = m_ndbvt.m_root->volume.Maxs();
+//        const btScalar csm = getCollisionShape()->getMargin();
+//        const btVector3 mrg = btVector3(csm,
+//                                        csm,
+//                                        csm) *
+//                              1;  // ??? to investigate...
+//        m_bounds[0] = mins - mrg;
+//        m_bounds[1] = maxs + mrg;
+//        if (0 != getBroadphaseHandle())
+//        {
+//            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+//                                               m_bounds[0],
+//                                               m_bounds[1],
+//                                               m_worldInfo->m_dispatcher);
+//        }
+//    }
+//    else
+//    {
+//        m_bounds[0] =
+//            m_bounds[1] = btVector3(0, 0, 0);
+//    }
+    if (m_nodes.size())
+    {
+        btVector3 mins = m_nodes[0].m_x;
+        btVector3 maxs = m_nodes[0].m_x;
+        for (int i = 1; i < m_nodes.size(); ++i)
+        {
+            for (int d = 0; d < 3; ++d)
+            {
+                if (m_nodes[i].m_x[d] > maxs[d])
+                    maxs[d] = m_nodes[i].m_x[d];
+                if (m_nodes[i].m_x[d] < mins[d])
+                    mins[d] = m_nodes[i].m_x[d];
+            }
+        }
+        const btScalar csm = getCollisionShape()->getMargin();
+        const btVector3 mrg = btVector3(csm,
+                                        csm,
+                                        csm);
+        m_bounds[0] = mins - mrg;
+        m_bounds[1] = maxs + mrg;
+        if (0 != getBroadphaseHandle())
+        {
+            m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(),
+                                               m_bounds[0],
+                                               m_bounds[1],
+                                               m_worldInfo->m_dispatcher);
+        }
+    }
+    else
+    {
+        m_bounds[0] =
+        m_bounds[1] = btVector3(0, 0, 0);
+    }
 }
 
 //
@@ -3162,6 +3291,12 @@ void btSoftBody::applyForces()
 	}
 }
 
+//
+void btSoftBody::setMaxStress(btScalar maxStress)
+{
+    m_cfg.m_maxStress = maxStress;
+}
+
 //
 void btSoftBody::interpolateRenderMesh()
 {
@@ -3378,6 +3513,16 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver)
 	return (0);
 }
 
+void btSoftBody::setSelfCollision(bool useSelfCollision)
+{
+    m_useSelfCollision = useSelfCollision;
+}
+
+bool btSoftBody::useSelfCollision()
+{
+   return m_useSelfCollision;
+}
+
 //
 void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap)
 {
@@ -3420,35 +3565,41 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
         {
             
             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));
-            btSoftColliders::CollideSDF_RD docollideNode;
-            docollideNode.psb = this;
-            docollideNode.m_colObj1Wrap = pcoWrap;
-            docollideNode.m_rigidBody = prb1;
-            docollideNode.dynmargin = basemargin + timemargin;
-            docollideNode.stamargin = basemargin;
-            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
-            
-            btSoftColliders::CollideSDF_RDF docollideFace;
-            docollideFace.psb = this;
-            docollideFace.m_colObj1Wrap = pcoWrap;
-            docollideFace.m_rigidBody = prb1;
-            docollideFace.dynmargin = basemargin + timemargin;
-            docollideFace.stamargin = basemargin;
-            m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+            if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
+            {
+                const btTransform wtr = pcoWrap->getWorldTransform();
+//                const btTransform ctr = pcoWrap->getWorldTransform();
+//                const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+                const btScalar timemargin = 0;
+                const btScalar basemargin = getCollisionShape()->getMargin();
+                btVector3 mins;
+                btVector3 maxs;
+                ATTRIBUTE_ALIGNED16(btDbvtVolume)
+                volume;
+                pcoWrap->getCollisionShape()->getAabb(wtr,
+                                                      mins,
+                                                      maxs);
+                volume = btDbvtVolume::FromMM(mins, maxs);
+                volume.Expand(btVector3(basemargin, basemargin, basemargin));
+                btSoftColliders::CollideSDF_RD docollideNode;
+                docollideNode.psb = this;
+                docollideNode.m_colObj1Wrap = pcoWrap;
+                docollideNode.m_rigidBody = prb1;
+                docollideNode.dynmargin = basemargin + timemargin;
+                docollideNode.stamargin = basemargin;
+                m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+                
+                if (this->m_useFaceContact)
+                {
+                    btSoftColliders::CollideSDF_RDF docollideFace;
+                    docollideFace.psb = this;
+                    docollideFace.m_colObj1Wrap = pcoWrap;
+                    docollideFace.m_rigidBody = prb1;
+                    docollideFace.dynmargin = basemargin + timemargin;
+                    docollideFace.stamargin = basemargin;
+                    m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+                }
+            }
         }
         break;
 	}
@@ -3542,48 +3693,43 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 		break;
         case fCollision::VF_DD:
         {
-            if (this != psb)
+            if (psb->isActive() || this->isActive())
             {
-                btSoftColliders::CollideVF_DD docollide;
-                /* common                    */
-                docollide.mrg = getCollisionShape()->getMargin() +
-                psb->getCollisionShape()->getMargin();
-                /* psb0 nodes vs psb1 faces    */
-                docollide.psb[0] = this;
-                docollide.psb[1] = psb;
-                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-                                                    docollide.psb[1]->m_fdbvt.m_root,
-                                                    docollide);
-                /* psb1 nodes vs psb0 faces    */
-                docollide.psb[0] = psb;
-                docollide.psb[1] = this;
-                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
-                                                    docollide.psb[1]->m_fdbvt.m_root,
-                                                    docollide);
-            }
-            else
-            {
-                btSoftColliders::CollideFF_DD docollide;
-                docollide.mrg = getCollisionShape()->getMargin() +
-                psb->getCollisionShape()->getMargin();
-                docollide.psb[0] = this;
-                docollide.psb[1] = psb;
-                /* psb0 faces vs psb0 faces    */
-                btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
-                calculateNormalCone(root);
-                this->m_fdbvt.selfCollideT(root,docollide);
-                delete root;
-                
-//                btSoftColliders::CollideFF_DD docollide;
-//                /* common                    */
-//                docollide.mrg = getCollisionShape()->getMargin() +
-//                psb->getCollisionShape()->getMargin();
-//                /* psb0 nodes vs psb1 faces    */
-//                docollide.psb[0] = this;
-//                docollide.psb[1] = psb;
-//                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root,
-//                                                    docollide.psb[1]->m_fdbvt.m_root,
-//                                                    docollide);
+                if (this != psb)
+                {
+                    btSoftColliders::CollideVF_DD docollide;
+                    /* common                    */
+                    docollide.mrg = getCollisionShape()->getMargin() +
+                    psb->getCollisionShape()->getMargin();
+                    /* psb0 nodes vs psb1 faces    */
+                    docollide.psb[0] = this;
+                    docollide.psb[1] = psb;
+                    docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+                                                        docollide.psb[1]->m_fdbvt.m_root,
+                                                        docollide);
+                    /* psb1 nodes vs psb0 faces    */
+                    docollide.psb[0] = psb;
+                    docollide.psb[1] = this;
+                    docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+                                                        docollide.psb[1]->m_fdbvt.m_root,
+                                                        docollide);
+                }
+                else
+                {
+                    if (psb->useSelfCollision())
+                    {
+                      btSoftColliders::CollideFF_DD docollide;
+                      docollide.mrg = getCollisionShape()->getMargin() +
+                      psb->getCollisionShape()->getMargin();
+                      docollide.psb[0] = this;
+                      docollide.psb[1] = psb;
+                      /* psb0 faces vs psb0 faces    */
+                      btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
+                      calculateNormalCone(root);
+                      this->m_fdbvt.selfCollideT(root,docollide);
+                      delete root;
+                    }
+                }
             }
         }
         break;
@@ -3989,3 +4135,47 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 
 	return btSoftBodyDataName;
 }
+
+void btSoftBody::updateDeactivation(btScalar timeStep)
+{
+    if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
+        return;
+
+    if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold)
+    {
+        m_deactivationTime += timeStep;
+    }
+    else
+    {
+        m_deactivationTime = btScalar(0.);
+        setActivationState(0);
+    }
+}
+
+
+void btSoftBody::setZeroVelocity()
+{
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+        m_nodes[i].m_v.setZero();
+    }
+}
+
+bool btSoftBody::wantsSleeping()
+{
+    if (getActivationState() == DISABLE_DEACTIVATION)
+        return false;
+
+    //disable deactivation
+    if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
+        return false;
+
+    if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
+        return true;
+
+    if (m_deactivationTime > gDeactivationTime)
+    {
+        return true;
+    }
+    return false;
+}
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 1016fe9f6..51f0dad21 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -354,6 +354,12 @@ public:
         Node* m_node;      // Owner node
     };
     
+    class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
+    {
+    public:
+        btVector3 m_local;    // Anchor position in body space
+    };
+    
     class DeformableFaceRigidContact : public DeformableRigidContact
     {
     public:
@@ -702,6 +708,8 @@ public:
 		tVSolverArray m_vsequence;  // Velocity solvers sequence
 		tPSolverArray m_psequence;  // Position solvers sequence
 		tPSolverArray m_dsequence;  // Drift solvers sequence
+        btScalar drag;           // deformable air drag
+        btScalar m_maxStress;       // Maximum principle first Piola stress
 	};
 	/* SolverState	*/
 	struct SolverState
@@ -772,6 +780,7 @@ public:
     btAlignedObjectArray<TetraScratch> m_tetraScratches;
     btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
 	tAnchorArray m_anchors;            // Anchors
+    btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
 	tRContactArray m_rcontacts;        // Rigid contacts
     btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
     btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
@@ -787,9 +796,13 @@ public:
 	btDbvt m_cdbvt;                    // Clusters tree
 	tClusterArray m_clusters;          // Clusters
     btScalar m_dampingCoefficient;     // Damping Coefficient
+    btScalar m_sleepingThreshold;
+    btScalar m_maxSpeedSquared;
+    bool m_useFaceContact;
     
     btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
     btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
+    bool m_useSelfCollision;
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
@@ -826,6 +839,11 @@ public:
     {
         m_dampingCoefficient = damping_coeff;
     }
+    
+    void setUseFaceContact(bool useFaceContact)
+    {
+        m_useFaceContact = false;
+    }
 
 	///@todo: avoid internal softbody shape hack and move collision code to collision library
 	virtual void setCollisionShape(btCollisionShape* collisionShape)
@@ -886,7 +904,9 @@ public:
 					 Material* mat = 0);
 
 	/* Append anchor														*/
-	void appendAnchor(int node,
+    void appendDeformableAnchor(int node, btRigidBody* body);
+    void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
+    void appendAnchor(int node,
 					  btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 	void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 	/* Append linear joint													*/
@@ -1006,6 +1026,11 @@ public:
 	/* defaultCollisionHandlers												*/
 	void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap);
 	void defaultCollisionHandler(btSoftBody* psb);
+    void setSelfCollision(bool useSelfCollision);
+    bool useSelfCollision();
+    void updateDeactivation(btScalar timeStep);
+    void setZeroVelocity();
+    bool wantsSleeping();
 
 	//
 	// Functionality to deal with new accelerated solvers.
@@ -1103,6 +1128,7 @@ public:
     void updateDeformation();
     void advanceDeformation();
 	void applyForces();
+    void setMaxStress(btScalar maxStress);
     void interpolateRenderMesh();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index ff074eb43..6e20b3222 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -1074,6 +1074,7 @@ struct btSoftColliders
                 if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
+                    // todo: collision between multibody and fixed deformable node will be missed.
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
                     const btScalar ms = ima + imb;
                     if (ms > 0)
@@ -1096,8 +1097,6 @@ struct btSoftColliders
                             
                             c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
                             c.m_c1 = ra;
-                            if (m_rigidBody)
-                                m_rigidBody->activate();
                         }
                         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                         {
@@ -1169,6 +1168,7 @@ struct btSoftColliders
             {
                 btScalar ima = n0->m_im + n1->m_im + n2->m_im;
                 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                // todo: collision between multibody and fixed deformable face will be missed.
                 const btScalar ms = ima + imb;
                 if (ms > 0)
                 {
@@ -1198,8 +1198,6 @@ struct btSoftColliders
                         // we do not scale the impulse matrix by dt
                         c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
                         c.m_c1 = ra;
-                        if (m_rigidBody)
-                            m_rigidBody->activate();
                     }
                     else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                     {
diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt
index 0c8c0133a..a5491c325 100644
--- a/src/LinearMath/CMakeLists.txt
+++ b/src/LinearMath/CMakeLists.txt
@@ -32,6 +32,7 @@ SET(LinearMath_HDRS
 	btIDebugDraw.h
 	btList.h
 	btMatrix3x3.h
+	btImplicitQRSVD.h
 	btMinMax.h
 	btMotionState.h
 	btPolarDecomposition.h
diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h
new file mode 100644
index 000000000..cf96d4f72
--- /dev/null
+++ b/src/LinearMath/btImplicitQRSVD.h
@@ -0,0 +1,889 @@
+/**
+ 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.
+ 
+ Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran
+ 
+ Permission is hereby granted, free of charge, to any person obtaining a copy of
+ this software and associated documentation files (the "Software"), to deal in
+ the Software without restriction, including without limitation the rights to
+ use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+ of the Software, and to permit persons to whom the Software is furnished to do
+ so, subject to the following conditions:
+ 
+ The above copyright notice and this permission notice shall be included in all
+ copies or substantial portions of the Software.
+ 
+ If the code is used in an article, the following paper shall be cited:
+ @techreport{qrsvd:2016,
+ title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
+ author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
+ year={2016},
+ institution={University of California Los Angeles}
+ }
+ 
+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ SOFTWARE.
+**/
+
+#ifndef btImplicitQRSVD_h
+#define btImplicitQRSVD_h
+
+#include "btMatrix3x3.h"
+class btMatrix2x2
+{
+public:
+    btScalar m_00, m_01, m_10, m_11;
+    btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0)
+    {
+    }
+    btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11)
+    {}
+    btScalar& operator()(int i, int j)
+    {
+        if (i == 0 && j == 0)
+            return m_00;
+        if (i == 1 && j == 0)
+            return m_10;
+        if (i == 0 && j == 1)
+            return m_01;
+        if (i == 1 && j == 1)
+            return m_11;
+        btAssert(false);
+        return m_00;
+    }
+    const btScalar& operator()(int i, int j) const
+    {
+        if (i == 0 && j == 0)
+            return m_00;
+        if (i == 1 && j == 0)
+            return m_10;
+        if (i == 0 && j == 1)
+            return m_01;
+        if (i == 1 && j == 1)
+            return m_11;
+        btAssert(false);
+        return m_00;
+    }
+    void setIdentity()
+    {
+        m_00 = 1;
+        m_11 = 1;
+        m_01 = 0;
+        m_10 = 0;
+    }
+};
+
+static inline btScalar copySign(btScalar x, btScalar y) {
+    if ((x < 0 && y > 0) || (x > 0 && y < 0))
+        return -x;
+    return x;
+}
+
+/**
+ Class for givens rotation.
+ Row rotation G*A corresponds to something like
+ c -s  0
+ ( s  c  0 ) A
+ 0  0  1
+ Column rotation A G' corresponds to something like
+ c -s  0
+ A ( s  c  0 )
+ 0  0  1
+ 
+ c and s are always computed so that
+ ( c -s ) ( a )  =  ( * )
+ s  c     b       ( 0 )
+ 
+ Assume rowi<rowk.
+ */
+
+class GivensRotation {
+public:
+    int rowi;
+    int rowk;
+    btScalar c;
+    btScalar s;
+    
+    inline GivensRotation(int rowi_in, int rowk_in)
+    : rowi(rowi_in)
+    , rowk(rowk_in)
+    , c(1)
+    , s(0)
+    {
+    }
+    
+    inline GivensRotation(btScalar a, btScalar b, int rowi_in, int rowk_in)
+    : rowi(rowi_in)
+    , rowk(rowk_in)
+    {
+        compute(a, b);
+    }
+    
+    ~GivensRotation() {}
+    
+    inline void transposeInPlace()
+    {
+        s = -s;
+    }
+    
+    /**
+     Compute c and s from a and b so that
+     ( c -s ) ( a )  =  ( * )
+     s  c     b       ( 0 )
+     */
+    inline void compute(const btScalar a, const btScalar b)
+    {
+        btScalar d = a * a + b * b;
+        c = 1;
+        s = 0;
+        if (d != 0) {
+            btScalar t = btScalar(1.0)/btSqrt(d);
+            c = a * t;
+            s = -b * t;
+        }
+    }
+    
+    /**
+     This function computes c and s so that
+     ( c -s ) ( a )  =  ( 0 )
+     s  c     b       ( * )
+     */
+    inline void computeUnconventional(const btScalar a, const btScalar b)
+    {
+        btScalar d = a * a + b * b;
+        c = 0;
+        s = 1;
+        if (d != 0) {
+            btScalar t = btScalar(1.0)/btSqrt(d);
+            s = a * t;
+            c = b * t;
+        }
+    }
+    /**
+     Fill the R with the entries of this rotation
+     */
+    inline void fill(const btMatrix3x3& R) const
+    {
+        btMatrix3x3& A = const_cast<btMatrix3x3&>(R);
+        A.setIdentity();
+        A[rowi][rowi] = c;
+        A[rowk][rowi] = -s;
+        A[rowi][rowk] = s;
+        A[rowk][rowk] = c;
+    }
+    
+    inline void fill(const btMatrix2x2& R) const
+    {
+        btMatrix2x2& A = const_cast<btMatrix2x2&>(R);
+        A(rowi,rowi) = c;
+        A(rowk,rowi) = -s;
+        A(rowi,rowk) = s;
+        A(rowk,rowk) = c;
+    }
+    
+    /**
+     This function does something like
+     c -s  0
+     ( s  c  0 ) A -> A
+     0  0  1
+     It only affects row i and row k of A.
+     */
+    inline void rowRotation(btMatrix3x3& A) const
+    {
+        for (int j = 0; j < 3; j++) {
+            btScalar tau1 = A[rowi][j];
+            btScalar tau2 = A[rowk][j];
+            A[rowi][j] = c * tau1 - s * tau2;
+            A[rowk][j] = s * tau1 + c * tau2;
+        }
+    }
+    inline void rowRotation(btMatrix2x2& A) const
+    {
+        for (int j = 0; j < 2; j++) {
+            btScalar tau1 = A(rowi,j);
+            btScalar tau2 = A(rowk,j);
+            A(rowi,j) = c * tau1 - s * tau2;
+            A(rowk,j) = s * tau1 + c * tau2;
+        }
+    }
+    
+    /**
+     This function does something like
+     c  s  0
+     A ( -s  c  0 )  -> A
+     0  0  1
+     It only affects column i and column k of A.
+     */
+    inline void columnRotation(btMatrix3x3& A) const
+    {
+        for (int j = 0; j < 3; j++) {
+            btScalar tau1 = A[j][rowi];
+            btScalar tau2 = A[j][rowk];
+            A[j][rowi] = c * tau1 - s * tau2;
+            A[j][rowk] = s * tau1 + c * tau2;
+        }
+    }
+    inline void columnRotation(btMatrix2x2& A) const
+    {
+        for (int j = 0; j < 2; j++) {
+            btScalar tau1 = A(j,rowi);
+            btScalar tau2 = A(j,rowk);
+            A(j,rowi) = c * tau1 - s * tau2;
+            A(j,rowk) = s * tau1 + c * tau2;
+        }
+    }
+    
+    /**
+     Multiply givens must be for same row and column
+     **/
+    inline void operator*=(const GivensRotation& A)
+    {
+        btScalar new_c = c * A.c - s * A.s;
+        btScalar new_s = s * A.c + c * A.s;
+        c = new_c;
+        s = new_s;
+    }
+    
+    /**
+     Multiply givens must be for same row and column
+     **/
+    inline GivensRotation operator*(const GivensRotation& A) const
+    {
+        GivensRotation r(*this);
+        r *= A;
+        return r;
+    }
+};
+
+/**
+ \brief zero chasing the 3X3 matrix to bidiagonal form
+ original form of H:   x x 0
+ x x x
+ 0 0 x
+ after zero chase:
+ x x 0
+ 0 x x
+ 0 0 x
+ */
+inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    
+    /**
+     Reduce H to of form
+     x x +
+     0 x x
+     0 0 x
+     */
+    GivensRotation r1(H[0][0], H[1][0], 0, 1);
+    /**
+     Reduce H to of form
+     x x 0
+     0 x x
+     0 + x
+     Can calculate r2 without multiplying by r1 since both entries are in first two
+     rows thus no need to divide by sqrt(a^2+b^2)
+     */
+    GivensRotation r2(1, 2);
+    if (H[1][0] != 0)
+        r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]);
+    else
+        r2.compute(H[0][1], H[0][2]);
+    
+    r1.rowRotation(H);
+    
+    /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     x x 0
+     0 x x
+     0 0 x
+     */
+    GivensRotation r3(H[1][1], H[2][1], 1, 2);
+    r3.rowRotation(H);
+    
+    // Save this till end for better cache coherency
+    // r1.rowRotation(u_transpose);
+    // r3.rowRotation(u_transpose);
+    r1.columnRotation(U);
+    r3.columnRotation(U);
+}
+
+/**
+ \brief make a 3X3 matrix to upper bidiagonal form
+ original form of H:   x x x
+ x x x
+ x x x
+ after zero chase:
+ x x 0
+ 0 x x
+ 0 0 x
+ */
+inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    U.setIdentity();
+    V.setIdentity();
+    
+    /**
+     Reduce H to of form
+     x x x
+     x x x
+     0 x x
+     */
+    
+    GivensRotation r(H[1][0], H[2][0], 1, 2);
+    r.rowRotation(H);
+    // r.rowRotation(u_transpose);
+    r.columnRotation(U);
+    // zeroChase(H, u_transpose, V);
+    zeroChase(H, U, V);
+}
+
+/**
+ \brief make a 3X3 matrix to lambda shape
+ original form of H:   x x x
+ *                     x x x
+ *                     x x x
+ after :
+ *                     x 0 0
+ *                     x x 0
+ *                     x 0 x
+ */
+inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V)
+{
+    U.setIdentity();
+    V.setIdentity();
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x x
+     *                    x x x
+     */
+    
+    GivensRotation r1(H[0][1], H[0][2], 1, 2);
+    r1.columnRotation(H);
+    r1.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x 0
+     *                    x x x
+     */
+    
+    r1.computeUnconventional(H[1][2], H[2][2]);
+    r1.rowRotation(H);
+    r1.columnRotation(U);
+    
+    /**
+     Reduce H to of form
+     *                    x x 0
+     *                    x x 0
+     *                    x 0 x
+     */
+    
+    GivensRotation r2(H[2][0], H[2][1], 0, 1);
+    r2.columnRotation(H);
+    r2.columnRotation(V);
+    
+    /**
+     Reduce H to of form
+     *                    x 0 0
+     *                    x x 0
+     *                    x 0 x
+     */
+    r2.computeUnconventional(H[0][1], H[1][1]);
+    r2.rowRotation(H);
+    r2.columnRotation(U);
+}
+
+/**
+ \brief 2x2 polar decomposition.
+ \param[in] A matrix.
+ \param[out] R Robustly a rotation matrix.
+ \param[out] S_Sym Symmetric. Whole matrix is stored
+ 
+ Polar guarantees negative sign is on the small magnitude singular value.
+ S is guaranteed to be the closest one to identity.
+ R is guaranteed to be the closest rotation to A.
+ */
+inline void polarDecomposition(const btMatrix2x2& A,
+                   GivensRotation& R,
+                   const btMatrix2x2& S_Sym)
+{
+    btScalar a = (A(0, 0) + A(1, 1)),  b = (A(1, 0) - A(0, 1));
+    btScalar denominator = btSqrt(a*a+b*b);
+    R.c = (btScalar)1;
+    R.s = (btScalar)0;
+    if (denominator != 0) {
+        /*
+         No need to use a tolerance here because x(0) and x(1) always have
+         smaller magnitude then denominator, therefore overflow never happens.
+         */
+        R.c = a / denominator;
+        R.s = -b / denominator;
+    }
+    btMatrix2x2& S = const_cast<btMatrix2x2&>(S_Sym);
+    S = A;
+    R.rowRotation(S);
+}
+
+inline void polarDecomposition(const btMatrix2x2& A,
+                   const btMatrix2x2& R,
+                   const btMatrix2x2& S_Sym)
+{
+    GivensRotation r(0, 1);
+    polarDecomposition(A, r, S_Sym);
+    r.fill(R);
+}
+
+/**
+ \brief 2x2 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U Robustly a rotation matrix in Givens form
+ \param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative.
+ \param[out] V Robustly a rotation matrix in Givens form
+ */
+inline void singularValueDecomposition(
+                           const btMatrix2x2& A,
+                           GivensRotation& U,
+                           const btMatrix2x2& Sigma,
+                           GivensRotation& V,
+                           const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
+{
+    btMatrix2x2& sigma = const_cast<btMatrix2x2&>(Sigma);
+    sigma.setIdentity();
+    btMatrix2x2 S_Sym;
+    polarDecomposition(A, U, S_Sym);
+    btScalar cosine, sine;
+    btScalar x = S_Sym(0, 0);
+    btScalar y = S_Sym(0, 1);
+    btScalar z = S_Sym(1, 1);
+    if (y == 0) {
+        // S is already diagonal
+        cosine = 1;
+        sine = 0;
+        sigma(0,0) = x;
+        sigma(1,1) = z;
+    }
+    else {
+        btScalar tau = 0.5 * (x - z);
+        btScalar w = btSqrt(tau * tau + y * y);
+        // w > y > 0
+        btScalar t;
+        if (tau > 0) {
+            // tau + w > w > y > 0 ==> division is safe
+            t = y / (tau + w);
+        }
+        else {
+            // tau - w < -w < -y < 0 ==> division is safe
+            t = y / (tau - w);
+        }
+        cosine = btScalar(1) / btSqrt(t * t + btScalar(1));
+        sine = -t * cosine;
+        /*
+         V = [cosine -sine; sine cosine]
+         Sigma = V'SV. Only compute the diagonals for efficiency.
+         Also utilize symmetry of S and don't form V yet.
+         */
+        btScalar c2 = cosine * cosine;
+        btScalar csy = 2 * cosine * sine * y;
+        btScalar s2 = sine * sine;
+        sigma(0,0) = c2 * x - csy + s2 * z;
+        sigma(1,1) = s2 * x + csy + c2 * z;
+    }
+    
+    // Sorting
+    // Polar already guarantees negative sign is on the small magnitude singular value.
+    if (sigma(0,0) < sigma(1,1)) {
+        std::swap(sigma(0,0), sigma(1,1));
+        V.c = -sine;
+        V.s = cosine;
+    }
+    else {
+        V.c = cosine;
+        V.s = sine;
+    }
+    U *= V;
+}
+
+/**
+ \brief 2x2 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U Robustly a rotation matrix.
+ \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative.
+ \param[out] V Robustly a rotation matrix.
+ */
+inline void singularValueDecomposition(
+                           const btMatrix2x2& A,
+                           const btMatrix2x2& U,
+                           const btMatrix2x2& Sigma,
+                           const btMatrix2x2& V,
+                           const btScalar tol = 64 * std::numeric_limits<btScalar>::epsilon())
+{
+    GivensRotation gv(0, 1);
+    GivensRotation gu(0, 1);
+    singularValueDecomposition(A, gu, Sigma, gv);
+    
+    gu.fill(U);
+    gv.fill(V);
+}
+
+/**
+ \brief compute wilkinsonShift of the block
+ a1     b1
+ b1     a2
+ based on the wilkinsonShift formula
+ mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2
+ 
+ */
+inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2)
+{
+    btScalar d = (btScalar)0.5 * (a1 - a2);
+    btScalar bs = b1 * b1;
+    btScalar mu = a2 - copysign(bs / (btFabs(d) + btSqrt(d * d + bs)), d);
+    // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
+    return mu;
+}
+
+/**
+ \brief Helper function of 3X3 SVD for processing 2X2 SVD
+ */
+template <int t>
+inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V)
+{
+    int other = (t == 1) ? 0 : 2;
+    GivensRotation u(0, 1);
+    GivensRotation v(0, 1);
+    sigma[other] = B[other][other];
+    
+    btMatrix2x2 B_sub, sigma_sub;
+    if (t == 0)
+    {
+        B_sub.m_00 = B[0][0];
+        B_sub.m_10 = B[1][0];
+        B_sub.m_01 = B[0][1];
+        B_sub.m_11 = B[1][1];
+        sigma_sub.m_00 = sigma[0];
+        sigma_sub.m_11 = sigma[1];
+//        singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
+        singularValueDecomposition(B_sub, u, sigma_sub, v);
+        B[0][0] = B_sub.m_00;
+        B[1][0] = B_sub.m_10;
+        B[0][1] = B_sub.m_01;
+        B[1][1] = B_sub.m_11;
+        sigma[0] = sigma_sub.m_00;
+        sigma[1] = sigma_sub.m_11;
+    }
+    else
+    {
+        B_sub.m_00 = B[1][1];
+        B_sub.m_10 = B[2][1];
+        B_sub.m_01 = B[1][2];
+        B_sub.m_11 = B[2][2];
+        sigma_sub.m_00 = sigma[1];
+        sigma_sub.m_11 = sigma[2];
+        //        singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
+        singularValueDecomposition(B_sub, u, sigma_sub, v);
+        B[1][1] = B_sub.m_00;
+        B[2][1] = B_sub.m_10;
+        B[1][2] = B_sub.m_01;
+        B[2][2] = B_sub.m_11;
+        sigma[1] = sigma_sub.m_00;
+        sigma[2] = sigma_sub.m_11;
+    }
+    u.rowi += t;
+    u.rowk += t;
+    v.rowi += t;
+    v.rowk += t;
+    u.columnRotation(U);
+    v.columnRotation(V);
+}
+
+/**
+ \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma
+ */
+inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma)
+{
+    sigma[i] = -sigma[i];
+    U[0][i] = -U[0][i];
+    U[1][i] = -U[1][i];
+    U[2][i] = -U[2][i];
+}
+
+inline void flipSign(int i, btMatrix3x3& U)
+{
+    U[0][i] = -U[0][i];
+    U[1][i] = -U[1][i];
+    U[2][i] = -U[2][i];
+}
+
+inline void swapCol(btMatrix3x3& A, int i, int j)
+{
+    for (int d = 0; d < 3; ++d)
+        std::swap(A[d][i], A[d][j]);
+}
+/**
+ \brief Helper function of 3X3 SVD for sorting singular values
+ */
+inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t)
+{
+    if (t == 0)
+    {
+        // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
+        if (btFabs(sigma[1]) >= btFabs(sigma[2])) {
+            if (sigma[1] < 0) {
+                flipSign(1, U, sigma);
+                flipSign(2, U, sigma);
+            }
+            return;
+        }
+        
+        //fix sign of sigma for both cases
+        if (sigma[2] < 0) {
+            flipSign(1, U, sigma);
+            flipSign(2, U, sigma);
+        }
+        
+        //swap sigma(1) and sigma(2) for both cases
+        std::swap(sigma[1], sigma[2]);
+        // swap the col 1 and col 2 for U,V
+        swapCol(U,1,2);
+        swapCol(V,1,2);
+        
+        // Case: |sigma(2)| >= sigma(0) > |simga(1)|
+        if (sigma[1] > sigma[0]) {
+            std::swap(sigma[0], sigma[1]);
+            swapCol(U,0,1);
+            swapCol(V,0,1);
+        }
+        
+        // Case: sigma(0) >= |sigma(2)| > |simga(1)|
+        else {
+            flipSign(2, U);
+            flipSign(2, V);
+        }
+    }
+    else if (t == 1)
+    {
+        // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
+        if (btFabs(sigma[0]) >= sigma[1]) {
+            if (sigma[0] < 0) {
+                flipSign(0, U, sigma);
+                flipSign(2, U, sigma);
+            }
+            return;
+        }
+        
+        //swap sigma(0) and sigma(1) for both cases
+        std::swap(sigma[0], sigma[1]);
+        swapCol(U, 0, 1);
+        swapCol(V, 0, 1);
+        
+        // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
+        if (btFabs(sigma[1]) < btFabs(sigma[2])) {
+            std::swap(sigma[1], sigma[2]);
+            swapCol(U, 1, 2);
+            swapCol(V, 1, 2);
+        }
+        
+        // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
+        else {
+            flipSign(1, U);
+            flipSign(1, V);
+        }
+        
+        // fix sign for both cases
+        if (sigma[1] < 0) {
+            flipSign(1, U, sigma);
+            flipSign(2, U, sigma);
+        }
+    }
+}
+
+/**
+ \brief 3X3 SVD (singular value decomposition) A=USV'
+ \param[in] A Input matrix.
+ \param[out] U is a rotation matrix.
+ \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative.
+ \param[out] V is a rotation matrix.
+ */
+inline int singularValueDecomposition(const btMatrix3x3& A,
+                                     btMatrix3x3& U,
+                                     btVector3& sigma,
+                                     btMatrix3x3& V,
+                                     btScalar tol = 128*std::numeric_limits<btScalar>::epsilon())
+{
+    using std::fabs;
+    btMatrix3x3 B = A;
+    U.setIdentity();
+    V.setIdentity();
+    
+    makeUpperBidiag(B, U, V);
+    
+    int count = 0;
+    btScalar mu = (btScalar)0;
+    GivensRotation r(0, 1);
+    
+    btScalar alpha_1 = B[0][0];
+    btScalar beta_1 = B[0][1];
+    btScalar alpha_2 = B[1][1];
+    btScalar alpha_3 = B[2][2];
+    btScalar beta_2 = B[1][2];
+    btScalar gamma_1 = alpha_1 * beta_1;
+    btScalar gamma_2 = alpha_2 * beta_2;
+    tol *= btMax((btScalar)0.5 * btSqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1);
+    
+    /**
+     Do implicit shift QR until A^T A is block diagonal
+     */
+    
+    while (btFabs(beta_2) > tol && btFabs(beta_1) > tol
+           && btFabs(alpha_1) > tol && btFabs(alpha_2) > tol
+           && btFabs(alpha_3) > tol) {
+        mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
+        
+        r.compute(alpha_1 * alpha_1 - mu, gamma_1);
+        r.columnRotation(B);
+        
+        r.columnRotation(V);
+        zeroChase(B, U, V);
+        
+        alpha_1 = B[0][0];
+        beta_1 = B[0][1];
+        alpha_2 = B[1][1];
+        alpha_3 = B[2][2];
+        beta_2 = B[1][2];
+        gamma_1 = alpha_1 * beta_1;
+        gamma_2 = alpha_2 * beta_2;
+        count++;
+    }
+    /**
+     Handle the cases of one of the alphas and betas being 0
+     Sorted by ease of handling and then frequency
+     of occurrence
+     
+     If B is of form
+     x x 0
+     0 x 0
+     0 0 x
+     */
+    if (btFabs(beta_2) <= tol) {
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V,0);
+    }
+    /**
+     If B is of form
+     x 0 0
+     0 x x
+     0 0 x
+     */
+    else if (btFabs(beta_1) <= tol) {
+        process<1>(B, U, sigma, V);
+        sort(U, sigma, V,1);
+    }
+    /**
+     If B is of form
+     x x 0
+     0 0 x
+     0 0 x
+     */
+    else if (btFabs(alpha_2) <= tol) {
+        /**
+         Reduce B to
+         x x 0
+         0 0 0
+         0 0 x
+         */
+        GivensRotation r1(1, 2);
+        r1.computeUnconventional(B[1][2], B[2][2]);
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+        
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V, 0);
+    }
+    /**
+     If B is of form
+     x x 0
+     0 x x
+     0 0 0
+     */
+    else if (btFabs(alpha_3) <= tol) {
+        /**
+         Reduce B to
+         x x +
+         0 x 0
+         0 0 0
+         */
+        GivensRotation r1(1, 2);
+        r1.compute(B[1][1], B[1][2]);
+        r1.columnRotation(B);
+        r1.columnRotation(V);
+        /**
+         Reduce B to
+         x x 0
+         + x 0
+         0 0 0
+         */
+        GivensRotation r2(0, 2);
+        r2.compute(B[0][0], B[0][2]);
+        r2.columnRotation(B);
+        r2.columnRotation(V);
+        
+        process<0>(B, U, sigma, V);
+        sort(U, sigma, V, 0);
+    }
+    /**
+     If B is of form
+     0 x 0
+     0 x x
+     0 0 x
+     */
+    else if (btFabs(alpha_1) <= tol) {
+        /**
+         Reduce B to
+         0 0 +
+         0 x x
+         0 0 x
+         */
+        GivensRotation r1(0, 1);
+        r1.computeUnconventional(B[0][1], B[1][1]);
+        r1.rowRotation(B);
+        r1.columnRotation(U);
+        
+        /**
+         Reduce B to
+         0 0 0
+         0 x x
+         0 + x
+         */
+        GivensRotation r2(0, 2);
+        r2.computeUnconventional(B[0][2], B[2][2]);
+        r2.rowRotation(B);
+        r2.columnRotation(U);
+        
+        process<1>(B, U, sigma, V);
+        sort(U, sigma, V, 1);
+    }
+    
+    return count;
+}
+#endif /* btImplicitQRSVD_h */