combat friction drift in positionCorrect by changing velocity and change it back (effectively only changing position)

15 files changed, 291 insertions, 152 deletions

diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp
index b70751e2f..a43fc04ca 100644
--- a/examples/DeformableContact/DeformableContact.cpp
+++ b/examples/DeformableContact/DeformableContact.cpp
@@ -54,13 +54,6 @@ class DeformableContact : public CommonMultiBodyBase
     btMultiBody* m_multiBody;
     btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
 public:
-    
-    struct TetraBunny
-    {
-        #include "../SoftDemo/bunny.inl"
-    };
-
-    
 	DeformableContact(struct GUIHelperInterface* helper)
 		: CommonMultiBodyBase(helper)
 	{
@@ -147,7 +140,7 @@ void DeformableContact::initPhysics()
         btTransform groundTransform;
         groundTransform.setIdentity();
         groundTransform.setOrigin(btVector3(0, -40, 0));
-        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1));
         //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
         btScalar mass(0.);
 
@@ -170,16 +163,16 @@ void DeformableContact::initPhysics()
 
     
     {
-        bool damping = true;
-        bool gyro = true;
-        int numLinks = 3;
+        bool damping = false;
+        bool gyro = false;
+        int numLinks = 0;
         bool spherical = true;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
         bool canSleep = false;
         bool selfCollide = true;
         btVector3 linkHalfExtents(1, 1, 1);
         btVector3 baseHalfExtents(1, 1, 1);
         
-        btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+        btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
         
         mbC->setCanSleep(canSleep);
         mbC->setHasSelfCollision(selfCollide);
@@ -216,12 +209,13 @@ void DeformableContact::initPhysics()
     
     // create a patch of cloth
     {
-        const btScalar s = 6;
+        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,
+//                                                         20,20,
+                                                         3,3,
                                                          1 + 2 + 4 + 8, true);
 
         psb->getCollisionShape()->setMargin(0.25);
@@ -232,7 +226,6 @@ void DeformableContact::initPhysics()
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 0;
         getDeformableDynamicsWorld()->addSoftBody(psb);
-
     }
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp
index 7ce94a314..aa58fbd6f 100644
--- a/examples/DeformableDemo/DeformableDemo.cpp
+++ b/examples/DeformableDemo/DeformableDemo.cpp
@@ -105,15 +105,15 @@ public:
 //        }
         btTransform startTransform;
         startTransform.setIdentity();
-        startTransform.setOrigin(btVector3(1, 2, 1));
+        startTransform.setOrigin(btVector3(1, 1.5, 1));
         createRigidBody(mass, startTransform, shape[0]);
-        startTransform.setOrigin(btVector3(1, 2, -1));
+        startTransform.setOrigin(btVector3(1, 1.5, -1));
         createRigidBody(mass, startTransform, shape[0]);
-        startTransform.setOrigin(btVector3(-1, 2, 1));
+        startTransform.setOrigin(btVector3(-1, 1.5, 1));
         createRigidBody(mass, startTransform, shape[0]);
-        startTransform.setOrigin(btVector3(-1, 2, -1));
+        startTransform.setOrigin(btVector3(-1, 1.5, -1));
         createRigidBody(mass, startTransform, shape[0]);
-        startTransform.setOrigin(btVector3(0, 4, 0));
+        startTransform.setOrigin(btVector3(0, 3.5, 0));
         createRigidBody(mass, startTransform, shape[0]);
     }
     
@@ -182,7 +182,7 @@ void DeformableDemo::initPhysics()
 
         btTransform groundTransform;
         groundTransform.setIdentity();
-        groundTransform.setOrigin(btVector3(0, -30, 0));
+        groundTransform.setOrigin(btVector3(0, -32, 0));
         groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
         //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
         btScalar mass(0.);
@@ -206,7 +206,7 @@ void DeformableDemo::initPhysics()
     
     // create a piece of cloth
     {
-        bool onGround = true;
+        bool onGround = false;
         const btScalar s = 4;
         btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
                                                          btVector3(+s, 0, -s),
@@ -222,13 +222,14 @@ void DeformableDemo::initPhysics()
                                                  btVector3(+s, 0, -s),
                                                  btVector3(-s, 0, +s),
                                                  btVector3(+s, 0, +s),
-                                                 20,20,
+//                                                 20,20,
+                                                 2,2,
                                                  0, true);
         
         psb->getCollisionShape()->setMargin(0.1);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(10);
