Merge pull request #2800 from xhan0619/splitImpulseMulti

Stability improvements for deformable.

34 files changed, 297 insertions, 177 deletions

diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf
index 7d09ac281..20fa3e1a3 100644
--- a/data/torus_deform.urdf
+++ b/data/torus_deform.urdf
@@ -6,7 +6,7 @@
       <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
     </inertial>
     <collision_margin value="0.006"/>
-    <repulsion_stiffness value="50.0"/>
+    <repulsion_stiffness value="800.0"/>
     <friction value= "0.5"/>
     <neohookean mu= "60" lambda= "200" damping= "0.01" />
     <visual filename="torus.vtk"/>
diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp
index ef4908766..1d091fa38 100644
--- a/examples/DeformableDemo/ClothFriction.cpp
+++ b/examples/DeformableDemo/ClothFriction.cpp
@@ -144,7 +144,7 @@ void ClothFriction::initPhysics()
                                                          10,10,
                                                          0, true);
         
-        psb->getCollisionShape()->setMargin(0.05);
+        psb->getCollisionShape()->setMargin(0.06);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
         psb->setSpringStiffness(100);
@@ -173,7 +173,7 @@ void ClothFriction::initPhysics()
                                                           btVector3(+s, h, +s),
                                                           5,5,
                                                           0, true);
-        psb2->getCollisionShape()->setMargin(0.05);
+        psb2->getCollisionShape()->setMargin(0.06);
         psb2->generateBendingConstraints(2);
         psb2->setTotalMass(1);
         psb2->setSpringStiffness(100);
diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp
index cb83d1441..b1ad0b67b 100644
--- a/examples/DeformableDemo/DeformableContact.cpp
+++ b/examples/DeformableDemo/DeformableContact.cpp
@@ -143,7 +143,7 @@ void DeformableContact::initPhysics()
                                                          20,20,
                                                          1 + 2 + 4 + 8, true);
         
-        psb->getCollisionShape()->setMargin(0.1);
+        psb->getCollisionShape()->setMargin(0.05);
         psb->generateBendingConstraints(2);
         psb->setSpringStiffness(10);
         psb->setTotalMass(1);
@@ -172,7 +172,7 @@ void DeformableContact::initPhysics()
                                                           btVector3(+s, h, +s),
                                                           10,10,
                                                           0, true);
-        psb2->getCollisionShape()->setMargin(0.1);
+        psb2->getCollisionShape()->setMargin(0.05);
         psb2->generateBendingConstraints(2);
         psb2->setSpringStiffness(10);
         psb2->setTotalMass(1);
diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp
index b8d4a86ad..041624b00 100644
--- a/examples/DeformableDemo/DeformableMultibody.cpp
+++ b/examples/DeformableDemo/DeformableMultibody.cpp
@@ -192,7 +192,7 @@ void DeformableMultibody::initPhysics()
 //                                                         3,3,
                                                          1 + 2 + 4 + 8, true);
 
-        psb->getCollisionShape()->setMargin(0.25);
+        psb->getCollisionShape()->setMargin(0.025);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@@ -341,6 +341,7 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM
         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);
+        box->setMargin(0.01);
         btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
         col->setCollisionShape(box);
         
diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp
index 4e5191caf..7efe118a6 100644
--- a/examples/DeformableDemo/DeformableRigid.cpp
+++ b/examples/DeformableDemo/DeformableRigid.cpp
@@ -53,6 +53,48 @@ public:
         //use a smaller internal timestep, there are stability issues
         float internalTimeStep = 1. / 240.f;
         m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        
