add contact constraint as projections in CG

9 files changed, 392 insertions, 141 deletions

diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 7fe961921..51b3d906d 100644
--- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -17,7 +17,6 @@ subject to the following restrictions:
 #define BT_DISCRETE_DYNAMICS_WORLD_H
 
 #include "btDynamicsWorld.h"
-
 class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
@@ -26,6 +25,7 @@ class btTypedConstraint;
 class btActionInterface;
 class btPersistentManifold;
 class btIDebugDraw;
+
 struct InplaceSolverIslandCallback;
 
 #include "LinearMath/btAlignedObjectArray.h"
@@ -76,7 +76,7 @@ protected:
 
 	virtual void calculateSimulationIslands();
 
-	virtual void solveConstraints(btContactSolverInfo & solverInfo);
+	
 
 	virtual void updateActivationState(btScalar timeStep);
 
@@ -95,7 +95,7 @@ protected:
 	void serializeRigidBodies(btSerializer * serializer);
 
 	void serializeDynamicsWorldInfo(btSerializer * serializer);
-
+    
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
@@ -107,6 +107,8 @@ public:
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
 	virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
 
+    virtual void solveConstraints(btContactSolverInfo & solverInfo);
+    
 	virtual void synchronizeMotionStates();
 
 	///this can be useful to synchronize a single rigid body -> graphics object
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index 2dca39bb6..f8f0596b5 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -11,42 +11,14 @@
 #include "btConjugateGradient.h"
 #include "btLagrangianForce.h"
 #include "btMassSpring.h"
+#include "btContactProjection.h"
+#include "btDeformableRigidDynamicsWorld.h"
 
-struct DirichletDofProjection
-{
-    using TVStack = btAlignedObjectArray<btVector3>;
-    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    DirichletDofProjection(const btAlignedObjectArray<btSoftBody *>& softBodies)
-    : m_softBodies(softBodies)
-    {}
-    
-    void operator()(TVStack& x)
-    {
-        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)
-                {
-                    x[counter].setZero();
-                }
-                ++counter;
-            }
-        }
-    }
-};
-
+class btDeformableRigidDynamicsWorld;
 class btBackwardEulerObjective
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
-    struct EmptyProjection
-    {
-        void operator()(TVStack& x)
-        {}
-    };
     struct DefaultPreconditioner
     {
         void operator()(const TVStack& x, TVStack& b)
@@ -58,19 +30,19 @@ public:
     };
     btScalar m_dt;
     btConjugateGradient<btBackwardEulerObjective> cg;
-    
+    btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    std::function<void(TVStack&)> project;
     std::function<void(const TVStack&, TVStack&)> precondition;
+    btContactProjection projection;
     
-    btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies)
+    btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
     : cg(20)
     , m_softBodies(softBodies)
-    , project(EmptyProjection())
     , precondition(DefaultPreconditioner())
+    , projection(m_softBodies, backup_v)
     {
-        // this should really be specified in initialization instead of here
+        // TODO: this should really be specified in initialization instead of here
         btMassSpring* mass_spring = new btMassSpring(m_softBodies);
         m_lf.push_back(mass_spring);
     }
@@ -103,7 +75,6 @@ public:
     void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
     {
         m_dt = dt;
-        // TODO:figure out what the tolerance should be
         btScalar tolerance = std::numeric_limits<float>::epsilon()*16 * computeNorm(residual);
         cg.solve(*this, dv, residual, tolerance);
     }
@@ -133,24 +104,33 @@ public:
         }
     }
     
+    void updateProjection(const TVStack& dv)
+    {
+        projection.update(m_dt, dv);
+    }
+    
     void reinitialize(bool nodeUpdated)
     {
-        for (int i = 0; i < m_lf.size(); ++i)
+        if(nodeUpdated)
         {
-            m_lf[i]->reinitialize(nodeUpdated);
+            projection.setSoftBodies(m_softBodies);
         }
-        
-        if(nodeUpdated)
+        for (int i = 0; i < m_lf.size(); ++i)
         {
-            DirichletDofProjection dirichlet(m_softBodies);
-            setProjection(dirichlet);
+            m_lf[i]->reinitialize(nodeUpdated);
+            projection.reinitialize(nodeUpdated);
         }
     }
     
