bug fixes in constraints projections; cpplized various functions

12 files changed, 450 insertions, 263 deletions

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
new file mode 100644
index 000000000..22614c994
--- /dev/null
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -0,0 +1,88 @@
+//
+//  btBackwardEulerObjective.cpp
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/9/19.
+//
+
+#include "btBackwardEulerObjective.h"
+
+btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+: cg(20)
+, m_softBodies(softBodies)
+, precondition(DefaultPreconditioner())
+, 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_lf.push_back(mass_spring);
+}
+
+void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
+{
+    if(nodeUpdated)
+    {
+        projection.setSoftBodies(m_softBodies);
+    }
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        m_lf[i]->reinitialize(nodeUpdated);
+        projection.reinitialize(nodeUpdated);
+    }
+}
+
+
+void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
+{
+    for (int i = 0; i < b.size(); ++i)
+        b[i].setZero();
+    
+    // add in the mass term
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            const auto& node = psb->m_nodes[j];
+            b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
+            ++counter;
+        }
+    }
+    
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        // add damping matrix
+        m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+        
+        // add stiffness matrix when fully implicity
+        m_lf[i]->addScaledElasticForceDifferential(-m_dt*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()* 16 * computeNorm(residual);
+    cg.solve(*this, dv, residual, tolerance);
+    }
+
+void btBackwardEulerObjective::updateVelocity(const TVStack& dv)
+{
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        int counter = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                // only the velocity of the constrained nodes needs to be updated during CG solve
+                if (projection.m_constrainedDirections.size() > 0)
+                    psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
+                ++counter;
+            }
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index 5e81fc0f2..424291700 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -35,17 +35,9 @@ public:
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     std::function<void(const TVStack&, TVStack&)> precondition;
     btContactProjection projection;
+    const TVStack& m_backupVelocity;
     
-    btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
-    : cg(20)
-    , m_softBodies(softBodies)
-    , precondition(DefaultPreconditioner())
-    , projection(m_softBodies)
-    {
-        // TODO: this should really be specified in initialization instead of here
-        btMassSpring* mass_spring = new btMassSpring(m_softBodies);
-        m_lf.push_back(mass_spring);
-    }
+    btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
     virtual ~btBackwardEulerObjective() {}
     
@@ -54,7 +46,6 @@ public:
     void computeResidual(btScalar dt, TVStack& residual) const
     {
         // gravity is treated explicitly in predictUnconstraintMotion
-        
         // add force
         for (int i = 0; i < m_lf.size(); ++i)
         {
@@ -72,62 +63,29 @@ public:
         return std::sqrt(norm_squared);
     }
     
-    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
-    {
-        m_dt = dt;
-        btScalar tolerance = std::numeric_limits<float>::epsilon()*16 * computeNorm(residual);
-        cg.solve(*this, dv, residual, tolerance);
-    }
+    void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt);
     
-    void multiply(const TVStack& x, TVStack& b) const
-    {
-        for (int i = 0; i < b.size(); ++i)
-            b[i].setZero();
-        
-        // add in the mass term
-        size_t counter = 0;
-        for (int i = 0; i < m_softBodies.size(); ++i)
-        {
-            btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_nodes.size(); ++j)
-            {
-                const auto& node = psb->m_nodes[j];
-                b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im;
-                ++counter;
-            }
-        }
-        
-        for (int i = 0; i < m_lf.size(); ++i)
-        {
-            // damping force is implicit and elastic force is explicit
-            m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
-//            m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
-        }
-    }
+    void multiply(const TVStack& x, TVStack& b) const;
     
     void updateProjection(const TVStack& dv)
     {
-        projection.update(m_dt, dv);
+        projection.update(dv, m_backupVelocity);
     }
     