+//
+//        btCollisionShape* boxShape = new btBoxShape(btVector3(1, 1, 1));
+//        boxShape->setMargin(1e-3);
+//        if (0)
+//        {
+//        btVector3 p(0.99,1.01,0.99);
+//        for (int i = 0; i < 40; ++i)
+//        {
+//            p[1] -= 0.001;
+//            btScalar margin(.000001);
+//            btTransform trans;
+//            trans.setIdentity();
+//            btGjkEpaSolver2::sResults results;
+//            const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape);
+//            btScalar d = btGjkEpaSolver2::SignedDistance(p, margin, csh, trans, results);
+//            printf("d = %f\n", d);
+//            printf("----\n");
+//        }
+//        }
+//
+//        btVector3 p(.991,1.01,.99);
+//        for (int i = 0; i < 40; ++i)
+//        {
+//            p[1] -= 0.001;
+//            btScalar margin(.006);
+//            btTransform trans;
+//            trans.setIdentity();
+//            btScalar dst;
+//            btGjkEpaSolver2::sResults results;
+//            btTransform point_transform;
+//            point_transform.setIdentity();
+//            point_transform.setOrigin(p);
+//            btSphereShape sphere(margin);
+//            btVector3 guess(0,0,0);
+//            const btConvexShape* csh = static_cast<const btConvexShape*>(boxShape);
+//            btGjkEpaSolver2::SignedDistance(&sphere, point_transform, csh, trans, guess, results);
+//            dst = results.distance-csh->getMargin();
+//            dst -= margin;
+//            printf("d = %f\n", dst);
+//             printf("----\n");
+//        }
     }
     
     void Ctor_RbUpStack(int count)
@@ -214,7 +256,7 @@ void DeformableRigid::initPhysics()
                                                  2,2,
                                                  0, true);
         
-        psb->getCollisionShape()->setMargin(0.1);
+        psb->getCollisionShape()->setMargin(0.05);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp
index 9806bdb37..576f8f488 100644
--- a/examples/DeformableDemo/DeformableSelfCollision.cpp
+++ b/examples/DeformableDemo/DeformableSelfCollision.cpp
@@ -41,7 +41,7 @@ public:
     
     void resetCamera()
     {
-        float dist = 1.0;
+        float dist = 2.0;
         float pitch = -8;
         float yaw = 100;
         float targetPos[3] = {0, -1.0, 0};
@@ -93,7 +93,7 @@ void DeformableSelfCollision::initPhysics()
     {
         ///create a ground
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.)));
-        
+        groundShape->setMargin(0.0001);
         m_collisionShapes.push_back(groundShape);
         
         btTransform groundTransform;
@@ -128,7 +128,7 @@ void DeformableSelfCollision::initPhysics()
 void DeformableSelfCollision::addCloth(btVector3 origin)
 // create a piece of cloth
 {
-    const btScalar s = 0.3;
+    const btScalar s = 0.6;
     const btScalar h = 0;
     
     btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s),
@@ -140,7 +140,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin)
                                                      0, true, 0.0);
 
     
-    psb->getCollisionShape()->setMargin(0.01);
+    psb->getCollisionShape()->setMargin(0.02);
     psb->generateBendingConstraints(2);
     psb->setTotalMass(.5);
     psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp
index 0fede1b56..f52f67fe9 100644
--- a/examples/DeformableDemo/GraspDeformable.cpp
+++ b/examples/DeformableDemo/GraspDeformable.cpp
@@ -198,6 +198,9 @@ void GraspDeformable::initPhysics()
     btVector3 gravity = btVector3(0, -9.81, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
     m_maxPickingForce = 0.001;
     // build a gripper
@@ -253,7 +256,7 @@ void GraspDeformable::initPhysics()
     //create a ground
     {
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
-
+        groundShape->setMargin(0.001);
         m_collisionShapes.push_back(groundShape);
 
         btTransform groundTransform;
@@ -352,17 +355,17 @@ void GraspDeformable::initPhysics()
                                                  2,2,
                                                  0, true);
 
-        psb->getCollisionShape()->setMargin(0.003);
+        psb->getCollisionShape()->setMargin(0.001);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(0.005);
+        psb->setTotalMass(0.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::SDF_RDF;
         psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));
diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp
index 534cad828..ab78efa68 100644
--- a/examples/DeformableDemo/MultibodyClothAnchor.cpp
+++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp
@@ -143,7 +143,7 @@ void MultibodyClothAnchor::initPhysics()
                                                          btVector3(+s, h, -s),
                                                          btVector3(-s, h, +s),
                                                          btVector3(+s, h, +s), r, r, 4 + 8, true);
-        psb->getCollisionShape()->setMargin(0.1);
+        psb->getCollisionShape()->setMargin(0.01);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp
index 4f294c7b6..dbf1e6c80 100644
--- a/examples/DeformableDemo/Pinch.cpp
+++ b/examples/DeformableDemo/Pinch.cpp
@@ -296,7 +296,7 @@ void Pinch::initPhysics()
         
         psb->scale(btVector3(2, 2, 2));
         psb->translate(btVector3(0, 4, 0));