-    template <class Func>
-    void setProjection(Func project_func)
+    void enforceConstraint(TVStack& x)
+    {
+        projection.enforceConstraint(x);
+    }
+    
+    void project(TVStack& r, const TVStack& dv)
     {
-        project = project_func;
+        updateProjection(dv);
+        projection(r);
     }
     
     template <class Func>
@@ -158,6 +138,12 @@ public:
     {
         precondition = preconditioner_func;
     }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+        projection.setWorld(world);
+    }
 };
 
 #endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
new file mode 100644
index 000000000..89743ef27
--- /dev/null
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -0,0 +1,80 @@
+//  btCGProjection.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/4/19.
+//
+
+#ifndef BT_CG_PROJECTION_H
+#define BT_CG_PROJECTION_H
+
+#include "btSoftBody.h"
+#include <unordered_map>
+
+class btDeformableRigidDynamicsWorld;
+class btCGProjection
+{
+public:
+//    static const int dim = 3;
+    using TVStack = btAlignedObjectArray<btVector3>;
+    using TVArrayStack = btAlignedObjectArray<btAlignedObjectArray<btVector3> >;
+    using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;
+    btDeformableRigidDynamicsWorld* m_world;
+    std::unordered_map<btSoftBody::Node *, size_t> m_indices;
+    TVArrayStack m_constrainedDirections;
+    TArrayStack m_constrainedValues;
+    const TVStack& m_backupVelocity;
+    
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
+    : m_softBodies(softBodies)
+    , m_backupVelocity(backup_v)
+    {
+        
+    }
+    
+    virtual ~btCGProjection()
+    {
+        
+    }
+    
+    // apply the constraints
+    virtual void operator()(TVStack& x) = 0;
+    
+    // update the constraints
+    virtual void update(btScalar dt, const TVStack& dv) = 0;
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        if (nodeUpdated)
+            updateId();
+        m_constrainedValues.resize(m_indices.size());
+        m_constrainedDirections.resize(m_indices.size());
+    }
+    
+    void updateId()
+    {
+        size_t index = 0;
+        m_indices.clear();
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                m_indices[&(psb->m_nodes[j])] = index++;
+            }
+        }
+    }
+    
+    void setSoftBodies(btAlignedObjectArray<btSoftBody* > softBodies)
+    {
+        m_softBodies.copyFromArray(softBodies);
+    }
+    
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
+    {
+        m_world = world;
+    }
+};
+
+
+#endif /* btCGProjection_h */
diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index 76297226d..2754a208d 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -89,7 +89,7 @@ public:
 //    }
     
     // return the number of iterations taken
