deformable code refactor

17 files changed, 1065 insertions, 382 deletions

diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 51b3d906d..73607c61f 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -229,6 +229,16 @@ public:
 	{
 		return m_latencyMotionStateInterpolation;
 	}
+    
+    btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies()
+    {
+        return m_nonStaticRigidBodies;
+    }
+    
+    const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const
+    {
+        return m_nonStaticRigidBodies;
+    }
 };
 
 #endif  //BT_DISCRETE_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index 68f7f66fe..16ec0a542 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -1,5 +1,5 @@
 //
-//  btBackwardEulerObjective.h
+//  btDeformableBackwardEulerObjective.h
 //  BulletSoftBody
 //
 //  Created by Xuchen Han on 7/1/19.
@@ -11,27 +11,26 @@
 #include "btConjugateGradient.h"
 #include "btLagrangianForce.h"
 #include "btMassSpring.h"
-#include "btContactProjection.h"
+#include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"
 #include "btDeformableRigidDynamicsWorld.h"
 
 class btDeformableRigidDynamicsWorld;
-class btBackwardEulerObjective
+class btDeformableBackwardEulerObjective
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
     btScalar m_dt;
-    btConjugateGradient<btBackwardEulerObjective> cg;
     btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     Preconditioner* m_preconditioner;
-    btContactProjection projection;
+    btDeformableContactProjection projection;
     const TVStack& m_backupVelocity;
     
-    btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
+    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
-    virtual ~btBackwardEulerObjective() {}
+    virtual ~btDeformableBackwardEulerObjective() {}
     
     void initialize(){}
     
@@ -53,18 +52,14 @@ public:
     // perform A*x = b
     void multiply(const TVStack& x, TVStack& b) const;
     
-    // update the constraints treated as projections
-    void updateProjection(const TVStack& dv)
-    {
-        projection.update(dv, m_backupVelocity);
-    }
-    
     // set initial guess for CG solve
     void initialGuess(TVStack& dv, const TVStack& residual);
     
     // reset data structure
     void reinitialize(bool nodeUpdated);
     
+    void setDt(btScalar dt);
+    
     // enforce constraints in CG solve
     void enforceConstraint(TVStack& x)
     {
@@ -82,10 +77,11 @@ public:
     }
     
     // update the projections and project the residual
-    void project(TVStack& r, const TVStack& dv)
+    void project(TVStack& r)
     {
-        updateProjection(dv);
-        projection(r);
+        projection.update();
+        // TODO rename
+        projection.project(r);
     }
     
     // perform precondition M^(-1) x = b
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index ff5eecd8c..09055c7d1 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -8,53 +8,61 @@
 #define BT_CG_PROJECTION_H
 
 #include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <unordered_map>
 
 class btDeformableRigidDynamicsWorld;
 
-struct Constraint
+struct DeformableContactConstraint
 {
     btAlignedObjectArray<const btSoftBody::RContact*> m_contact;
     btAlignedObjectArray<btVector3> m_direction;
     btAlignedObjectArray<btScalar> m_value;
     // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve
     btAlignedObjectArray<btScalar> m_accumulated_normal_impulse;
+    btAlignedObjectArray<btMultiBodyJacobianData> m_normal_jacobian;
     
-    Constraint(const btSoftBody::RContact& rcontact)
+    DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
     {
-        append(rcontact);
+        append(rcontact, jacobian);
     }
     
-    Constraint(const btVector3 dir)
+    DeformableContactConstraint(const btVector3 dir)
     {
         m_contact.push_back(nullptr);
         m_direction.push_back(dir);
         m_value.push_back(0);
         m_accumulated_normal_impulse.push_back(0);
+        btMultiBodyJacobianData j;
+        m_normal_jacobian.push_back(j);
     }
     
-    Constraint()
+    DeformableContactConstraint()
     {
         m_contact.push_back(nullptr);
         m_direction.push_back(btVector3(0,0,0));
         m_value.push_back(0);
         m_accumulated_normal_impulse.push_back(0);
+        btMultiBodyJacobianData j;
+        m_normal_jacobian.push_back(j);
     }
     
-    void append(const btSoftBody::RContact& rcontact)
+    void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian)
     {
         m_contact.push_back(&rcontact);
         m_direction.push_back(rcontact.m_cti.m_normal);
         m_value.push_back(0);
         m_accumulated_normal_impulse.push_back(0);
+        m_normal_jacobian.push_back(jacobian);
     }
     
-    ~Constraint()
+    ~DeformableContactConstraint()
     {
     }
 };
 
-struct Friction
+struct DeformableFrictionConstraint
 {
     
     btAlignedObjectArray<bool> m_static; // whether the friction is static
@@ -69,14 +77,22 @@ struct Friction
     btAlignedObjectArray<btVector3> m_direction_prev;
     
     btAlignedObjectArray<bool> m_released; // whether the contact is released
-    
+    btAlignedObjectArray<btMultiBodyJacobianData> m_complementary_jacobian;
+    btAlignedObjectArray<btVector3> m_complementaryDirection;
 
     
     // the total impulse the node applied to the rb in the tangential direction in the cg solve
     btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse;
-    Friction()
+    
+    DeformableFrictionConstraint()
+    {
+        append();
+    }
+    
+    DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
     {
         append();
+        addJacobian(complementaryDir, jacobian);
     }
     
     void append()
@@ -96,6 +112,13 @@ struct Friction
         m_accumulated_tangent_impulse.push_back(btVector3(0,0,0));
         m_released.push_back(false);
     }
+    
+    void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian)
+    {
+        m_complementary_jacobian.push_back(jacobian);
+        m_complementaryDirection.push_back(complementaryDir);
+    }
+    
 };
 
 class btCGProjection
@@ -109,8 +132,6 @@ public:
     btDeformableRigidDynamicsWorld* m_world;
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     const btScalar& m_dt;
-    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Constraint> > m_constraints;
-    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<Friction> > m_frictions;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
@@ -123,19 +144,17 @@ public:
     }
     
     // apply the constraints
-    virtual void operator()(TVStack& x) = 0;
+    virtual void project(TVStack& x) = 0;
     
     virtual void setConstraints() = 0;
     
     // update the constraints
-    virtual void update(const TVStack& dv, const TVStack& backup_v) = 0;
+    virtual void update() = 0;
     
     virtual void reinitialize(bool nodeUpdated)
     {
         if (nodeUpdated)
             updateId();
-        m_constraints.clear();
-        m_frictions.clear();
     }
     
     void updateId()
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index f23b26385..0c8384a00 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -36,7 +36,7 @@ public:
         // r = b - A * x --with assigned dof zeroed out
         A.multiply(x, temp);
         r = sub(b, temp);
-        A.project(r,x);
+        A.project(r);
         A.enforceConstraint(x);
         
         btScalar r_norm = std::sqrt(squaredNorm(r));
@@ -62,7 +62,7 @@ public:
             multAndAddTo(alpha, p, x);
             multAndAddTo(-alpha, temp, r);
             // zero out the dofs of r