-        psb->getCollisionShape()->setMargin(0.1);
+        psb->getCollisionShape()->setMargin(0.01);
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp
index 38ca03e23..5d31331b6 100644
--- a/examples/DeformableDemo/PinchFriction.cpp
+++ b/examples/DeformableDemo/PinchFriction.cpp
@@ -265,7 +265,7 @@ void PinchFriction::initPhysics()
         
         psb->scale(btVector3(2, 2, 1));
         psb->translate(btVector3(0, 2.1, 2.2));
-        psb->getCollisionShape()->setMargin(0.05);
+        psb->getCollisionShape()->setMargin(0.025);
         psb->setSpringStiffness(10);
         psb->setTotalMass(.6);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
@@ -296,7 +296,7 @@ void PinchFriction::initPhysics()
         
         psb2->scale(btVector3(2, 2, 1));
         psb2->translate(btVector3(0, 2.1, -2.2));
-        psb2->getCollisionShape()->setMargin(0.05);
+        psb2->getCollisionShape()->setMargin(0.025);
         psb2->setTotalMass(.6);
         psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
@@ -327,7 +327,7 @@ void PinchFriction::initPhysics()
         
         psb3->scale(btVector3(2, 2, 1));
         psb3->translate(btVector3(0, 2.1, 0));
-        psb3->getCollisionShape()->setMargin(0.05);
+        psb3->getCollisionShape()->setMargin(0.025);
         psb3->setTotalMass(.6);
         psb3->setSpringStiffness(10);
         psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/examples/DeformableDemo/SplitImpulse.cpp b/examples/DeformableDemo/SplitImpulse.cpp
index 306f98f96..962d49d66 100644
--- a/examples/DeformableDemo/SplitImpulse.cpp
+++ b/examples/DeformableDemo/SplitImpulse.cpp
@@ -160,7 +160,7 @@ void SplitImpulse::initPhysics()
 //                                                          0, true);
 
         
-        psb->getCollisionShape()->setMargin(0.15);
+        psb->getCollisionShape()->setMargin(0.015);
         psb->generateBendingConstraints(2);
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp
index e37c17e0a..a60bb10d8 100644
--- a/examples/DeformableDemo/VolumetricDeformable.cpp
+++ b/examples/DeformableDemo/VolumetricDeformable.cpp
@@ -208,7 +208,7 @@ void VolumetricDeformable::initPhysics()
         getDeformableDynamicsWorld()->addSoftBody(psb);
         psb->scale(btVector3(2, 2, 2));
         psb->translate(btVector3(0, 5, 0));
-        psb->getCollisionShape()->setMargin(0.25);
+        psb->getCollisionShape()->setMargin(0.025);
         psb->setTotalMass(0.5);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index d873e4d30..7cd87c49a 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -205,10 +205,12 @@ struct SpringCoeffcients{
     double elastic_stiffness;
     double damping_stiffness;
     double bending_stiffness;
+	int damp_all_directions;
     SpringCoeffcients():
-      elastic_stiffness(0.),
-      damping_stiffness(0.),
-      bending_stiffness(0.){}
+	elastic_stiffness(0.),
+	damping_stiffness(0.),
+	bending_stiffness(0.),
+	damp_all_directions(0){}
 };
 
 struct LameCoefficients
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index b3f4d4552..d764f26e6 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -381,8 +381,8 @@ B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle c
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
-        command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
-        command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
+	command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness;
+	command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness;
 	command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE;
 	return 0;
 }
@@ -441,6 +441,15 @@ B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle co
 	return 0;
 }
 
+B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int allDirectionDamping)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
+	command->m_loadSoftBodyArguments.m_dampAllDirections = allDirectionDamping;
+	command->m_updateFlags |= LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE;
+	return 0;
+}
+
 B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 560546736..8c1de72fb 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -657,6 +657,7 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
 	B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
 	B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness);
+    B3_SHARED_API int b3LoadSoftBodyUseAllDirectionDampingSprings(b3SharedMemoryCommandHandle commandHandle, int useAllDirectionDamping);
 	
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]);
 
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 4c2d92e35..c6a550b66 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8396,6 +8396,11 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe
 		deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness;
 	}
 