-    void reinitialize(bool nodeUpdated)
-    {
-        if(nodeUpdated)
-        {
-            projection.setSoftBodies(m_softBodies);
-        }
-        for (int i = 0; i < m_lf.size(); ++i)
-        {
-            m_lf[i]->reinitialize(nodeUpdated);
-            projection.reinitialize(nodeUpdated);
-        }
-    }
+    void reinitialize(bool nodeUpdated);
     
     void enforceConstraint(TVStack& x)
     {
         projection.enforceConstraint(x);
+        updateVelocity(x);
     }
     
+    void updateVelocity(const TVStack& dv);
+    
+    void setConstraintDirections()
+    {
+        projection.setConstraintDirections();
+    }
     void project(TVStack& r, const TVStack& dv)
     {
         updateProjection(dv);
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 533e19c7a..8648e99f8 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -23,30 +23,39 @@ public:
     std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     TVArrayStack m_constrainedDirections;
     TArrayStack m_constrainedValues;
+    const btScalar& m_dt;
     
-    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
+    , m_dt(dt)
     {
-        
     }
     
     virtual ~btCGProjection()
     {
-        
     }
     
     // apply the constraints
     virtual void operator()(TVStack& x) = 0;
     
+    virtual void setConstraintDirections() = 0;
+    
     // update the constraints
-    virtual void update(btScalar dt, const TVStack& dv) = 0;
+    virtual void update(const TVStack& dv, const TVStack& backup_v) = 0;
     
     virtual void reinitialize(bool nodeUpdated)
     {
         if (nodeUpdated)
             updateId();
+        
+        // resize and clear the old constraints
         m_constrainedValues.resize(m_indices.size());
         m_constrainedDirections.resize(m_indices.size());
+        for (int i = 0; i < m_constrainedDirections.size(); ++i)
+        {
+            m_constrainedDirections[i].clear();
+            m_constrainedValues[i].clear();
+        }
     }
     
     void updateId()
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index 2754a208d..0a88e6b94 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -8,6 +8,10 @@
 #ifndef BT_CONJUGATE_GRADIENT_H
 #define BT_CONJUGATE_GRADIENT_H
 #include <iostream>
+#include <cmath>
+#include <LinearMath/btAlignedObjectArray.h>
+#include <LinearMath/btVector3.h>
+
 template <class TM>
 class btConjugateGradient
 {
@@ -25,69 +29,6 @@ public:
     
     virtual ~btConjugateGradient(){}
     
-//    // return the number of iterations taken
-//    int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
-//    {
-//        btAssert(x.size() == b.size());
-//        reinitialize(b);
-//
-//        // r = M * (b - A * x) --with assigned dof zeroed out
-//        A.multiply(x, temp);
-//        temp = sub(b, temp);
-//        A.project(temp);
-//        A.precondition(temp, r);
-//
-//        btScalar r_dot_r = squaredNorm(r), r_dot_r_new;
-//        btScalar r_norm = std::sqrt(r_dot_r);
-//        if (r_norm < tolerance) {
-//            std::cout << "Iteration = 0" << std::endl;
-//            std::cout << "Two norm of the residual = " << r_norm << std::endl;
-//            return 0;
-//        }
-//
-//        p = r;
-//        // q = M * A * q;
-//        A.multiply(p, temp);
-//        A.precondition(temp, q);
-//
-//        // alpha = |r|^2 / (p^T * A * p)
-//        btScalar alpha = r_dot_r / dot(p, q), beta;
-//
-//        for (int k = 1; k < max_iterations; k++) {
-////            x += alpha * p;
-////            r -= alpha * q;
-//            multAndAddTo(alpha, p, x);
-//            multAndAddTo(-alpha, q, r);
-//
-//            // zero out the dofs of r
-//            A.project(r);
-//
-//            r_dot_r_new = squaredNorm(r);
-//            r_norm = std::sqrt(r_dot_r_new);
-//
-//            if (r_norm < tolerance) {
-//                std::cout << "ConjugateGradient iterations " << k << std::endl;
-//                return k;
-//
-//                beta = r_dot_r_new / r_dot_r;
-//                r_dot_r = r_dot_r_new;
-////                p = r + beta * p;
-//                p = multAndAdd(beta, p, r);
-//
-//                // q = M * A * q;
-//                A.multiply(p, temp);
-//                A.precondition(temp, q);
-//
-//                alpha = r_dot_r / dot(p, q);
-//            }
-//
-//            setZero(q);
-//            setZero(r);
-//        }
-//        std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl;
-//        return max_iterations;
-//    }
-    
     // return the number of iterations taken
     int solve(TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
     {
@@ -109,7 +50,6 @@ public:
         
         // z = M^(-1) * r
         A.precondition(r, z);
-        // p = z;
         p = z;
         // temp = A*p
         A.multiply(p, temp);
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
index 8f9f9225a..4db3e019e 100644
--- a/src/BulletSoftBody/btContactProjection.cpp
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -7,44 +7,16 @@
 
 #include "btContactProjection.h"
 #include "btDeformableRigidDynamicsWorld.h"
-void btContactProjection::update(btScalar dt, const TVStack& dv)
+void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity)
 {
     ///solve rigid body constraints
     m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo());
-    
-    // clear the old constraints
-    for (int i = 0; i < m_constrainedDirections.size(); ++i)
-    {
-        m_constrainedDirections[i].clear();
-        m_constrainedValues[i].clear();
-    }
-    
-    // Set dirichlet constraints
-    size_t counter = 0;
-    for (int i = 0; i < m_softBodies.size(); ++i)
-    {
-        const btSoftBody* psb = m_softBodies[i];
-        for (int j = 0; j < psb->m_nodes.size(); ++j)
-        {
-            if (psb->m_nodes[j].m_im == 0)
-            {
-                m_constrainedDirections[counter].push_back(btVector3(1,0,0));
-                m_constrainedDirections[counter].push_back(btVector3(0,1,0));
-                m_constrainedDirections[counter].push_back(btVector3(0,0,1));
-                m_constrainedValues[counter].push_back(0);
-                m_constrainedValues[counter].push_back(0);
-                m_constrainedValues[counter].push_back(0);
-            }
-            ++counter;
-        }
-    }
-    
+
     // loop through contacts to create contact constraints
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
         btMultiBodyJacobianData jacobianData;
-        const btScalar mrg = psb->getCollisionShape()->getMargin();
         for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
         {
             const btSoftBody::RContact& c = psb->m_rcontacts[i];
@@ -65,7 +37,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
                     rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0);
+                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                 {
@@ -86,29 +58,25 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
                         {
                             vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
                         }
-                        va = cti.m_normal * vel * dt;
+                        va = cti.m_normal * vel * m_dt;
                     }
                 }
                 
-                // TODO: rethink what the velocity of the soft body node should be
-                //                    const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
-                const btVector3 vb = c.m_node->m_v * 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)
+                if (1) // in the same CG solve, the set of constraits doesn't change
+//                if (dn <= SIMD_EPSILON)
                 {
-                    const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg);
-                    const btVector3 fv = vr - (cti.m_normal * dn);
                     // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient
-//                    const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3)));
-                    const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4)));
-                    
+                    const btVector3 impulse = c.m_c0 *(cti.m_normal * dn);
                     // TODO: only contact is considered here, add friction later