-            A.project(r,x);
+            A.project(r);
             A.enforceConstraint(x);
             r_norm = std::sqrt(squaredNorm(r));
             
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index cf2e05e59..99ab6f473 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -1,14 +1,47 @@
 //
-//  btContactProjection.cpp
+//  btDeformableContactProjection.cpp
 //  BulletSoftBody
 //
 //  Created by Xuchen Han on 7/4/19.
 //
 
-#include "btContactProjection.h"
+#include "btDeformableContactProjection.h"
 #include "btDeformableRigidDynamicsWorld.h"
 #include <algorithm>
-void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    jacobianData.m_jacobians.resize(ndof);
+    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+    btScalar* jac = &jacobianData.m_jacobians[0];
+    
+    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+}
+
+static btVector3 generateUnitOrthogonalVector(const btVector3& u)
+{
+    btScalar ux = u.getX();
+    btScalar uy = u.getY();
+    btScalar uz = u.getZ();
+    btScalar ax = std::abs(ux);
+    btScalar ay = std::abs(uy);
+    btScalar az = std::abs(uz);
+    btVector3 v;
+    if (ax <= ay && ax <= az)
+        v = btVector3(0, -uz, uy);
+    else if (ay <= ax && ay <= az)
+        v = btVector3(-uz, 0, ux);
+    else
+        v = btVector3(-uy, ux, 0);
+    v.normalize();
+    return v;
+}
+
+void btDeformableContactProjection::update()
 {
     ///solve rigid body constraints
     m_world->getSolverInfo().m_numIterations = 10;
@@ -32,13 +65,17 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                 }
                 const btSoftBody::RContact* c = constraint.m_contact[j];
                 const btSoftBody::sCti& cti = c->m_cti;
-                btMultiBodyJacobianData jacobianData;
+                
+                // normal jacobian is precompute but tangent jacobian is not
+                const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
+                const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
+                
                 if (cti.m_colObj->hasContactResponse())
                 {
                     btVector3 va(0, 0, 0);
                     btRigidBody* rigidCol = 0;
                     btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                    btScalar* deltaV;
+                    const btScalar* deltaV_normal;
                     
                     // grab the velocity of the rigid body
                     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
@@ -52,20 +89,25 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         if (multibodyLinkCol)
                         {
                             const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                            jacobianData.m_jacobians.resize(ndof);
-                            jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
-                            btScalar* jac = &jacobianData.m_jacobians[0];
-                            
-                            multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c->m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
-                            deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
-                            multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
+                            const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
+                            deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
                             
+                            // add in the normal component of the va
                             btScalar vel = 0.0;
                             for (int k = 0; k < ndof; ++k)
                             {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k];
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
                             }
                             va = cti.m_normal * vel * m_dt;
+                            
+                            // add in complementary direction of va
+                            const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
+                            vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
+                            }
+                            va += friction.m_complementaryDirection[j] * vel * m_dt;
                         }
                     }
                     
@@ -109,7 +151,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         else
                         {
                             friction.m_static[j] = true;
-                            friction.m_impulse[j] = local_tangent_norm;
+                            friction.m_impulse[j] = tangent_norm;
                         }
                     }
                     else
@@ -125,15 +167,22 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                     // 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;
-//                        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);
+
                         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:
@@ -147,10 +196,20 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
                         }
                         else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                         {
+                            
                             if (multibodyLinkCol)
                             {
-                                double multiplier = 0.5;
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
+                                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);
+                                }
                             }
                         }
                     }
@@ -160,7 +219,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit
     }
 }
 
-void btContactProjection::setConstraints()
+void btDeformableContactProjection::setConstraints()
 {
     // set Dirichlet constraint
     for (int i = 0; i < m_softBodies.size(); ++i)
@@ -188,8 +247,8 @@ void btContactProjection::setConstraints()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
-        btMultiBodyJacobianData jacobianData;
-        btAlignedObjectArray<btScalar> jacobian;
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_complementary;
         
         for (int j = 0; j < psb->m_rcontacts.size(); ++j)
         {
@@ -206,7 +265,6 @@ void btContactProjection::setConstraints()
                 btVector3 va(0, 0, 0);
                 btRigidBody* rigidCol = 0;
                 btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                btScalar* deltaV;
                 
                 // grab the velocity of the rigid body
                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
@@ -216,25 +274,18 @@ void btContactProjection::setConstraints()
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
-                    jacobian.clear();
                     multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
                     if (multibodyLinkCol)
                     {
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        jacobianData.m_jacobians.resize(ndof);
-                        jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
-                        btScalar* jac = &jacobianData.m_jacobians[0];
-                        
-                        multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
-                        deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
-                        multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
-                        
+                        findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
                         btScalar vel = 0.0;
-                        jacobian.resize(ndof);
+                        const btScalar* jac = &jacobianData_normal.m_jacobians[0];
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
                         for (int j = 0; j < ndof; ++j)
                         {
                             vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
-                            jacobian[j] = jac[j];
+                            std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
+                            std::cout << jac[j] << std::endl;
                         }
                         va = cti.m_normal * vel * m_dt;
                     }
@@ -245,13 +296,25 @@ void btContactProjection::setConstraints()
                 const btScalar dn = btDot(vr, cti.m_normal);
                 if (dn < SIMD_EPSILON)
                 {
+                    // find complementary jacobian
+                    btVector3 complementaryDirection;
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
+                            findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
+                        }
+                    }
+                    
                     if (m_constraints.find(c.m_node) == m_constraints.end())
                     {
                         btAlignedObjectArray<Constraint> constraints;
-                        constraints.push_back(Constraint(c));
+                        constraints.push_back(Constraint(c, jacobianData_normal));
                         m_constraints[c.m_node] = constraints;
                         btAlignedObjectArray<Friction> frictions;
-                        frictions.push_back(Friction());
+                        frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
                         m_frictions[c.m_node] = frictions;
                     }
                     else
@@ -268,9 +331,10 @@ void btContactProjection::setConstraints()
                             if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
                             {
                                 // group the constraints
-                                constraints[j].append(c);
+                                constraints[j].append(c, jacobianData_normal);
                                 // push in an empty friction
                                 frictions[j].append();
+                                frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
                                 merged = true;
                                 break;
                             }
@@ -279,8 +343,8 @@ void btContactProjection::setConstraints()
                         // hard coded no more than 3 constraint directions
                         if (!merged && constraints.size() < dim)
                         {
-                            constraints.push_back(Constraint(c));
-                            frictions.push_back(Friction());
+                            constraints.push_back(Constraint(c, jacobianData_normal));
+                            frictions.push_back(Friction(complementaryDirection, jacobianData_complementary));
                         }
                     }
                 }
@@ -289,7 +353,7 @@ void btContactProjection::setConstraints()
     }
 }
 