+	if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE)
+	{
+		deformable.m_springCoefficients.damp_all_directions = loadSoftBodyArgs.m_dampAllDirections;
+	}
+
 	if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
 	{
 		deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu;
@@ -8501,7 +8506,8 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 			btDeformableLagrangianForce* springForce =
 				new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness,
 												deformable.m_springCoefficients.damping_stiffness,
-												true, deformable.m_springCoefficients.bending_stiffness);
+												!deformable.m_springCoefficients.damp_all_directions,
+												deformable.m_springCoefficients.bending_stiffness);
 			deformWorld->addForce(psb, springForce);
 			m_data->m_lf.push_back(springForce);
 		}
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index b892df048..66a9d4494 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -514,6 +514,7 @@ enum EnumLoadSoftBodyUpdateFlags
 	LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
 	LOAD_SOFT_BODY_SIM_MESH = 1<<15,
 	LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16,
+	LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17,
 };
 
 enum EnumSimParamInternalSimFlags
@@ -534,6 +535,7 @@ struct LoadSoftBodyArgs
     double m_initialOrientation[4];
     double m_springElasticStiffness;
     double m_springDampingStiffness;
+	int m_dampAllDirections;
     double m_springBendingStiffness;
     double m_corotatedMu;
     double m_corotatedLambda;
diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py
index 1c01ed6d4..fa3d75867 100644
--- a/examples/pybullet/examples/deformable_anchor.py
+++ b/examples/pybullet/examples/deformable_anchor.py
@@ -15,7 +15,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
 
 boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
 
-clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, springDampingAllDirections = 1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
 
 
 p.createSoftBodyAnchor(clothId  ,0,-1,-1)
diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py
index 7e2732fb3..0408e8e90 100644
--- a/examples/pybullet/examples/deformable_torus.py
+++ b/examples/pybullet/examples/deformable_torus.py
@@ -14,7 +14,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
 
 boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
 
-bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50)
+bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 800)
 
 bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)
 
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index f0789b258..e06fe643e 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2038,7 +2038,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 	int physicsClientId = 0;
 	int flags = 0;
 
-	static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
+	static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springDampingAllDirections", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", "physicsClientId", NULL};
 
 	int bodyUniqueId = -1;
 	const char* fileName = "";
@@ -2050,6 +2050,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 	int useNeoHookean = 0;
 	double springElasticStiffness = 1;
 	double springDampingStiffness = 0.1;
+	int springDampingAllDirections = 0;
 	double springBendingStiffness = 0.1;
 	double NeoHookeanMu = 1;
 	double NeoHookeanLambda = 1;
@@ -2068,7 +2069,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 	PyObject* basePosObj = 0;
 	PyObject* baseOrnObj = 0;
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiidddddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiddidddddiidi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springDampingAllDirections, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -2125,6 +2126,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 		{
 			b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness);
 			b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness);
+                        b3LoadSoftBodyUseAllDirectionDampingSprings(command, springDampingAllDirections);
 		}
 		if (useNeoHookean)
 		{
diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index 4356c12ab..6968a3310 100644
--- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -47,6 +47,7 @@ struct btContactSolverInfoData
 	btScalar m_erp;          //error reduction for non-contact constraints
 	btScalar m_erp2;         //error reduction for contact constraints
 	btScalar m_deformable_erp;          //error reduction for deformable constraints
+	btScalar m_deformable_cfm;          //constraint force mixing for deformable constraints
 	btScalar m_globalCfm;    //constraint force mixing for contacts and non-contacts
 	btScalar m_frictionERP;  //error reduction for friction constraints
 	btScalar m_frictionCFM;  //constraint force mixing for friction constraints
@@ -83,7 +84,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
-		m_deformable_erp = btScalar(0.1);
+		m_deformable_erp = btScalar(0.06);
+		m_deformable_cfm = btScalar(0.01);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);
diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h
index 943d724cc..0a597d90c 100644
--- a/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -465,6 +465,12 @@ public:
 		//for kinematic objects, we could also use use:
 		//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
 	}