-        psb->setSpringStiffness(10);
+        psb->setTotalMass(1);
+        psb->setSpringStiffness(1);
         psb->setDampingCoefficient(0.01);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp
index cedc51750..cc3008f9c 100644
--- a/examples/Pinch/Pinch.cpp
+++ b/examples/Pinch/Pinch.cpp
@@ -73,9 +73,9 @@ public:
 	void resetCamera()
 	{
         float dist = 25;
-        float pitch = -45;
+        float pitch = -30;
         float yaw = 100;
-        float targetPos[3] = {0, -3, 0};
+        float targetPos[3] = {0, -0, 0};
 		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
 	}
     
@@ -137,17 +137,56 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
         return;
     btRigidBody* rb0 = rbs[0];
     btScalar pressTime = 0.9;
+    btScalar liftTime = 2.5;
+    btScalar shiftTime = 3.5;
+    btScalar holdTime = 4.5*1000;
+    btScalar dropTime = 5.3*1000;
     btTransform rbTransform;
     rbTransform.setIdentity();
-    btVector3 translation = btVector3(0.5,3,4);
-    btVector3 velocity = btVector3(0,5,0);
+    btVector3 translation;
+    btVector3 velocity;
+    
+    btVector3 initialTranslationLeft = btVector3(0.5,3,4);
+    btVector3 initialTranslationRight = btVector3(0.5,3,-4);
+    btVector3 pinchVelocityLeft = btVector3(0,0,-2);
+    btVector3 pinchVelocityRight = btVector3(0,0,2);
+    btVector3 liftVelocity = btVector3(0,5,0);
+    btVector3 shiftVelocity = btVector3(0,0,5);
+    btVector3 holdVelocity = btVector3(0,0,0);
+    btVector3 openVelocityLeft = btVector3(0,0,4);
+    btVector3 openVelocityRight = btVector3(0,0,-4);
+    
     if (time < pressTime)
     {
-        velocity = btVector3(0,0,-2);
-        translation += velocity * time;
+        velocity = pinchVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
     }
     else
-        translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity;
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
+    }
     rbTransform.setOrigin(translation);
     rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
     rb0->setCenterOfMassTransform(rbTransform);
@@ -155,23 +194,45 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world)
     rb0->setLinearVelocity(velocity);
     
     btRigidBody* rb1 = rbs[1];
-    translation = btVector3(0.5,3,-4);
-    velocity = btVector3(0,5,0);
     if (time < pressTime)
     {
-        velocity = btVector3(0,0,2);
-        translation += velocity * time;
+        velocity = pinchVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * time;
+    }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
+        
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
     }
     else
-        translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity;
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
+    }
     rbTransform.setOrigin(translation);
     rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
     rb1->setCenterOfMassTransform(rbTransform);
     rb1->setAngularVelocity(btVector3(0,0,0));
     rb1->setLinearVelocity(velocity);
     
-    rb0->setFriction(2);
-    rb1->setFriction(2);
+    rb0->setFriction(20);
+    rb1->setFriction(20);
 }
 
 void Pinch::initPhysics()
@@ -194,7 +255,7 @@ void Pinch::initPhysics()
 	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
     
-    getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
+    getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     //create a ground
@@ -229,21 +290,58 @@ void Pinch::initPhysics()
     
     // create a soft block
     {
+        btScalar verts[24] = {0.f, 0.f, 0.f,
+            1.f, 0.f, 0.f,
+            0.f, 1.f, 0.f,
+            0.f, 0.f, 1.f,
+            1.f, 1.f, 0.f,
+            0.f, 1.f, 1.f,
+            1.f, 0.f, 1.f,
+            1.f, 1.f, 1.f
+        };
+        int triangles[60] = {0, 6, 3,
+            0,1,6,
+            7,5,3,
+            7,3,6,
+            4,7,6,
+            4,6,1,
+            7,2,5,
+            7,4,2,
+            0,3,2,
+            2,3,5,
+            0,2,4,
+            0,4,1,
+            0,6,5,
+            0,6,4,
+            3,4,2,
+            3,4,7,
+            2,7,3,
+            2,7,1,
+            4,5,0,
+            4,5,6,
+        };
+//       btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20);
+////
         btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
                                                                   TetraCube::getElements(),
                                                                   0,
                                                                   TetraCube::getNodes(),
                                                                   false, true, true);