-void btContactProjection::enforceConstraint(TVStack& x)
+void btDeformableContactProjection::enforceConstraint(TVStack& x)
 {
     const int dim = 3;
     for (auto& it : m_constraints)
@@ -356,7 +420,7 @@ void btContactProjection::enforceConstraint(TVStack& x)
     }
 }
 
-void btContactProjection::operator()(TVStack& x)
+void btDeformableContactProjection::project(TVStack& x)
 {
     const int dim = 3;
     for (auto& it : m_constraints)
@@ -412,3 +476,13 @@ void btContactProjection::operator()(TVStack& x)
         }
     }
 }
+
+void btDeformableContactProjection::reinitialize(bool nodeUpdated)
+{
+    btCGProjection::reinitialize(nodeUpdated);
+    m_constraints.clear();
+    m_frictions.clear();
+}
+
+
+
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 024cc66b6..31b0905ac 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -1,26 +1,26 @@
 //
-//  btBackwardEulerObjective.cpp
+//  btDeformableBackwardEulerObjective.cpp
 //  BulletSoftBody
 //
 //  Created by Xuchen Han on 7/9/19.
 //
 
-#include "btBackwardEulerObjective.h"
+#include "btDeformableBackwardEulerObjective.h"
 
-btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
-: cg(10)
-, m_softBodies(softBodies)
+btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+: m_softBodies(softBodies)
 , projection(m_softBodies, m_dt)
 , m_backupVelocity(backup_v)
 {
     // TODO: this should really be specified in initialization instead of here
-    btMassSpring* mass_spring = new btMassSpring(m_softBodies);
-//    m_preconditioner = new MassPreconditioner(m_softBodies);
+    btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
+    btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
     m_preconditioner = new DefaultPreconditioner();
     m_lf.push_back(mass_spring);
+    m_lf.push_back(gravity);
 }
 
-void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
+void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
 {
     if(nodeUpdated)
     {
@@ -34,8 +34,12 @@ void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
     m_preconditioner->reinitialize(nodeUpdated);
 }
 
+void btDeformableBackwardEulerObjective::setDt(btScalar dt)
+{
+    m_dt = dt;
+}
 
-void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
+void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
 {
     for (int i = 0; i < b.size(); ++i)
         b[i].setZero();
@@ -56,18 +60,11 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
     for (int i = 0; i < m_lf.size(); ++i)
     {
         // add damping matrix
-        m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+        m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
     }
 }
 
-void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
-{
-    m_dt = dt;
-    btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * computeNorm(residual);
-    cg.solve(*this, dv, residual, tolerance);
-}
-
-void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
+void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
     // only the velocity of the constrained nodes needs to be updated during CG solve
     for (auto it : projection.m_constraints)
@@ -77,7 +74,7 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
     }
 }
 
-void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
+void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
 {
     size_t counter = 0;
     for (int i = 0; i < m_softBodies.size(); ++i)
@@ -96,7 +93,7 @@ void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero)
     }
 }
 
-void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
+void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
 {
     // add implicit force
     for (int i = 0; i < m_lf.size(); ++i)
@@ -105,7 +102,7 @@ void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) c
     }
 }
 
-btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const
+btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
 {
     btScalar norm_squared = 0;
     for (int i = 0; i < residual.size(); ++i)
@@ -115,14 +112,14 @@ btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const
     return std::sqrt(norm_squared+SIMD_EPSILON);
 }
 
-void btBackwardEulerObjective::applyExplicitForce(TVStack& force)
+void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 {
     for (int i = 0; i < m_lf.size(); ++i)
         m_lf[i]->addScaledExplicitForce(m_dt, force);
     applyForce(force, true);
 }
 
-void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
+void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual)
 {
     size_t counter = 0;
     for (int i = 0; i < m_softBodies.size(); ++i)
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
new file mode 100644
index 000000000..bfd4ca3af
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -0,0 +1,100 @@
+//
+//  btDeformableBackwardEulerObjective.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/1/19.
+//
+
+#ifndef BT_BACKWARD_EULER_OBJECTIVE_H
+#define BT_BACKWARD_EULER_OBJECTIVE_H
+#include <functional>
+#include "btConjugateGradient.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
+#include "btDeformableGravityForce.h"
+#include "btDeformableContactProjection.h"
+#include "btPreconditioner.h"
+#include "btDeformableRigidDynamicsWorld.h"
+
+class btDeformableRigidDynamicsWorld;
+class btDeformableBackwardEulerObjective
+{
+public:
+    using TVStack = btAlignedObjectArray<btVector3>;
+    btScalar m_dt;
+    btDeformableRigidDynamicsWorld* m_world;
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
+    btAlignedObjectArray<btSoftBody *>& m_softBodies;
+    Preconditioner* m_preconditioner;
+    btDeformableContactProjection projection;
+    const TVStack& m_backupVelocity;
+    
+    btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
+    
+    virtual ~btDeformableBackwardEulerObjective() {}
+    
+    void initialize(){}
+    
+    // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual
+    void computeResidual(btScalar dt, TVStack& residual) const;
+    
+    // add explicit force to the velocity
+    void applyExplicitForce(TVStack& force);
+    
+    // apply force to velocity and optionally reset the force to zero
+    void applyForce(TVStack& force, bool setZero);
+    
+    // compute the norm of the residual
+    btScalar computeNorm(const TVStack& residual) const;
+    
+    // compute one step of the solve (there is only one solve if the system is linear)
+    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
+    
+    // perform A*x = b
+    void multiply(const TVStack& x, TVStack& b) const;
+    
+    // set initial guess for CG solve
+    void initialGuess(TVStack& dv, const TVStack& residual);
+    
+    // reset data structure
+    void reinitialize(bool nodeUpdated);
+    
+    void setDt(btScalar dt);
+    
+    // enforce constraints in CG solve
+    void enforceConstraint(TVStack& x)
+    {
+        projection.enforceConstraint(x);
+        updateVelocity(x);
+    }
+    
+    // add dv to velocity
+    void updateVelocity(const TVStack& dv);
+    
+    //set constraints as projections
+    void setConstraints()
+    {
+        projection.setConstraints();
+    }
+    
+    // update the projections and project the residual
+    void project(TVStack& r)
+    {
+        projection.update();
+        projection.project(r);
+    }
+    
+    // perform precondition M^(-1) x = b
+    void precondition(const TVStack& x, TVStack& b)
+    {
+        m_preconditioner->operator()(x,b);
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+        projection.setWorld(world);
+    }
+};
+
+#endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index f98dfacbb..96d2e0029 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -10,11 +10,9 @@
 
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
-, m_solveIterations(1)
-, m_impulseIterations(1)
-, m_world(nullptr)
+, cg(10)
 {
-    m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+    m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
@@ -22,124 +20,31 @@ btDeformableBodySolver::~btDeformableBodySolver()
     delete m_objective;
 }
 
-void btDeformableBodySolver::postStabilize()
-{
-    for (int i = 0; i < m_softBodySet.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodySet[i];
-        btMultiBodyJacobianData jacobianData;
-        const btScalar mrg = psb->getCollisionShape()->getMargin();
-        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
-        {
-            const btSoftBody::RContact& c = psb->m_rcontacts[j];
-            // skip anchor points
-            if (c.m_node->m_im == 0)
-                continue;
-            
-            const btSoftBody::sCti& cti = c.m_cti;
-            if (cti.m_colObj->hasContactResponse())
-            {
-                btVector3 va(0, 0, 0);
-                btRigidBody* rigidCol = 0;
-                btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                btScalar* deltaV;
-                
-                // grab the velocity of the rigid body
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                {
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
-                }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                {
-                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                    if (multibodyLinkCol)
-                    {
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        jacobianData.m_jacobians.resize(ndof);
-                        jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
-                        btScalar* jac = &jacobianData.m_jacobians[0];
-                        
-                        multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
-                        deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0];
-                        multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v);
-                        
-                        btScalar vel = 0.0;
-                        for (int j = 0; j < ndof; ++j)
-                        {
-                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
-                        }
-                        va = cti.m_normal * vel * m_dt;
-                    }
-                }
-                
-                const btVector3 vb = c.m_node->m_v * m_dt;
-                const btVector3 vr = vb - va;
-                const btScalar dn = btDot(vr, cti.m_normal);
-                
-                btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
-//                dp += mrg;
-                // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
-                
-                btScalar dvn = dn * c.m_c4;
-                const btVector3 impulse = c.m_c0 * ((cti.m_normal * (dn * c.m_c4)));
-                // TODO: only contact is considered here, add friction later
-                if (dp < 0)
-                {
-                    bool two_way = false;
-                    if (two_way)
-                    {
-                        c.m_node->m_x -= impulse * c.m_c2;
-                        
-                        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 = 0.5;
-                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier);
-                            }
-                        }
-                    }
-                    else
-                        c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
-                }
-            }
-        }
-    }
-}
-
 void btDeformableBodySolver::solveConstraints(float solverdt)
 {
-    m_dt = solverdt;
-    bool nodeUpdated = updateNodes();
-    reinitialize(nodeUpdated);
-    
-    // apply explicit force
-    m_objective->applyExplicitForce(m_residual);
+    m_objective->setDt(solverdt);
     
     // add constraints to the solver
     setConstraints();
     
+    // save v_{n+1}^* velocity after explicit forces
     backupVelocity();
-    
-    for (int i = 0; i < m_solveIterations; ++i)
-    {
-        m_objective->computeResidual(solverdt, m_residual);
-        m_objective->initialGuess(m_dv, m_residual);
-        m_objective->computeStep(m_dv, m_residual, solverdt);
-        updateVelocity();
-    }
-    advect(solverdt);
-    postStabilize();
+    m_objective->computeResidual(solverdt, m_residual);
+//   m_objective->initialGuess(m_dv, m_residual);
+    computeStep(m_dv, m_residual);
+    updateVelocity();
 }
 