+    
+    btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const
+    {
+        //we also calculate lin/ang velocity for kinematic objects
+        return m_pushVelocity + m_turnVelocity.cross(rel_pos);
+    }
 
 	void translate(const btVector3& v)
 	{
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index a1d5bb9ca..9862bd2e2 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -344,6 +344,8 @@ void btMultiBody::finalizeMultiDof()
 {
 	m_deltaV.resize(0);
 	m_deltaV.resize(6 + m_dofCount);
+    m_splitV.resize(0);
+    m_splitV.resize(6 + m_dofCount);
 	m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);  //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
 	m_vectorBuf.resize(2 * m_dofCount);                                           //two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
 	m_matrixBuf.resize(m_links.size() + 1);
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index be795633f..f2acfab9b 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -278,6 +278,11 @@ public:
     {
         return &m_deltaV[0];
     }
+    
+    const btScalar *getSplitVelocityVector() const
+    {
+        return &m_splitV[0];
+    }
 	/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 
@@ -397,6 +402,26 @@ public:
 			m_deltaV[dof] += delta_vee[dof] * multiplier;
 		}
 	}
+    void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
+    {
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] += delta_vee[dof] * multiplier;
+        }
+    }
+    void addSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], 1);
+    }
+    void substractSplitV()
+    {
+        applyDeltaVeeMultiDof(&m_splitV[0], -1);
+        
+        for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
+        {
+            m_splitV[dof] = 0.f;
+        }
+    }
 	void processDeltaVeeMultiDof2()
 	{
 		applyDeltaVeeMultiDof(&m_deltaV[0], 1);
@@ -711,6 +736,7 @@ private:
 	//  offset         size         array
 	//   0              num_links+1  rot_from_parent
 	//
+    btAlignedObjectArray<btScalar> m_splitV;
 	btAlignedObjectArray<btScalar> m_deltaV;
 	btAlignedObjectArray<btScalar> m_realBuf;
 	btAlignedObjectArray<btVector3> m_vectorBuf;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index cd1bad089..f599c9ccb 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -592,6 +592,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 
 			if (!isSleeping)
 			{
+				bod->addSplitV();
 				int nLinks = bod->getNumLinks();
 
 				///base + num m_links
@@ -610,6 +611,7 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep)
 				m_scratch_world_to_local.resize(nLinks + 1);
 				m_scratch_local_origin.resize(nLinks + 1);
                 bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
+				bod->substractSplitV();
 			}
 			else
 			{
diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h
index d4739da83..8fdc5d107 100644
--- a/src/BulletSoftBody/btConjugateResidual.h
+++ b/src/BulletSoftBody/btConjugateResidual.h
@@ -29,7 +29,7 @@ class btConjugateResidual : public btKrylovSolver<MatrixX>
     btScalar best_r;
 public:
     btConjugateResidual(const int max_it_in)
-    : Base(max_it_in, 1e-8)
+    : Base(max_it_in, 1e-4)
     {
     }
     
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 736fe4695..321492882 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -18,7 +18,7 @@
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyInternals.h"
 #include "LinearMath/btQuickprof.h"
-static const int kMaxConjugateGradientIterations = 50;
+static const int kMaxConjugateGradientIterations = 300;
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
 , m_cg(kMaxConjugateGradientIterations)
@@ -437,7 +437,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
         }
         n.m_q = n.m_x + n.m_v * dt;
         n.m_splitv.setZero();
-        n.m_penetration = 0;
+        n.m_constrained = false;
     }
 
     /* Nodes                */
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index dda9d5ef6..907639bb2 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -210,34 +210,87 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
     return va;
 }
 
+btVector3 btDeformableRigidContactConstraint::getSplitVa() const
+{
+    const btSoftBody::sCti& cti = m_contact->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->getPushVelocityInLocalPoint(m_contact->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_contact->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+                const btScalar* local_split_v = multibodyLinkCol->m_multiBody->getSplitVelocityVector();
+                // add in the normal component of the va
+                btScalar vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += local_split_v[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_split_v[k] * J_t1[k];
+                }
+                va += m_contact->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += local_split_v[k] * J_t2[k];
+                }
+                va += m_contact->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
 btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     const btSoftBody::sCti& cti = m_contact->m_cti;
     btVector3 va = getVa();
     btVector3 vb = getVb();
     btVector3 vr = vb - va;
-	btScalar dn = btDot(vr, cti.m_normal);
-	if (!infoGlobal.m_splitImpulse)
-	{
-		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
-	}
+    btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.m_deformable_cfm;
+    if (m_penetration > 0)
+    {
+        dn += m_penetration / infoGlobal.m_timeStep;
+    }
+    if (!infoGlobal.m_splitImpulse)
+    {
+        dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+    }
     // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
-    btVector3 impulse = m_contact->m_c0 * vr;
-	if (!infoGlobal.m_splitImpulse)
-	{
-		impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
-	}
+    btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0,0,0)));
+    if (!infoGlobal.m_splitImpulse)
+    {
+        impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal);
+    }
     btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
     btVector3 impulse_tangent = impulse - impulse_normal;