-        getDeformableDynamicsWorld()->addSoftBody(psb);
+        
         psb->scale(btVector3(2, 2, 2));
         psb->translate(btVector3(0, 4, 0));
         psb->getCollisionShape()->setMargin(0.1);
         psb->setTotalMass(1);
-        psb->setSpringStiffness(10);
-        psb->setDampingCoefficient(0.01);
+//        psb->scale(btVector3(5, 5, 5));
+//        psb->translate(btVector3(-2.5, 4, -2.5));
+//        psb->getCollisionShape()->setMargin(0.1);
+//        psb->setTotalMass(1);
+        psb->setSpringStiffness(4);
+        psb->setDampingCoefficient(0.02);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 0.5;
+        psb->m_cfg.kDF = 2;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
         // add a grippers
         createGrip();
     }
diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp
index b30e9bc24..9bcf63ab2 100644
--- a/examples/VolumetricDeformable/VolumetricDeformable.cpp
+++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp
@@ -77,8 +77,8 @@ public:
     void stepSimulation(float deltaTime)
     {
         //use a smaller internal timestep, there are stability issues
-        float internalTimeStep = 1. / 240.f;
-        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        float internalTimeStep = 1. / 480.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep);
     }
     
     void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index e5097ccbb..11fa97cdd 100644
--- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 				//				printf("error in island management\n");
 			}
 