-                    btVector3 normal = cti.m_normal.normalized();
-                    btVector3 dv = -impulse * c.m_c2/dt;
-                    btScalar dvn = dv.dot(normal);
-                    m_constrainedDirections[m_indices[c.m_node]].push_back(normal);
-                    m_constrainedValues[m_indices[c.m_node]].push_back(dvn);
+                    
+                    // dv = new_impulse + accumulated velocity change in previous CG iterations
+                    // so we have the invariant node->m_v = backupVelocity + dv;
+                    btVector3 dv = -impulse * c.m_c2/m_dt + c.m_node->m_v - backupVelocity[m_indices[c.m_node]];
+                    btScalar dvn = dv.dot(cti.m_normal);
+                    m_constrainedValues[m_indices[c.m_node]][0]=(dvn);
                     
                     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                     {
@@ -128,3 +96,94 @@ void btContactProjection::update(btScalar dt, const TVStack& dv)
         }
     }
 }
+
+void btContactProjection::setConstraintDirections()
+{
+    // set Dirichlet constraint
+    size_t counter = 0;
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        const btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            if (psb->m_nodes[j].m_im == 0)
+            {
+                m_constrainedDirections[counter].push_back(btVector3(1,0,0));
+                m_constrainedDirections[counter].push_back(btVector3(0,1,0));
+                m_constrainedDirections[counter].push_back(btVector3(0,0,1));
+                m_constrainedValues[counter].push_back(0);
+                m_constrainedValues[counter].push_back(0);
+                m_constrainedValues[counter].push_back(0);
+            }
+            ++counter;
+        }
+    }
+
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        btMultiBodyJacobianData jacobianData;
+        
+        int j = 0;
+        while (j < psb->m_rcontacts.size())
+        {
+            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            // skip anchor points
+            if (c.m_node->m_im == 0)
+            {
+                psb->m_rcontacts.removeAtIndex(j);
+                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);
+                if (dn < SIMD_EPSILON)
+                {
+                    ++j;
+                    m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal);
+                    m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1);
+                    continue;
+                }
+            }
+            psb->m_rcontacts.removeAtIndex(j);
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
index e1c3bc50d..5598ada0a 100644
--- a/src/BulletSoftBody/btContactProjection.h
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -15,8 +15,8 @@
 class btContactProjection : public btCGProjection
 {
 public:
-    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
-    : btCGProjection(softBodies)
+    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    : btCGProjection(softBodies, dt)
     {
         
     }
@@ -51,6 +51,8 @@ public:
     }
     
     // update the constraints