-void btDeformableBodySolver::reinitialize(bool nodeUpdated)
+void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
 {
+    btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * m_objective->computeNorm(residual);
+    cg.solve(*m_objective, dv, residual, tolerance);
+}
+
+void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies)
+{
+    m_softBodySet.copyFromArray(softBodies);
+    bool nodeUpdated = updateNodes();
     if (nodeUpdated)
     {
         m_dv.resize(m_numNodes);
@@ -161,7 +66,6 @@ void btDeformableBodySolver::setConstraints()
 
 void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
 {
-    m_world = world;
     m_objective->setWorld(world);
 }
 
@@ -180,20 +84,6 @@ void btDeformableBodySolver::updateVelocity()
     }
 }
 
-
-void btDeformableBodySolver::advect(btScalar dt)
-{
-    for (int i = 0; i < m_softBodySet.size(); ++i)
-    {
-        btSoftBody* psb = m_softBodySet[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            auto& node = psb->m_nodes[j];
-            node.m_x  =  node.m_q + dt * node.m_v;
-        }
-    }
-}
-
 void btDeformableBodySolver::backupVelocity()
 {
     // serial implementation
@@ -231,11 +121,53 @@ void btDeformableBodySolver::predictMotion(float solverdt)
         
         if (psb->isActive())
         {
-            psb->predictMotion(solverdt);
+            // apply explicit forces to velocity
+            m_objective->applyExplicitForce(m_residual);
+            // predict motion for collision detection
+            predictDeformableMotion(psb, solverdt);
         }
     }
 }
 
+void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt)
+{
+    int i, ni;
+    
+    /* Prepare                */
+    psb->m_sst.sdt = dt * psb->m_cfg.timescale;
+    psb->m_sst.isdt = 1 / psb->m_sst.sdt;
+    psb->m_sst.velmrg = psb->m_sst.sdt * 3;
+    psb->m_sst.radmrg = psb->getCollisionShape()->getMargin();
+    psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25;
+    /* Integrate            */
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        n.m_q = n.m_x;
+        n.m_x += n.m_v * dt;
+    }
+    /* Bounds                */
+    psb->updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
+    {
+        btSoftBody::Node& n = psb->m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg);
+        psb->m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * psb->m_sst.velmrg,
+                       psb->m_sst.updmrg);
+    }
+
+    /* Clear contacts        */
+    psb->m_rcontacts.resize(0);
+    psb->m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    psb->m_ndbvt.optimizeIncremental(1);
+}
+
 void btDeformableBodySolver::updateSoftBodies()
 {
     for (int i = 0; i < m_softBodySet.size(); i++)
@@ -243,7 +175,7 @@ void btDeformableBodySolver::updateSoftBodies()
         btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
         if (psb->isActive())
         {
-            psb->integrateMotion(); // normal is updated here
+            psb->updateNormals(); // normal is updated here
         }
     }
 }
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index a1cd45077..570cce90f 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -10,31 +10,27 @@
 
 #include <iostream>
 #include "btSoftBodySolvers.h"
-#include "btBackwardEulerObjective.h"
+#include "btDeformableBackwardEulerObjective.h"
 #include "btDeformableRigidDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 struct btCollisionObjectWrapper;
-class btBackwardEulerObjective;
+class btDeformableBackwardEulerObjective;
 class btDeformableRigidDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
     using TVStack = btAlignedObjectArray<btVector3>;
 protected:
-    /** Variable to define whether we need to update solver constants on the next iteration */
-    bool m_updateSolverConstants;
     int m_numNodes;
     TVStack m_dv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
-    btBackwardEulerObjective* m_objective;
-    int m_solveIterations;
-    int m_impulseIterations;
-    btDeformableRigidDynamicsWorld* m_world;
+    btDeformableBackwardEulerObjective* m_objective;
     btAlignedObjectArray<btVector3> m_backupVelocity;
     btScalar m_dt;
+    btConjugateGradient<btDeformableBackwardEulerObjective> cg;
     
 public:
     btDeformableBodySolver();
@@ -45,30 +41,20 @@ public:
     {
         return DEFORMABLE_SOLVER;
     }