-    int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
+    int solve(TM& A, TVStack& x, const TVStack& b, btScalar tolerance)
     {
         btAssert(x.size() == b.size());
         reinitialize(b);
@@ -97,7 +97,8 @@ public:
         // r = b - A * x --with assigned dof zeroed out
         A.multiply(x, temp);
         r = sub(b, temp);
-        A.project(r);
+        A.project(r,x);
+        A.enforceConstraint(x);
         
         btScalar r_norm = std::sqrt(squaredNorm(r));
         if (r_norm < tolerance) {
@@ -124,7 +125,8 @@ public:
             multAndAddTo(-alpha, temp, r);
             
             // zero out the dofs of r
-            A.project(r);
+            A.project(r,x);
+            A.enforceConstraint(x);
             
             r_norm = std::sqrt(squaredNorm(r));
             
diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp
new file mode 100644
index 000000000..b50918af4
--- /dev/null
+++ b/src/BulletSoftBody/btContactProjection.cpp
@@ -0,0 +1,142 @@
+//
+//  btContactProjection.cpp
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/4/19.
+//
+
+#include "btContactProjection.h"
+#include "btDeformableRigidDynamicsWorld.h"
+void btContactProjection::update(btScalar dt, const TVStack& dv)
+{
+    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)
+        {
+            psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter];
+            ++counter;
+        }
+    }
+        
+    ///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
+    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];
+            
+            // 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)) * 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 * 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 vr = vb - va;
+                const btScalar dn = btDot(vr, cti.m_normal);
+                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)));
+                    
+                    //c.m_node->m_v -= impulse * c.m_c2 / dt;
+                    // TODO: only contact is considered here, add friction later
+                    btVector3 normal = cti.m_normal.normalized();
+                    btVector3 dv = -impulse * c.m_c2;
+                    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);
+                    
+                    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);
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h
new file mode 100644
index 000000000..dc2c8800e
--- /dev/null
+++ b/src/BulletSoftBody/btContactProjection.h
@@ -0,0 +1,56 @@
+//
+//  btContactProjection.h
+//  BulletSoftBody
+//
+//  Created by Xuchen Han on 7/4/19.
+//
+
+#ifndef BT_CONTACT_PROJECTION_H
+#define BT_CONTACT_PROJECTION_H
+#include "btCGProjection.h"
+#include "btSoftBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include <iostream>
+class btContactProjection : public btCGProjection
+{
+public:
+    btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btAlignedObjectArray<btVector3>& backup_v)
+    : btCGProjection(softBodies, backup_v)
+    {
+        
+    }
+    
+    virtual ~btContactProjection()
+    {
+        
+    }
+    
+    // apply the constraints
+    virtual void operator()(TVStack& x)
+    {
+        for (int i = 0; i < x.size(); ++i)
+        {
+            for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+            {
+                x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
+            }
+        }
+    }
+    
+    virtual void enforceConstraint(TVStack& x)
+    {
+        for (int i = 0; i < x.size(); ++i)
+        {
+            for (int j = 0; j < m_constrainedDirections[i].size(); ++j)
+            {
+                x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j];
+                x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j];
+            }
+        }
+    }
+    
+    // update the constraints
+    virtual void update(btScalar dt, const TVStack& dv);
+};
+#endif /* btContactProjection_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 4e36a2112..85441a803 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -10,12 +10,14 @@
 
 #include "btSoftBodySolvers.h"
 #include "btBackwardEulerObjective.h"
-
+#include "btDeformableRigidDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
 struct btCollisionObjectWrapper;
 
+class btDeformableRigidDynamicsWorld;
+
 class btDeformableBodySolver : public btSoftBodySolver
 {
     using TVStack = btAlignedObjectArray<btVector3>;
@@ -29,13 +31,16 @@ protected:
     btBackwardEulerObjective m_objective;
     int m_solveIterations;
     int m_impulseIterations;
+    btDeformableRigidDynamicsWorld* m_world;
+    btAlignedObjectArray<btVector3> m_backupVelocity;
     
 public:
     btDeformableBodySolver()
     : m_numNodes(0)
-    , m_objective(m_softBodySet)
+    , m_objective(m_softBodySet, m_backupVelocity)
     , m_solveIterations(1)
     , m_impulseIterations(1)
+    , m_world(nullptr)
     {
     }
     
@@ -76,23 +81,11 @@ public:
     {
         bool nodeUpdated = updateNodes();
         reinitialize(nodeUpdated);
-        
+        backupVelocity();
         for (int i = 0; i < m_solveIterations; ++i)
         {
-            // get the velocity after contact solve
-            // TODO: perform contact solve here
-            for (int j = 0; j < m_impulseIterations; ++j)
-            {
-                for (int s = 0; s < m_softBodySet.size(); ++s)
-                    VSolve_RContacts(m_softBodySet[s], 0, solverdt);
-            }
-            
-            // advect with v_n+1 ** to update position based states
-            // where v_n+1 ** is the velocity after contact response
-            
             // 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);
             
@@ -107,7 +100,9 @@ public:
         {
             m_dv.resize(m_numNodes);
             m_residual.resize(m_numNodes);
+            m_backupVelocity.resize(m_numNodes);
         }
+        
         for (int i = 0; i < m_dv.size(); ++i)
         {
             m_dv[i].setZero();
@@ -151,7 +146,22 @@ 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++];
+                psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter];
+                ++counter;
+            }
+        }
+    }
+    
+    void backupVelocity()
+    {
+        // 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)
+            {
+                m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
             }
         }
     }