-    virtual void update(btScalar dt, const TVStack& dv);
+    virtual void update(const TVStack& dv, const TVStack& backupVelocity);
+    
+    virtual void setConstraintDirections();
 };
 #endif /* btContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
new file mode 100644
index 000000000..d3d448db0
--- /dev/null
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -0,0 +1,157 @@
+//
+//  btDeformableBodySolver.cpp
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/9/19.
+//
+
+#include <stdio.h>
+#include "btDeformableBodySolver.h"
+
+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);
+                
+                const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), 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)
+                {
+                    c.m_node->m_x -= dp * cti.m_normal * c.m_c4;
+                    
+                    ////
+                    //                        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);
+                //                        }
+                //                    }
+            }
+        }
+    }
+}
+
+void btDeformableBodySolver::solveConstraints(float solverdt)
+{
+    m_dt = solverdt;
+    bool nodeUpdated = updateNodes();
+    reinitialize(nodeUpdated);
+    backupVelocity();
+    postStabilize();
+    for (int i = 0; i < m_solveIterations; ++i)
+    {
+        m_objective->computeResidual(solverdt, m_residual);
+        m_objective->computeStep(m_dv, m_residual, solverdt);
+        updateVelocity();
+    }
+    advect(solverdt);
+}
+
+void btDeformableBodySolver::reinitialize(bool nodeUpdated)
+{
+    if (nodeUpdated)
+    {
+        m_dv.resize(m_numNodes);
+        m_residual.resize(m_numNodes);
+    }
+    
+    for (int i = 0; i < m_dv.size(); ++i)
+    {
+        m_dv[i].setZero();
+        m_residual[i].setZero();
+    }
+    m_objective->reinitialize(nodeUpdated);
+    
+    // remove contact constraints with separating velocity
+    setConstraintDirections();
+}
+
+void btDeformableBodySolver::setConstraintDirections()
+{
+    m_objective->setConstraintDirections();
+}
+
+void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
+{
+    m_world = world;
+    m_objective->setWorld(world);
+}
+
+void btDeformableBodySolver::updateVelocity()
+{
+    // 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_dv[counter];
+            psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter];
+            ++counter;
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 9afec42dc..540a2b6cb 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -16,7 +16,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 struct btCollisionObjectWrapper;
-
+class btBackwardEulerObjective;
 class btDeformableRigidDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