-    
-    virtual bool checkInitialized()
-    {
-        return true;
-    }
 
     virtual void updateSoftBodies();
-    
-    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false)
-    {
-        m_softBodySet.copyFromArray(softBodies);
-    }
 
     virtual void copyBackToSoftBodies(bool bMove = true) {}
 
-    virtual void solveConstraints(float solverdt);
+    void extracted(float solverdt);
     
-    void postStabilize();
+    virtual void solveConstraints(float solverdt);
     
-    void reinitialize(bool nodeUpdated);
+    void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies);
     
     void setConstraints();
     
-    void advect(btScalar dt);
+    void predictDeformableMotion(btSoftBody* psb, btScalar dt);
     
     void backupVelocity();
     
@@ -76,6 +62,8 @@ public:
     
     bool updateNodes();
     
+    void computeStep(TVStack& dv, const TVStack& residual);
+                     
     virtual void predictMotion(float solverdt);
 
     virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
@@ -88,7 +76,8 @@ public:
     virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
         softBody->defaultCollisionHandler(otherSoftBody);
     }
-    
+    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+    virtual bool checkInitialized(){return true;}
     virtual void setWorld(btDeformableRigidDynamicsWorld* world);
 };
 
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
new file mode 100644
index 000000000..e81cb2c50
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -0,0 +1,488 @@
+//
+//  btDeformableContactProjection.cpp
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/4/19.
+//
+
+#include "btDeformableContactProjection.h"
+#include "btDeformableRigidDynamicsWorld.h"
+#include <algorithm>
+static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
+                         btMultiBodyJacobianData& jacobianData,
+                         const btVector3& contact_point,
+                         const btVector3& dir)
+{
+    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+    jacobianData.m_jacobians.resize(ndof);
+    jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof);
+    btScalar* jac = &jacobianData.m_jacobians[0];
+    
+    multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m);
+    multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v);
+}
+
+static btVector3 generateUnitOrthogonalVector(const btVector3& u)
+{
+    btScalar ux = u.getX();
+    btScalar uy = u.getY();
+    btScalar uz = u.getZ();
+    btScalar ax = std::abs(ux);
+    btScalar ay = std::abs(uy);
+    btScalar az = std::abs(uz);
+    btVector3 v;
+    if (ax <= ay && ax <= az)
+        v = btVector3(0, -uz, uy);
+    else if (ay <= ax && ay <= az)
+        v = btVector3(-uz, 0, ux);
+    else
+        v = btVector3(-uy, ux, 0);
+    v.normalize();
+    return v;
+}
+
+void btDeformableContactProjection::update()
+{
+    ///solve rigid body constraints
+    m_world->getSolverInfo().m_numIterations = 10;
+    m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo());
+
+    // loop through constraints to set constrained values
+    for (auto& it : m_constraints)
+    {
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        for (int i = 0; i < constraints.size(); ++i)
+        {
+            DeformableContactConstraint& constraint = constraints[i];
+            DeformableFrictionConstraint& friction = frictions[i];
+            for (int j = 0; j < constraint.m_contact.size(); ++j)
+            {
+                if (constraint.m_contact[j] == nullptr)
+                {
+                    // nothing needs to be done for dirichelet constraints
+                    continue;
+                }
+                const btSoftBody::RContact* c = constraint.m_contact[j];
+                const btSoftBody::sCti& cti = c->m_cti;
+                
+                // normal jacobian is precompute but tangent jacobian is not
+                const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j];
+                const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j];
+                
+                if (cti.m_colObj->hasContactResponse())
+                {
+                    btVector3 va(0, 0, 0);
+                    btRigidBody* rigidCol = 0;
+                    btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                    const btScalar* deltaV_normal;
+                    
+                    // grab the velocity of the rigid body
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    {
+                        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                        va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                            const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0];
+                            deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                            
+                            // add in the normal component of the va
+                            btScalar vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k];
+                            }
+                            va = cti.m_normal * vel * m_dt;
+                            
+                            // add in complementary direction of va
+                            const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0];
+                            vel = 0.0;
+                            for (int k = 0; k < ndof; ++k)
+                            {
+                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k];
+                            }
+                            va += friction.m_complementaryDirection[j] * vel * m_dt;
+                        }
+                    }
+                    
+                    const btVector3 vb = c->m_node->m_v * m_dt;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, cti.m_normal);
+                    btVector3 impulse = c->m_c0 * vr;
+                    const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
+                    const btVector3 impulse_tangent = impulse - impulse_normal;
+                    
+                    // start friction handling
+                    // copy old data
+                    friction.m_impulse_prev[j] = friction.m_impulse[j];
+                    friction.m_dv_prev[j] = friction.m_dv[j];
+                    friction.m_static_prev[j] = friction.m_static[j];
+                    
+                    // get the current tangent direction
+                    btScalar local_tangent_norm = impulse_tangent.norm();
+                    btVector3 local_tangent_dir = btVector3(0,0,0);
+                    if (local_tangent_norm > SIMD_EPSILON)
+                        local_tangent_dir = impulse_tangent.normalized();
+
+                    // accumulated impulse on the rb in this and all prev cg iterations
+                    constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal);
+                    const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j];
+                    
+                    // the total tangential impulse required to stop sliding
+                    btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent;
+                    btScalar tangent_norm = tangent.norm();
+  
+                    if (accumulated_normal < 0)
+                    {
+                        friction.m_direction[j] = -local_tangent_dir;
+                        // do not allow switching from static friction to dynamic friction
+                        // it causes cg to explode
+                        if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false)
+                        {
+                            friction.m_static[j] = false;
+                            friction.m_impulse[j] = -accumulated_normal*c->m_c3;
+                        }
+                        else
+                        {
+                            friction.m_static[j] = true;
+                            friction.m_impulse[j] = tangent_norm;
+                        }
+                    }
+                    else
+                    {
+                        friction.m_released[j] = true;
+                        friction.m_static[j] = false;
+                        friction.m_impulse[j] = 0;
+                        friction.m_direction[j] = btVector3(0,0,0);
+                    }
+                    friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt;
+                    friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j];
+                    
+                    // the incremental impulse applied to rb in the tangential direction
+                    btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]);
+                    
+                    // TODO cleanup
+                    if (1) // in the same CG solve, the set of constraits doesn't change
+                    {
+                        // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
+                        
+                        // dv = new_impulse + accumulated velocity change in previous CG iterations
+                        // so we have the invariant node->m_v = backupVelocity + dv;
+
+                        btScalar dvn = -accumulated_normal * c->m_c2/m_dt;
+                        
+                        // the following is equivalent
+                        /*
+                            btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
+                            btScalar dvn = dv.dot(cti.m_normal);
+                         */
+                        
+                        constraint.m_value[j] = dvn;
+                        
+                        // the incremental impulse:
+                        // in the normal direction it's the normal component of "impulse"
+                        // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration
+                        impulse = impulse_normal + incremental_tangent;
+                        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                        {
+                            if (rigidCol)
+                                rigidCol->applyImpulse(impulse, c->m_c1);
+                        }
+                        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                        {
+                            
+                            if (multibodyLinkCol)
+                            {
+                                double multiplier = 1;
+                                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier);
+                                
+                                if (incremental_tangent.norm() > SIMD_EPSILON)
+                                {
+                                    btMultiBodyJacobianData jacobian_tangent;
+                                    btVector3 tangent = incremental_tangent.normalized();
+                                    findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent);
+                                    const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0];
+                                    multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::setConstraints()
+{
+    // set Dirichlet constraint
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            if (psb->m_nodes[j].m_im == 0)
+            {
+                btAlignedObjectArray<DeformableContactConstraint> c;
+                c.push_back(DeformableContactConstraint(btVector3(1,0,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,1,0)));
+                c.push_back(DeformableContactConstraint(btVector3(0,0,1)));
+                m_constraints[&(psb->m_nodes[j])] = c;
+                
+                btAlignedObjectArray<DeformableFrictionConstraint> f;
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
+                f.push_back(DeformableFrictionConstraint());
+                m_frictions[&(psb->m_nodes[j])] = f;
+            }
+        }
+    }
+
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_complementary;
+        
+        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
+        {
+            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            // skip anchor points
+            if (c.m_node->m_im == 0)
+            {
+                continue;
+            }
+            
+            const btSoftBody::sCti& cti = c.m_cti;
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btVector3 va(0, 0, 0);
+                btRigidBody* rigidCol = 0;
+                btMultiBodyLinkCollider* multibodyLinkCol = 0;
+                
+                // grab the velocity of the rigid body
+                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                {
+                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
+                }
+                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                {
+                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                    if (multibodyLinkCol)
+                    {
+                        findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal);
+                        btScalar vel = 0.0;
+                        const btScalar* jac = &jacobianData_normal.m_jacobians[0];
+                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                        for (int j = 0; j < ndof; ++j)
+                        {
+                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
+                            std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl;
+                            std::cout << jac[j] << std::endl;
+                        }
+                        va = cti.m_normal * vel * m_dt;
+                    }
+                }
+                
+                const btVector3 vb = c.m_node->m_v * m_dt;
+                const btVector3 vr = vb - va;
+                const btScalar dn = btDot(vr, cti.m_normal);
+                if (dn < SIMD_EPSILON)
+                {
+                    // find complementary jacobian
+                    btVector3 complementaryDirection;
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            complementaryDirection = generateUnitOrthogonalVector(cti.m_normal);
+                            findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection);
+                        }
+                    }
+                    
+                    if (m_constraints.find(c.m_node) == m_constraints.end())
+                    {
+                        btAlignedObjectArray<DeformableContactConstraint> constraints;
+                        constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
+                        m_constraints[c.m_node] = constraints;
+                        btAlignedObjectArray<DeformableFrictionConstraint> frictions;
+                        frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
+                        m_frictions[c.m_node] = frictions;
+                    }
+                    else
+                    {
+                        // group colinear constraints into one
+                        const btScalar angle_epsilon = 0.015192247; // less than 10 degree
+                        bool merged = false;
+                        btAlignedObjectArray<DeformableContactConstraint>& constraints = m_constraints[c.m_node];
+                        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[c.m_node];
+                        for (int j = 0; j < constraints.size(); ++j)
+                        {
+                            const btAlignedObjectArray<btVector3>& dirs = constraints[j].m_direction;
+                            btScalar dot_prod = dirs[0].dot(cti.m_normal);
+                            if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon)
+                            {
+                                // group the constraints
+                                constraints[j].append(c, jacobianData_normal);
+                                // push in an empty friction
+                                frictions[j].append();
+                                frictions[j].addJacobian(complementaryDirection, jacobianData_complementary);
+                                merged = true;
+                                break;
+                            }
+                        }
+                        const int dim = 3;
+                        // hard coded no more than 3 constraint directions
+                        if (!merged && constraints.size() < dim)
+                        {
+                            constraints.push_back(DeformableContactConstraint(c, jacobianData_normal));
+                            frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary));
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::enforceConstraint(TVStack& x)
+{
+    const int dim = 3;
+    for (auto& it : m_constraints)
+    {
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        size_t i = m_indices[it.first];
+        const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        btAssert(constraints.size() <= dim);
+        btAssert(constraints.size() > 0);
+        if (constraints.size() == 1)
+        {
+            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+            for (int j = 0; j < constraints[0].m_direction.size(); ++j)
+                x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j];
+        }
+        else if (constraints.size() == 2)
+        {
+            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+            btAssert(free_dir.norm() > SIMD_EPSILON)
+            free_dir.normalize();
+            x[i] = x[i].dot(free_dir) * free_dir;
+            for (int j = 0; j < constraints.size(); ++j)
+            {
+                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                {
+                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                }
+            }
+            
+        }
+        else
+        {
+            x[i].setZero();
+            for (int j = 0; j < constraints.size(); ++j)
+            {
+                for (int k = 0; k < constraints[j].m_direction.size(); ++k)
+                {
+                    x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k];
+                }
+            }
+        }
+        
+        // apply friction if the node is not constrained in all directions
+        if (constraints.size() < 3)
+        {
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                const DeformableFrictionConstraint& friction= frictions[f];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
+                {
+                    // clear the old constraint
+                    if (friction.m_static_prev[j] == true)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j];
+                    }
+                    // add the new constraint
+                    if (friction.m_static[j] == true)
+                    {
+                        x[i] += friction.m_direction[j] * friction.m_dv[j];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::project(TVStack& x)
+{
+    const int dim = 3;
+    for (auto& it : m_constraints)
+    {
+        const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
+        size_t i = m_indices[it.first];
+        btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
+        btAssert(constraints.size() <= dim);
+        btAssert(constraints.size() > 0);
+        if (constraints.size() == 1)
+        {
+            x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0];
+        }
+        else if (constraints.size() == 2)
+        {
+            btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]);
+            btAssert(free_dir.norm() > SIMD_EPSILON)
+            free_dir.normalize();
+            x[i] = x[i].dot(free_dir) * free_dir;
+        }
+        else
+            x[i].setZero();
+        
+        // apply friction if the node is not constrained in all directions
+        if (constraints.size() < 3)
+        {
+            bool has_static_constraint = false;
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                DeformableFrictionConstraint& friction= frictions[f];
+                for (int j = 0; j < friction.m_static.size(); ++j)
+                    has_static_constraint = has_static_constraint || friction.m_static[j];
+            }
+            
+            for (int f = 0; f < frictions.size(); ++f)
+            {
+                DeformableFrictionConstraint& friction= frictions[f];
+                for (int j = 0; j < friction.m_direction.size(); ++j)
+                {
+                    // clear the old friction force
+                    if (friction.m_static_prev[j] == false)
+                    {
+                        x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j];
+                    }
+                    
+                    // only add to the rhs if there is no static friction constraint on the node
+                    if (friction.m_static[j] == false && !has_static_constraint)
+                    {
+                        x[i] += friction.m_direction[j] * friction.m_impulse[j];
+                    }
+                }
+            }
+        }
+    }
+}
+
+void btDeformableContactProjection::reinitialize(bool nodeUpdated)
+{
+    btCGProjection::reinitialize(nodeUpdated);
+    m_constraints.clear();
+    m_frictions.clear();
+}
+
+
+
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index 725cc106b..aa7af70fb 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -1,5 +1,5 @@
 //