-			btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+//            btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 			if (colObj0->getIslandTag() == islandId)
 			{
 				if (colObj0->getActivationState() == ACTIVE_TAG ||
@@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+//                btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
 
 				if (colObj0->getIslandTag() == islandId)
 				{
@@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi
 					//					printf("error in island management\n");
 				}
 
-				btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+//                 btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
 
 				if (colObj0->getIslandTag() == islandId)
 				{
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 99ab6f473..e81cb2c50 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -50,12 +50,12 @@ void btDeformableContactProjection::update()
     // loop through constraints to set constrained values
     for (auto& it : m_constraints)
     {
-        btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
-        btAlignedObjectArray<Constraint>& constraints = it.second;
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
         for (int i = 0; i < constraints.size(); ++i)
         {
-            Constraint& constraint = constraints[i];
-            Friction& friction = frictions[i];
+            DeformableContactConstraint& constraint = constraints[i];
+            DeformableFrictionConstraint& friction = frictions[i];
             for (int j = 0; j < constraint.m_contact.size(); ++j)
             {
                 if (constraint.m_contact[j] == nullptr)
@@ -229,16 +229,16 @@ void btDeformableContactProjection::setConstraints()
         {
             if (psb->m_nodes[j].m_im == 0)
             {
-                btAlignedObjectArray<Constraint> c;
-                c.push_back(Constraint(btVector3(1,0,0)));
-                c.push_back(Constraint(btVector3(0,1,0)));
-                c.push_back(Constraint(btVector3(0,0,1)));
+                btAlignedObjectArray<DeformableContactConstraint> c;
+                c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
                 m_constraints[&(psb->m_nodes[j])] = c;
                 
-                btAlignedObjectArray<Friction> f;
-                f.push_back(Friction());
-                f.push_back(Friction());
-                f.push_back(Friction());
+                btAlignedObjectArray<DeformableFrictionConstraint> f;
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
                 m_frictions[&(psb->m_nodes[j])] = f;
             }
         }
@@ -310,11 +310,11 @@ void btDeformableContactProjection::setConstraints()
                     
                     if (m_constraints.find(c.m_node) == m_constraints.end())
                     {
-                        btAlignedObjectArray<Constraint> constraints;
-                        constraints.push_back(Constraint(c, jacobianData_normal));
+                        btAlignedObjectArray<DeformableContactConstraint> constraints;
+                        constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
                         m_constraints[c.m_node] = constraints;
-                        btAlignedObjectArray<Friction> frictions;
-                        frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
+                        btAlignedObjectArray<DeformableFrictionConstraint> frictions;
+                        frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
                         m_frictions[c.m_node] = frictions;
                     }
                     else
@@ -322,8 +322,8 @@ void btDeformableContactProjection::setConstraints()
                         // group colinear constraints into one
                         const btScalar angle_epsilon = 0.015192247; // less than 10 degree
                         bool merged = false;
-                        btAlignedObjectArray<Constraint>& constraints = m_constraints[c.m_node];
-                        btAlignedObjectArray<Friction>& frictions = m_frictions[c.m_node];
+                        btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
+                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
                         for (int j = 0; j < constraints.size(); ++j)
                         {
                             const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
@@ -343,8 +343,8 @@ void btDeformableContactProjection::setConstraints()
                         // hard coded no more than 3 constraint directions
                         if (!merged && constraints.size() < dim)
                         {
-                            constraints.push_back(Constraint(c, jacobianData_normal));
-                            frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
+                            constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
+                            frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
                         }
                     }
                 }
@@ -358,9 +358,9 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
     const int dim = 3;
     for (auto& it : m_constraints)
     {
-        const btAlignedObjectArray<Constraint>& constraints = it.second;
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
         size_t i = m_indices[it.first];
-        const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
+        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
         if (constraints.size() == 1)
@@ -401,7 +401,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
         {
             for (int f = 0; f < frictions.size(); ++f)
             {
-                const Friction& friction= frictions[f];
+                const DeformableFrictionConstraint& friction= frictions[f];
                 for (int j = 0; j < friction.m_direction.size(); ++j)
                 {
                     // clear the old constraint
@@ -425,9 +425,9 @@ void btDeformableContactProjection::project(TVStack& x)
     const int dim = 3;
     for (auto& it : m_constraints)
     {
-        const btAlignedObjectArray<Constraint>& constraints = it.second;
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
         size_t i = m_indices[it.first];
-        btAlignedObjectArray<Friction>& frictions = m_frictions[it.first];
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
         if (constraints.size() == 1)
@@ -450,14 +450,14 @@ void btDeformableContactProjection::project(TVStack& x)
             bool has_static_constraint = false;
             for (int f = 0; f < frictions.size(); ++f)
             {
-                Friction& friction= frictions[f];
+                DeformableFrictionConstraint& friction= frictions[f];
                 for (int j = 0; j < friction.m_static.size(); ++j)
                     has_static_constraint = has_static_constraint || friction.m_static[j];
             }
             
             for (int f = 0; f < frictions.size(); ++f)
             {
-                Friction& friction= frictions[f];
+                DeformableFrictionConstraint& friction= frictions[f];
                 for (int j = 0; j < friction.m_direction.size(); ++j)
                 {
                     // clear the old friction force
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 96d2e0029..e3009dcbb 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -29,6 +29,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt)
     
     // save v_{n+1}^* velocity after explicit forces
     backupVelocity();
+    
     m_objective->computeResidual(solverdt, m_residual);
 //   m_objective->initialGuess(m_dv, m_residual);
     computeStep(m_dv, m_residual);
@@ -98,6 +99,20 @@ void btDeformableBodySolver::backupVelocity()
     }
 }
 
+void btDeformableBodySolver::revertVelocity()
+{
+    // serial implementation
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter++];
+        }
+    }
+}
+
 bool btDeformableBodySolver::updateNodes()
 {
     int numNodes = 0;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 570cce90f..0b4a4819f 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -27,12 +27,14 @@ protected:
     TVStack m_dv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
-    btDeformableBackwardEulerObjective* m_objective;
+   
     btAlignedObjectArray<btVector3> m_backupVelocity;
     btScalar m_dt;
     btConjugateGradient<btDeformableBackwardEulerObjective> cg;
     
 public:
+    btDeformableBackwardEulerObjective* m_objective;
+    
     btDeformableBodySolver();
     
     virtual ~btDeformableBodySolver();
@@ -57,7 +59,7 @@ public:
     void predictDeformableMotion(btSoftBody* psb, btScalar dt);
     
     void backupVelocity();
-    
+    void revertVelocity();
     void updateVelocity();
     
     bool updateNodes();
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index e81cb2c50..25f667711 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -143,6 +143,8 @@ void btDeformableContactProjection::update()
                         friction.m_direction[j] = -local_tangent_dir;
                         // do not allow switching from static friction to dynamic friction
                         // it causes cg to explode
+                        btScalar comp1 = -accumulated_normal*c->m_c3;
+                        btScalar comp2 = tangent_norm;
                         if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
                         {
                             friction.m_static[j] = false;
@@ -167,49 +169,44 @@ void btDeformableContactProjection::update()
                     // the incremental impulse applied to rb in the tangential direction
                     btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
                     
-                    // TODO cleanup
-                    if (1) // in the same CG solve, the set of constraits doesn't change
-                    {
-                        // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
-                        
-                        // dv = new_impulse + accumulated velocity change in previous CG iterations
-                        // so we have the invariant node->m_v = backupVelocity + dv;
 
-                        btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
-                        
-                        // the following is equivalent
-                        /*
-                            btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
-                            btScalar dvn = dv.dot(cti.m_normal);
-                         */
-                        
-                        constraint.m_value[j] = dvn;
+                    // dv = new_impulse + accumulated velocity change in previous CG iterations
+                    // so we have the invariant node->m_v = backupVelocity + dv;
+
+                    btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
+                    
+                    // the following is equivalent
+                    /*
+                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
+                        btScalar dvn = dv.dot(cti.m_normal);
+                     */
+                    
+                    constraint.m_value[j] = dvn;
+                    
+                    // the incremental impulse:
+                    // in the normal direction it's the normal component of "impulse"
+                    // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
+                    impulse = impulse_normal + incremental_tangent;
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    {
+                        if (rigidCol)
+                            rigidCol->applyImpulse(impulse, c->m_c1);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
                         
-                        // the incremental impulse:
-                        // in the normal direction it's the normal component of "impulse"
-                        // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
-                        impulse = impulse_normal + incremental_tangent;
-                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                        {
-                            if (rigidCol)
-                                rigidCol->applyImpulse(impulse, c->m_c1);
-                        }
-                        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                        if (multibodyLinkCol)
                         {
+                            double multiplier = 1;
+                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
                             
-                            if (multibodyLinkCol)
+                            if (incremental_tangent.norm() > SIMD_EPSILON)
                             {
-                                double multiplier = 1;
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
-                                
-                                if (incremental_tangent.norm() > SIMD_EPSILON)
-                                {
-                                    btMultiBodyJacobianData jacobian_tangent;
-                                    btVector3 tangent = incremental_tangent.normalized();
-                                    findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
-                                    const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
-                                    multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
-                                }
+                                btMultiBodyJacobianData jacobian_tangent;
+                                btVector3 tangent = incremental_tangent.normalized();
+                                findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
+                                const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
+                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
                             }
                         }
                     }
@@ -404,14 +401,10 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
                 const DeformableFrictionConstraint& friction= frictions[f];
                 for (int j = 0; j < friction.m_direction.size(); ++j)
                 {
-                    // clear the old constraint
-                    if (friction.m_static_prev[j] == true)
-                    {
-                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
-                    }
-                    // add the new constraint
+                    // add the friction constraint
                     if (friction.m_static[j] == true)
                     {
+                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
                         x[i] += friction.m_direction[j] * friction.m_dv[j];
                     }
                 }
@@ -467,9 +460,15 @@ void btDeformableContactProjection::project(TVStack& x)
                     }
                     
                     // only add to the rhs if there is no static friction constraint on the node
-                    if (friction.m_static[j] == false && !has_static_constraint)
+                    if (friction.m_static[j] == false)
+                    {
+                        if (!has_static_constraint)
+                            x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                    }
+                    else
                     {
-                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                        // otherwise clear the constraint in the friction direction
+                        x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j];
                     }
                 }
             }
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
index 545615a47..d8571fa73 100644
--- a/src/BulletSoftBody/btDeformableGravityForce.h
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -23,6 +23,7 @@ public:
     
     virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
     {
+//        addScaledGravityForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 31389c20f..70ad6e69c 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -22,6 +22,7 @@ public:
     virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
     {
         addScaledDampingForce(scale, force);
+//        addScaledElasticForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -102,6 +103,8 @@ public:
             }
         }
     }
+    
+    
 };
 
 #endif /* btMassSpring_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 3af3c8499..216c1a24d 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -13,7 +13,7 @@
 void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     reinitialize(timeStep);
-    
+//    beforeSolverCallbacks(timeStep);
     // add gravity to velocity of rigid and multi bodys
     applyRigidBodyGravity(timeStep);
     
@@ -30,7 +30,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
     
-    positionCorrection();
+    afterSolverCallbacks(timeStep);
     
     integrateTransforms(timeStep);
     
@@ -42,36 +42,57 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     // ///////////////////////////////
 }
 
-void btDeformableRigidDynamicsWorld::positionCorrection()
+void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
 {
-    // perform position correction for all geometric collisions
-    for (int i = 0; i < m_softBodies.size(); ++i)
+    // perform position correction for all constraints 
+    for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints)
     {
-        btSoftBody* psb = m_softBodies[i];
-        const btScalar mrg = psb->getCollisionShape()->getMargin();
-        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        for (int i = 0; i < constraints.size(); ++i)
         {
-            const btSoftBody::RContact& c = psb->m_rcontacts[j];
-            // skip anchor points
-            if (c.m_node->m_im == 0)
-                continue;
-            
-            const btSoftBody::sCti& cti = c.m_cti;
-            if (cti.m_colObj->hasContactResponse())
+            DeformableContactConstraint& constraint = constraints[i];
+            DeformableFrictionConstraint& friction = frictions[i];
+            for (int j = 0; j < constraint.m_contact.size(); ++j)
             {
-                btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
-                if (dp < 0)
+                const btSoftBody::RContact* c = constraint.m_contact[j];
+                // skip anchor points
+                if (c == nullptr || c->m_node->m_im == 0)
+                    continue;
+                const btSoftBody::sCti& cti = c->m_cti;
+                btRigidBody* rigidCol = 0;
+                btVector3 va(0, 0, 0);
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
-                    // m_c4 is the collision hardness
-                    c.m_node->m_q -= dp * cti.m_normal * c.m_c4;
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
+                }
+                
+                if (cti.m_colObj->hasContactResponse())
+                {
+                    btScalar dp = cti.m_offset;
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    if (friction.m_static[j] == true)
+                    {
+                        c->m_node->m_v = va;
+                    }
+                    if (dp < 0)
+                    {
+                        c->m_node->m_v -= dp * cti.m_normal / dt;
+                    }
                 }
             }
         }
     }
 }
 