-	if (dn > 0)
-	{
-		dn = 0;
-		impulse_normal.setZero();
-		m_binding = false;
-		return 0;
-	}
-	m_binding = true;
-	btScalar residualSquare = dn*dn;
+    if (dn > 0)
+    {
+        m_binding = false;
+        return 0;
+    }
+    m_binding = true;
+    btScalar residualSquare = dn*dn;
     btVector3 old_total_tangent_dv = m_total_tangent_dv;
     // m_c2 is the inverse mass of the deformable node/face
     m_total_normal_dv -= impulse_normal * m_contact->m_c2;
@@ -312,8 +365,14 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
 	btScalar MAX_PENETRATION_CORRECTION = 0.1;
 	const btSoftBody::sCti& cti = m_contact->m_cti;
 	btVector3 vb = getSplitVb();
+	btVector3 va = getSplitVa();
 	btScalar p = m_penetration;
-	btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
+	if (p > 0)
+	{
+		return 0;
+	}
+	btVector3 vr = vb - va;
+	btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	if (dn > 0)
 	{
 		return 0;
@@ -329,8 +388,30 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
 	m_total_split_impulse += dn;
 
 	btScalar residualSquare = dn*dn;
-	const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn);
+	const btVector3 impulse = m_contact->m_c0 * (cti.m_normal * dn);
 	applySplitImpulse(impulse);
+    
+    // apply split 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->applyPushImpulse(impulse, m_contact->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_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaSplitVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
+        }
+    }
 	return residualSquare;
 }
 /* ================   Node vs. Rigid   =================== */
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h
index 9c4f58ca1..7997c762a 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.h
+++ b/src/BulletSoftBody/btDeformableContactConstraint.h
@@ -165,6 +165,9 @@ public:
     // get the split impulse velocity of the deformable face at the contact point
     virtual btVector3 getSplitVb() const = 0;
     
+    // get the split impulse velocity of the rigid/multibdoy at the contaft
+    virtual btVector3 getSplitVa() const;
+    
     virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual void setPenetrationScale(btScalar scale)
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 4e88c79d9..054b92099 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -126,15 +126,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in
 				continue;
 			}
 			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
-			btVector3 va = constraint.getVa();
-			btVector3 vb = constraint.getVb();
-			const btVector3 vr = vb - va;
-			const btSoftBody::sCti& cti = contact.m_cti;
-			const btScalar dn = btDot(vr, cti.m_normal);
-			if (dn < SIMD_EPSILON)
-			{
-				m_nodeRigidConstraints[i].push_back(constraint);
-			}
+			m_nodeRigidConstraints[i].push_back(constraint);
 		}
 		
 		// set Deformable Face vs. Rigid constraint