@@ -29,25 +29,17 @@ protected:
     TVStack m_dv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
-    btBackwardEulerObjective m_objective;
+    btBackwardEulerObjective* m_objective;
     int m_solveIterations;
     int m_impulseIterations;
     btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btVector3> m_backupVelocity;
+    btScalar m_dt;
     
 public:
-    btDeformableBodySolver()
-    : m_numNodes(0)
-    , m_objective(m_softBodySet, m_backupVelocity)
-    , m_solveIterations(1)
-    , m_impulseIterations(1)
-    , m_world(nullptr)
-    {
-    }
+    btDeformableBodySolver();
     
-    virtual ~btDeformableBodySolver()
-    {
-    }
+    virtual ~btDeformableBodySolver();
     
     virtual SolverTypes getSolverType() const
     {
@@ -78,21 +70,9 @@ public:
 
     virtual void copyBackToSoftBodies(bool bMove = true) {}
 
-    virtual void solveConstraints(float solverdt)
-    {
-        bool nodeUpdated = updateNodes();
-        reinitialize(nodeUpdated);
-        for (int i = 0; i < m_solveIterations; ++i)
-        {
-            // only need to advect x here if elastic force is implicit
-//            prepareSolve(solverdt);
-            m_objective.computeResidual(solverdt, m_residual);
-            m_objective.computeStep(m_dv, m_residual, solverdt);
-            
-            updateVelocity();
-        }
-        advect(solverdt);
-    }
+    virtual void solveConstraints(float solverdt);
+    
+    void postStabilize();
     
     void moveTempVelocity(btScalar dt, const TVStack& f)
     {
@@ -108,34 +88,10 @@ public:
         }
     }
     
-    void reinitialize(bool nodeUpdated)
-    {
-        if (nodeUpdated)
-        {
-            m_dv.resize(m_numNodes);
-            m_residual.resize(m_numNodes);
-        }
-        
-        for (int i = 0; i < m_dv.size(); ++i)
-        {
-            m_dv[i].setZero();
-            m_residual[i].setZero();
-        }
-        m_objective.reinitialize(nodeUpdated);
-    }
+    void reinitialize(bool nodeUpdated);
+    
+    void setConstraintDirections();
     
-    void prepareSolve(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 advect(btScalar dt)
     {
         size_t counter = 0;
@@ -145,13 +101,13 @@ public:
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
                 auto& node = psb->m_nodes[j];
-//                node.m_x +=  dt * m_dv[counter++];
-                node.m_x +=  dt * node.m_v;
+                node.m_x +=  dt * m_dv[counter++];
+//                node.m_x  =  node.m_q + dt * node.m_v;
             }
         }
     }
     
-    void updateVelocity()
+    void backupVelocity()
     {
         // serial implementation
         int counter = 0;
@@ -160,13 +116,13 @@ public:
             btSoftBody* psb = m_softBodySet[i];
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
-                psb->m_nodes[j].m_v += m_dv[counter];
-                
-                ++counter;
+                m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
             }
         }
     }
     
+    void updateVelocity();
+    
     bool updateNodes()
     {
         int numNodes = 0;
@@ -175,6 +131,7 @@ public:
         if (numNodes != m_numNodes)
         {
             m_numNodes = numNodes;
+            m_backupVelocity.resize(numNodes);
             return true;
         }
         return false;
@@ -200,15 +157,11 @@ public:
         softBody->defaultCollisionHandler(collisionObjectWrap);
     }
 