-//  btContactProjection.h
+//  btDeformableContactProjection.h
 //  BulletSoftBody
 //
 //  Created by Xuchen Han on 7/4/19.
@@ -12,27 +12,32 @@
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <iostream>
-class btContactProjection : public btCGProjection
+class btDeformableContactProjection : public btCGProjection
 {
 public:
-    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableContactConstraint> > m_constraints;
+    std::unordered_map<btSoftBody::Node *, btAlignedObjectArray<DeformableFrictionConstraint> > m_frictions;
+    
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : btCGProjection(softBodies, dt)
     {
     }
     
-    virtual ~btContactProjection()
+    virtual ~btDeformableContactProjection()
     {
     }
     
     // apply the constraints to the rhs
-    virtual void operator()(TVStack& x);
+    virtual void project(TVStack& x);
     
     // apply constraints to x in Ax=b
     virtual void enforceConstraint(TVStack& x);
     
     // update the constraints
-    virtual void update(const TVStack& dv, const TVStack& backupVelocity);
+    virtual void update();
     
     virtual void setConstraints();
+    
+    virtual void reinitialize(bool nodeUpdated);
 };
 #endif /* btContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
new file mode 100644
index 000000000..545615a47
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -0,0 +1,60 @@
+//
+//  btDeformableGravityForce.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/21/19.
+//
+
+#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H
+#define BT_DEFORMABLE_GRAVITY_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableGravityForce : public btDeformableLagrangianForce
+{
+public:
+    using TVStack = btDeformableLagrangianForce::TVStack;
+    btVector3 m_gravity;
+    
+    btDeformableGravityForce(const btAlignedObjectArray<btSoftBody *>& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g)
+    {
+        
+    }
+    
+    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledGravityForce(scale, force);
+    }
+    
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        
+    }
+    
+    virtual void addScaledGravityForce(btScalar scale, TVStack& force)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes == force.size())
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                btSoftBody::Node& n = psb->m_nodes[j];
+                size_t id = m_indices[&n];
+                btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
+                btVector3 scaled_force = scale * m_gravity * mass;
+                force[id] += scaled_force;
+            }
+        }
+    }
+    
+
+    
+    
+};
+#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */
diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
index b95a30ce9..acb9a28ff 100644
--- a/src/BulletSoftBody/btLagrangianForce.h
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -1,35 +1,33 @@
 //