+
 void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
 {
+    m_deformableBodySolver->backupVelocity();
+    positionCorrection(dt);
     btMultiBodyDynamicsWorld::integrateTransforms(dt);
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
@@ -82,6 +103,7 @@ void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
             node.m_x  =  node.m_q + dt * node.m_v;
         }
     }
+    m_deformableBodySolver->revertVelocity();
 }
 
 void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
@@ -146,7 +168,12 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
     {
         (*m_internalTickCallback)(this, timeStep);
     }
-    
+    for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
+        m_beforeSolverCallbacks[i](m_internalTime, this);
+}
+
+void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
+{
     for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
         m_beforeSolverCallbacks[i](m_internalTime, this);
 }
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index c5cf280ef..f1be4fd85 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -47,7 +47,7 @@ protected:
     
     virtual void integrateTransforms(btScalar timeStep);
     
-    void positionCorrection();
+    void positionCorrection(btScalar dt);
     
     void solveDeformableBodiesConstraints(btScalar timeStep);
     
@@ -124,6 +124,8 @@ public:
     
     void beforeSolverCallbacks(btScalar timeStep);
     
+    void afterSolverCallbacks(btScalar timeStep);
+    
     int getDrawFlags() const { return (m_drawFlags); }
     void setDrawFlags(int f) { m_drawFlags = f; }
 };
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 876b6b6e4..bd7587c47 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2286,7 +2286,8 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
-		cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+//        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+        cti.m_offset = dst;
 		return (true);
 	}
 	return (false);
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index b892498ea..9b3e45ea4 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -878,15 +878,11 @@ struct btSoftColliders
                     const btScalar ms = ima + imb;
                     if (ms > 0)
                     {
+                        psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
                         const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
                         static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
                         const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
-                        const btVector3 ra = n.m_x - wtr.getOrigin();
-                        const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
-                        const btVector3 vb = n.m_x - n.m_q;
-                        const btVector3 vr = vb - va;
-                        const btScalar dn = btDot(vr, c.m_cti.m_normal);
-                        const btVector3 fv = vr - c.m_cti.m_normal * dn;
+                        const btVector3 ra = n.m_q - wtr.getOrigin();
                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
                         c.m_node = &n;
                         c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);