@@ -168,6 +178,7 @@ public:
         }
         return false;
     }
+    
     virtual void predictMotion(float solverdt)
     {
         for (int i = 0; i < m_softBodySet.size(); ++i)
@@ -192,78 +203,11 @@ public:
         // TODO
     }
     
-    void VSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar dt)
+    virtual void setWorld(btDeformableRigidDynamicsWorld* world)
     {
-        const btScalar mrg = psb->getCollisionShape()->getMargin();
-        btMultiBodyJacobianData jacobianData;
-        for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i)
-        {
-            const btSoftBody::RContact& c = psb->m_rcontacts[i];
-            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;
-                
-                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);
-                }
-                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 * dt;
-                    }
-                }
-                
-                const btVector3 vb = c.m_node->m_x - c.m_node->m_q;
-                const btVector3 vr = vb - va;
-                const btScalar dn = btDot(vr, cti.m_normal);
-                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) + (cti.m_normal * (dp * c.m_c4))) * kst);
-                    c.m_node->m_v -= impulse * c.m_c2 / dt;
-                    
-                    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);
-                        }
-                    }
-                }
-            }
-        }
+        m_world = world;
+        m_objective.setWorld(world);
     }
-
 };
 
 #endif /* btDeformableBodySolver_h */
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 73d4f67f6..1b21ed5b3 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -18,12 +18,52 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS
     {
         btAssert("Solver initialization failed\n");
     }
+
+    // from btDiscreteDynamicsWorld singleStepSimulation
+    if (0 != m_internalPreTickCallback)
+    {
+        (*m_internalPreTickCallback)(this, timeStep);
+    }
+    
+    ///apply gravity, predict motion
+    btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+    
+    
+    btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo();
+    
+    dispatchInfo.m_timeStep = timeStep;
+    dispatchInfo.m_stepCount = 0;
+    dispatchInfo.m_debugDraw = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDebugDrawer();
+    
+    // only used in CCD
+//    createPredictiveContacts(timeStep);
+    
+    ///perform collision detection
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::performDiscreteCollisionDetection();
     
-    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::calculateSimulationIslands();
+    
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
+
+    if (0 != m_internalTickCallback)
+    {
+        (*m_internalTickCallback)(this, timeStep);
+    }
+    
+    
+//    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep);
     
     ///solve deformable bodies constraints
     solveDeformableBodiesConstraints(timeStep);
 
+    predictUnconstraintMotion(timeStep);
+    //integrate transforms
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep);
+    
+    ///update vehicle simulation
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActions(timeStep);
+    
+    btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActivationState(timeStep);
     
     ///update soft bodies
     m_deformableBodySolver->updateSoftBodies();
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index 55ed41d86..7f70fef56 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -31,7 +31,6 @@ class btDeformableRigidDynamicsWorld : public btSoftRigidDynamicsWorld
     ///Solver classes that encapsulate multiple deformable bodies for solving
     btDeformableBodySolver* m_deformableBodySolver;
 
-
 protected:
     virtual void internalSingleStepSimulation(btScalar timeStep);
     
@@ -65,7 +64,7 @@ public:
     
     virtual void predictUnconstraintMotion(btScalar timeStep)
     {
-        btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
+//        btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
         m_deformableBodySolver->predictMotion(float(timeStep));
     }
     // virtual void internalStepSingleStepSimulation(btScalar timeStep);