-    virtual void processCollision(btSoftBody *, btSoftBody *) {
-        // TODO
+    virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
+        softBody->defaultCollisionHandler(otherSoftBody);
     }
     
-    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
-    {
-        m_world = world;
-        m_objective.setWorld(world);
-    }
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world);
 };
 
 #endif /* btDeformableBodySolver_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 82b24ccb8..0bbb2e995 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -9,6 +9,20 @@
 #include "btDeformableRigidDynamicsWorld.h"
 #include "btDeformableBodySolver.h"
 
+btDeformableBodySolver::btDeformableBodySolver()
+: m_numNodes(0)
+, m_solveIterations(1)
+, m_impulseIterations(1)
+, m_world(nullptr)
+{
+    m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+}
+
+btDeformableBodySolver::~btDeformableBodySolver()
+{
+    delete m_objective;
+}
+
 void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     // Let the solver grab the soft bodies and if necessary optimize for it
@@ -50,9 +64,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
         (*m_internalTickCallback)(this, timeStep);
     }
     
-    
-//    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
-    
     // incorporate gravity into velocity and clear force
     for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
     {
@@ -64,7 +75,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
 
-//    predictUnconstraintMotion(timeStep);
     //integrate transforms
     btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep);
     
@@ -76,6 +86,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     ///update soft bodies
     m_deformableBodySolver->updateSoftBodies();
 
+    clearForces();
     // End solver-wise simulation step
     // ///////////////////////////////
 }
@@ -85,3 +96,23 @@ void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar t
     m_deformableBodySolver->solveConstraints(timeStep);
 }
 
+void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
+{
+    getSoftDynamicsWorld()->getSoftBodyArray().push_back(body);
+    
+    // Set the soft body solver that will deal with this body
+    // to be the world's solver
+    body->setSoftBodySolver(m_deformableBodySolver);
+    
+    btCollisionWorld::addCollisionObject(body,
+                                         collisionFilterGroup,
+                                         collisionFilterMask);
+}
+
+void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    m_deformableBodySolver->predictMotion(float(timeStep));
+}
+
+
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index 98dc4602c..11ad969a2 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -62,27 +62,12 @@ public:
         return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD;
     }
     
-    virtual void predictUnconstraintMotion(btScalar timeStep)
-    {
-        btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
-        m_deformableBodySolver->predictMotion(float(timeStep));
-    }
+    virtual void predictUnconstraintMotion(btScalar timeStep);
     // virtual void internalStepSingleStepSimulation(btScalar timeStep);
     
     // virtual void solveDeformableBodiesConstraints(btScalar timeStep);
     
-    virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter)
-    {
-        getSoftDynamicsWorld()->getSoftBodyArray().push_back(body);
-        
-        // Set the soft body solver that will deal with this body
-        // to be the world's solver
-        body->setSoftBodySolver(m_deformableBodySolver);
-        
-        btCollisionWorld::addCollisionObject(body,
-                                             collisionFilterGroup,
-                                             collisionFilterMask);
-    }
+    virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter);
 };
 
 #endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h
index ffe21c801..9fd3c405a 100644
--- a/src/BulletSoftBody/btMassSpring.h
+++ b/src/BulletSoftBody/btMassSpring.h
@@ -37,7 +37,12 @@ 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();
                 btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r);
                 force[id1] += scaled_force;
@@ -89,7 +94,7 @@ public:
                 const auto& link = psb->m_links[j];
                 const auto node1 = link.m_n[0];
                 const auto node2 = link.m_n[1];
-                btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly
+                btScalar k_damp = psb->m_dampingCoefficient;
                 size_t id1 = m_indices[node1];
                 size_t id2 = m_indices[node2];
                 btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 7efe514f3..41911b2bc 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -880,7 +880,7 @@ struct btSoftColliders
 					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 ra = n.m_q - 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;