@@ -229,7 +221,7 @@ void btDeformableContactProjection::setProjection()
         for (int j = 0; j < m_staticConstraints[i].size(); ++j)
         {
             int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+            m_staticConstraints[i][j].m_node->m_constrained = true;
             if (m_projectionsDict.find(index) == NULL)
             {
                 m_projectionsDict.insert(index, units);
@@ -246,7 +238,7 @@ void btDeformableContactProjection::setProjection()
         for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
         {
             int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
             if (m_projectionsDict.find(index) == NULL)
             {
                 m_projectionsDict.insert(index, units);
@@ -263,7 +255,7 @@ void btDeformableContactProjection::setProjection()
         for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
         {
             int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
             if (m_nodeRigidConstraints[i][j].m_static)
             {
                 if (m_projectionsDict.find(index) == NULL)
@@ -297,15 +289,16 @@ void btDeformableContactProjection::setProjection()
         for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
         {
             const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
-            for (int k = 0; k < 3; ++k)
-            {
-                face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
-            }
+            if (m_faceRigidConstraints[i][j].m_binding)
+			{
+				for (int k = 0; k < 3; ++k)
+				{
+					face->m_n[k]->m_constrained = true;
+				}
+			}
             for (int k = 0; k < 3; ++k)
             {
                 btSoftBody::Node* node = face->m_n[k];
-                node->m_penetration = true;
                 int index = node->index;
                 if (m_faceRigidConstraints[i][j].m_static)
                 {
@@ -477,7 +470,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
         for (int j = 0; j < m_staticConstraints[i].size(); ++j)
         {
             int index = m_staticConstraints[i][j].m_node->index;
-            m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY;
+            m_staticConstraints[i][j].m_node->m_constrained = true;
             LagrangeMultiplier lm;
             lm.m_num_nodes = 1;
             lm.m_indices[0] = index;
@@ -491,7 +484,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
         for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j)
         {
             int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index;
-            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY;
+            m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true;
             LagrangeMultiplier lm;
             lm.m_num_nodes = 1;
             lm.m_indices[0] = index;
@@ -509,7 +502,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
                 continue;
             }
             int index = m_nodeRigidConstraints[i][j].m_node->index;
-            m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset;
+			m_nodeRigidConstraints[i][j].m_node->m_constrained = true;
             LagrangeMultiplier lm;
             lm.m_num_nodes = 1;
             lm.m_indices[0] = index;
@@ -537,12 +530,11 @@ void btDeformableContactProjection::setLagrangeMultiplier()
             const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 			
             btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
-            btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 3;
 			for (int k = 0; k<3; ++k)
 			{
-				face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration);
+				face->m_n[k]->m_constrained = true;
 				lm.m_indices[k] = face->m_n[k]->index;
 				lm.m_weights[k] = bary[k];
 			}
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 3fc3ce964..969ff1ad0 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -532,12 +532,13 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
     if (m_useProjection)
     {
         m_deformableBodySolver->m_useProjection = true;
-//        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
+        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
         m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_massPreconditioner;
     }
     else
     {
         m_deformableBodySolver->m_useProjection = false;
+        m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
         m_deformableBodySolver->m_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_KKTPreconditioner;
     }
         
diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h
index c2db448ef..4dc6fcd71 100644
--- a/src/BulletSoftBody/btPreconditioner.h
+++ b/src/BulletSoftBody/btPreconditioner.h
@@ -178,7 +178,7 @@ public:
             }
         }
     }
-#define USE_FULL_PRECONDITIONER
+//#define USE_FULL_PRECONDITIONER
 #ifndef USE_FULL_PRECONDITIONER
     virtual void operator()(const TVStack& x, TVStack& b)
     {
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index f1ab3460f..4382481ea 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2877,93 +2877,28 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         return (dst < 0);
     }
 #endif
-    
-//    // regular face contact
-//    {
-//        btGjkEpaSolver2::sResults results;
-//        btTransform triangle_transform;
-//        triangle_transform.setIdentity();
-//        triangle_transform.setOrigin(f.m_n[0]->m_x);
-//        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-//        btVector3 guess(0,0,0);
-//        if (predict)
-//        {
-//            triangle_transform.setOrigin(f.m_n[0]->m_q);
-//            triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
-//        }
-//        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-////        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-////        dst = results.distance - margin;
-////        contact_point = results.witnesses[0];
-//        btGjkEpaSolver2::Penetration(&triangle, triangle_transform, csh, wtr, guess, results);
-//        if (results.status == btGjkEpaSolver2::sResults::Separated)
-//            return false;
-//        dst = results.distance - margin;
-//        contact_point = results.witnesses[1];
-//        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-//        nrm = results.normal;
-//        for (int i = 0; i < 3; ++i)
-//            f.m_pcontact[i] = bary[i];
-//    }
-//
-//    if (!predict)
-//    {
-//        cti.m_colObj = colObjWrap->getCollisionObject();
-//        cti.m_normal = nrm;
-//        cti.m_offset = dst;
-//    }
-//
-    
-    // regular face contact
-    if (0)
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_q);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance-csh->getMargin();
-        dst -= margin;
-        if (dst >= 0)
-            return false;
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, bary);
-        btVector3 curr = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        nrm = results.normal;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        cti.m_normal = nrm;
-        cti.m_offset = dst + (curr - contact_point).dot(nrm);
-    }
-    if(1)
-    {
-        btGjkEpaSolver2::sResults results;
-        btTransform triangle_transform;
-        triangle_transform.setIdentity();
-        triangle_transform.setOrigin(f.m_n[0]->m_q);
-        btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
-        btVector3 guess(0,0,0);
-        const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-        btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-        dst = results.distance-csh->getMargin();
-        dst -= margin;
-        if (dst >= 0)
-            return false;
-        wtr = colObjWrap->getWorldTransform();
-        btTriangleShape triangle2(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
-        triangle_transform.setOrigin(f.m_n[0]->m_x);
-        btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
-        contact_point = results.witnesses[0];
-        getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-        dst = results.distance-csh->getMargin()-margin;
-        cti.m_colObj = colObjWrap->getCollisionObject();
-        cti.m_normal = results.normal;
-        cti.m_offset = dst;
-        return true;
-    }
-    return true;
+	btGjkEpaSolver2::sResults results;
+	btTransform triangle_transform;
+	triangle_transform.setIdentity();
+	triangle_transform.setOrigin(f.m_n[0]->m_q);
+	btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
+	btVector3 guess(0,0,0);
+	const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+	btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+	dst = results.distance-2.0*csh->getMargin()-margin; // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
+	if (dst >= 0)
+		return false;
+	wtr = colObjWrap->getWorldTransform();
+	btTriangleShape triangle2(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+	triangle_transform.setOrigin(f.m_n[0]->m_x);
+	btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+	contact_point = results.witnesses[0];
+	getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+	dst = results.distance-csh->getMargin()-margin;
+	cti.m_colObj = colObjWrap->getCollisionObject();
+	cti.m_normal = results.normal;
+	cti.m_offset = dst;
+	return true;
 }
 //
 void btSoftBody::updateNormals()
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 954adf4d1..63c2adf9a 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -269,7 +269,7 @@ public:
 		btScalar m_im;       // 1/mass
 		btScalar m_area;     // Area
 		btDbvtNode* m_leaf;  // Leaf data
-		btScalar m_penetration;   // depth of penetration
+		int m_constrained;   // depth of penetration
 		int m_battach : 1;   // Attached
 		int index;
 		btVector3 m_splitv;  // velocity associated with split impulse
@@ -1313,20 +1313,22 @@ public:
 				I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
 			if (vn < 0)
 				I += 0.5 * mass * vn;
-			btScalar face_penetration = 0, node_penetration = node->m_penetration;
+			int face_penetration = 0, node_penetration = node->m_constrained;
 			for (int i = 0; i < 3; ++i)
-				face_penetration =  btMax(face_penetration, face->m_n[i]->m_penetration);
+				face_penetration |=  face->m_n[i]->m_constrained;
 			btScalar I_tilde = .5 *I /(1.0+w.length2());
 			
 //             double the impulse if node or face is constrained.
             if (face_penetration > 0 || node_penetration > 0)
-                I_tilde *= 2.0;
-            if (face_penetration <= node_penetration)
+			{
+				I_tilde *= 2.0;
+			}
+            if (face_penetration <= 0)
 			{
 				for (int j = 0; j < 3; ++j)
 					face->m_n[j]->m_v += w[j]*n*I_tilde*node->m_im;
 			}
-            if (face_penetration >= node_penetration)
+            if (node_penetration <= 0)
 			{
 				node->m_v -= I_tilde*node->m_im*n;
 			}
@@ -1344,12 +1346,12 @@ public:
 //                 double the impulse if node or face is constrained.
 //                if (face_penetration > 0 || node_penetration > 0)
 //                    I_tilde *= 2.0;
-                if (face_penetration <= node_penetration)
+                if (face_penetration <= 0)
 				{
 					for (int j = 0; j < 3; ++j)
 						face->m_n[j]->m_v += w[j] * vt * I_tilde * (face->m_n[j])->m_im;
 				}
-                if (face_penetration >= node_penetration)
+                if (node_penetration <= 0)
 				{
 					node->m_v -= I_tilde * node->m_im * vt;
 				}