-//  btLagrangianForce.h
+//  btDeformableLagrangianForce.h
 //  BulletSoftBody
 //
 //  Created by Xuchen Han on 7/1/19.
 //
 
-#ifndef BT_LAGRANGIAN_FORCE_H
-#define BT_LAGRANGIAN_FORCE_H
+#ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H
+#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H
 
 #include "btSoftBody.h"
 #include <unordered_map>
 
-class btLagrangianForce
+class btDeformableLagrangianForce
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
     const btAlignedObjectArray<btSoftBody *>& m_softBodies;
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     
-    btLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
+    btDeformableLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
     : m_softBodies(softBodies)
     {
     }
     
-    virtual ~btLagrangianForce(){}
+    virtual ~btDeformableLagrangianForce(){}
     
     virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
     
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
     
@@ -39,7 +37,7 @@ public:
             updateId();
     }
     
-    void updateId()
+    virtual void updateId()
     {
         size_t index = 0;
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -51,5 +49,15 @@ public:
             }
         }
     }
+    
+    virtual int getNumNodes()
+    {
+        int numNodes = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            numNodes += m_softBodies[i]->m_nodes.size();
+        }
+        return numNodes;
+    }
 };
-#endif /* btLagrangianForce_h */
+#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 78cdcf7e6..31389c20f 100644
--- a/src/BulletSoftBody/btMassSpring.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -1,20 +1,20 @@
 //
-//  btMassSpring.h
+//  btDeformableMassSpringForce.h
 //  BulletSoftBody
 //
-//  Created by Chuyuan Fu on 7/1/19.
+//  Created by Xuchen Gan on 7/1/19.
 //
 
 #ifndef BT_MASS_SPRING_H
 #define BT_MASS_SPRING_H
 
-#include "btLagrangianForce.h"
+#include "btDeformableLagrangianForce.h"
 
-class btMassSpring : public btLagrangianForce
+class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
 public:
-    using TVStack = btLagrangianForce::TVStack;
-    btMassSpring(const btAlignedObjectArray<btSoftBody *>& softBodies) : btLagrangianForce(softBodies)
+    using TVStack = btDeformableLagrangianForce::TVStack;
+    btDeformableMassSpringForce(const btAlignedObjectArray<btSoftBody *>& softBodies) : btDeformableLagrangianForce(softBodies)
     {
         
     }
@@ -72,10 +72,6 @@ public:
                 size_t id2 = m_indices[node2];
                 
                 // elastic force
-                
-                // fully implicit
-//                btVector3 dir = (node2->m_x - node1->m_x);
-                
                 // explicit elastic force
                 btVector3 dir = (node2->m_q - node1->m_q);
                 btVector3 dir_normalized = dir.normalized();
@@ -86,33 +82,7 @@ public:
         }
     }
     
-    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
-    {
-        int numNodes = getNumNodes();
-        btAssert(numNodes == dx.size());
-        btAssert(numNodes == df.size());
-        
-        // implicit elastic force differential
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            const btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_links.size(); ++j)
-            {
-                const auto& link = psb->m_links[j];
-                const auto node1 = link.m_n[0];
-                const auto node2 = link.m_n[1];
-                btScalar kLST = link.Feature::m_material->m_kLST;
-                size_t id1 = m_indices[node1];
-                size_t id2 = m_indices[node2];
-                btVector3 local_scaled_df = scale * kLST * (dx[id2] - dx[id1]);
-                df[id1] += local_scaled_df;
-                df[id2] -= local_scaled_df;
-            }
-        }
-    }
-    
-    
-    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
         // implicit damping force differential
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -132,16 +102,6 @@ public:
             }
         }
     }
-    
-    int getNumNodes()
-    {
-        int numNodes = 0;
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            numNodes += m_softBodies[i]->m_nodes.size();
-        }
-        return numNodes;
-    }
 };
 
 #endif /* btMassSpring_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 173f71206..3af3c8499 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -12,87 +12,78 @@
 
 void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
-    m_internalTime += timeStep;
-    // Let the solver grab the soft bodies and if necessary optimize for it
-    m_deformableBodySolver->optimize(m_softBodies);
+    reinitialize(timeStep);
     
-    if (!m_deformableBodySolver->checkInitialized())
-    {
-        btAssert("Solver initialization failed\n");
-    }
-
-    // from btDiscreteDynamicsWorld singleStepSimulation
-    if (0 != m_internalPreTickCallback)
-    {
-        (*m_internalPreTickCallback)(this, timeStep);
-    }
+    // add gravity to velocity of rigid and multi bodys
+    applyRigidBodyGravity(timeStep);
     
-    ///apply gravity, predict motion
+    ///apply gravity and explicit force to velocity, predict motion
     predictUnconstraintMotion(timeStep);
     
-    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
-    
-    dispatchInfo.m_timeStep = timeStep;
-    dispatchInfo.m_stepCount = 0;
-    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
-    
-    // only used in CCD
-//    createPredictiveContacts(timeStep);
-    
     ///perform collision detection
     btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
     
     btMultiBodyDynamicsWorld::calculateSimulationIslands();
     
-    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
-
-    if (0 != m_internalTickCallback)
-    {
-        (*m_internalTickCallback)(this, timeStep);
-    }
-    
-    // TODO: This is an ugly hack to get the desired gravity behavior.
-    // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
-    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
-    // when there are multiple substeps
-    clearForces();
-    clearMultiBodyForces();
-    btMultiBodyDynamicsWorld::applyGravity();
-    // integrate rigid body gravity
-    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
-    {
-        btRigidBody* rb = m_nonStaticRigidBodies[i];
-        rb->integrateVelocities(timeStep);
-    }
-    // integrate multibody gravity
-    btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
-    clearForces();
-    clearMultiBodyForces();
-    
+    beforeSolverCallbacks(timeStep);
     
-    for (int i = 0; i < before_solver_callbacks.size(); ++i)
-        before_solver_callbacks[i](m_internalTime, this);
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
-
     
+    positionCorrection();
     
-    //integrate transforms
-    btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
+    integrateTransforms(timeStep);
     
     ///update vehicle simulation
     btMultiBodyDynamicsWorld::updateActions(timeStep);
     
     btMultiBodyDynamicsWorld::updateActivationState(timeStep);
-    
-    ///update soft bodies
-    m_deformableBodySolver->updateSoftBodies();
-
-    clearForces();
     // End solver-wise simulation step
     // ///////////////////////////////
 }
 
+void btDeformableRigidDynamicsWorld::positionCorrection()
+{
+    // perform position correction for all geometric collisions
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        const btScalar mrg = psb->getCollisionShape()->getMargin();
+        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
+        {
+            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            // skip anchor points
+            if (c.m_node->m_im == 0)
+                continue;
+            
+            const btSoftBody::sCti& cti = c.m_cti;
+            if (cti.m_colObj->hasContactResponse())
+            {
+                btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
+                if (dp < 0)
+                {
+                    // m_c4 is the collision hardness
+                    c.m_node->m_q -= dp * cti.m_normal * c.m_c4;
+                }
+            }
+        }
+    }
+}
+
+void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt)
+{
+    btMultiBodyDynamicsWorld::integrateTransforms(dt);
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            auto& node = psb->m_nodes[j];
+            node.m_x  =  node.m_q + dt * node.m_v;
+        }
+    }
+}
+
 void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep)
 {
     m_deformableBodySolver->solveConstraints(timeStep);
@@ -117,4 +108,45 @@ void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep
     m_deformableBodySolver->predictMotion(float(timeStep));
 }
 
+void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep)
+{
+    m_internalTime += timeStep;
+    m_deformableBodySolver->reinitialize(m_softBodies);
+    btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
+    dispatchInfo.m_timeStep = timeStep;
+    dispatchInfo.m_stepCount = 0;
+    dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
+    btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+}
 
+void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
+{
+    // TODO: This is an ugly hack to get the desired gravity behavior.
+    // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
+    // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
+    // when there are multiple substeps
+    clearForces();
+    clearMultiBodyForces();
+    btMultiBodyDynamicsWorld::applyGravity();
+    // integrate rigid body gravity
+    for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
+    {
+        btRigidBody* rb = m_nonStaticRigidBodies[i];
+        rb->integrateVelocities(timeStep);
+    }
+    // integrate multibody gravity
+    btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo());
+    clearForces();
+    clearMultiBodyForces();
+}
+
+void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
+{
+    if (0 != m_internalTickCallback)
+    {
+        (*m_internalTickCallback)(this, timeStep);
+    }
+    
+    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 5bbebeda3..c5cf280ef 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -17,15 +17,15 @@
 #define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
 
 #include "btSoftMultiBodyDynamicsWorld.h"
-#include "btLagrangianForce.h"
-#include "btMassSpring.h"
+#include "btDeformableLagrangianForce.h"
+#include "btDeformableMassSpringForce.h"
 #include "btDeformableBodySolver.h"
 #include "btSoftBodyHelpers.h"
 #include <functional>
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btDeformableBodySolver;
-class btLagrangianForce;
+class btDeformableLagrangianForce;
 typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 
 class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
@@ -45,6 +45,10 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld
 protected:
     virtual void internalSingleStepSimulation(btScalar timeStep);
     
+    virtual void integrateTransforms(btScalar timeStep);
+    
+    void positionCorrection();
+    
     void solveDeformableBodiesConstraints(btScalar timeStep);
     
 public:
@@ -70,7 +74,7 @@ public:
         m_sbi.m_sparsesdf.Initialize();
         m_internalTime = 0.0;
     }
-    btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > before_solver_callbacks;
+    btAlignedObjectArray<std::function<void(btScalar, btDeformableRigidDynamicsWorld*)> > m_beforeSolverCallbacks;
     virtual ~btDeformableRigidDynamicsWorld()
     {
     }
@@ -108,10 +112,18 @@ public:
     {
         return m_sbi;
     }
+    
     const btSoftBodyWorldInfo& getWorldInfo() const
     {
         return m_sbi;
     }
+    
+    void reinitialize(btScalar timeStep);
+    
+    void applyRigidBodyGravity(btScalar timeStep);
+    
+    void beforeSolverCallbacks(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 974246448..876b6b6e4 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt)
 	m_sst.radmrg = getCollisionShape()->getMargin();
 	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
 	/* Forces				*/
-    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+//    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
 	applyForces();
 	/* Integrate			*/
 	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
@@ -1806,7 +1806,7 @@ void btSoftBody::predictMotion(btScalar dt)
 			}
 		}
         n.m_v += deltaV;
-		n.m_x += n.m_v * m_sst.sdt;
+        n.m_x += n.m_v * m_sst.sdt;
 		n.m_f = btVector3(0, 0, 0);
 	}
 	/* Clusters				*/
@@ -2270,10 +2270,11 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-	//	const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
-	//const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
-	const btTransform& wtr = colObjWrap->getWorldTransform();
-	//todo: check which transform is needed here
+        const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+    
+    // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
+    const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
+    // TODO: get the correct transform for multibody
 
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(