factor out force; now btDeformableLagrangianceForce can be specified at configuration time and to specific softbody

18 files changed, 150 insertions, 204 deletions

diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp
index f51978ae3..c40564867 100644
--- a/examples/DeformableContact/DeformableContact.cpp
+++ b/examples/DeformableContact/DeformableContact.cpp
@@ -127,8 +127,9 @@ void DeformableContact::initPhysics()
     
     m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
     deformableBodySolver->setWorld(getDeformableDynamicsWorld());
-	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     {
@@ -169,8 +170,8 @@ void DeformableContact::initPhysics()
         bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
         bool canSleep = false;
         bool selfCollide = true;
-        btVector3 linkHalfExtents(1, 1, 1);
-        btVector3 baseHalfExtents(1, 1, 1);
+        btVector3 linkHalfExtents(.4, 1, .4);
+        btVector3 baseHalfExtents(.4, 1, .4);
         
         btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
         
@@ -228,6 +229,8 @@ void DeformableContact::initPhysics()
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = .1;
         getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
     }
 
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp
index aa58fbd6f..2411670cf 100644
--- a/examples/DeformableDemo/DeformableDemo.cpp
+++ b/examples/DeformableDemo/DeformableDemo.cpp
@@ -168,8 +168,9 @@ void DeformableDemo::initPhysics()
 	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
     deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
-	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
     
 //    getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -235,6 +236,8 @@ void DeformableDemo::initPhysics()
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 1;
         getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
         
         // add a few rigid bodies
         Ctor_RbUpStack(1);
diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp
index cc3008f9c..4252a47b8 100644
--- a/examples/Pinch/Pinch.cpp
+++ b/examples/Pinch/Pinch.cpp
@@ -252,8 +252,9 @@ void Pinch::initPhysics()
 	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
     deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
-	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
     
     getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -336,12 +337,14 @@ void Pinch::initPhysics()
 //        psb->translate(btVector3(-2.5, 4, -2.5));
 //        psb->getCollisionShape()->setMargin(0.1);
 //        psb->setTotalMass(1);
-        psb->setSpringStiffness(4);
+        psb->setSpringStiffness(2);
         psb->setDampingCoefficient(0.02);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 2;
         getDeformableDynamicsWorld()->addSoftBody(psb);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
         // add a grippers
         createGrip();
     }
diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp
index b30e9bc24..15fb712f6 100644
--- a/examples/VolumetricDeformable/VolumetricDeformable.cpp
+++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp
@@ -186,8 +186,9 @@ void VolumetricDeformable::initPhysics()
 	m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
     deformableBodySolver->setWorld(getDeformableDynamicsWorld());
 	//	m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality
-	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
-    getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0);
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
     {
@@ -240,6 +241,8 @@ void VolumetricDeformable::initPhysics()
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 0.5;
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
     }
     // add a few rigid bodies
     Ctor_RbUpStack(4); 
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
deleted file mode 100644
index 16ec0a542..000000000
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ /dev/null
@@ -1,100 +0,0 @@
-//
-//  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 "btLagrangianForce.h"
-#include "btMassSpring.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<btLagrangianForce*> 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();
-        // TODO rename
-        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/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index 928ee46f8..b4a887fa6 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -109,12 +109,13 @@ public:
     using TArrayStack = btAlignedObjectArray<btAlignedObjectArray<btScalar> >;
     btAlignedObjectArray<btSoftBody *> m_softBodies;
     btDeformableRigidDynamicsWorld* m_world;
-    std::unordered_map<btSoftBody::Node *, size_t> m_indices;
+    const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
     const btScalar& m_dt;
     
-    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
     : m_softBodies(softBodies)
     , m_dt(dt)
+    , m_indices(indices)
     {
     }
     
@@ -132,22 +133,6 @@ public:
     
     virtual void reinitialize(bool nodeUpdated)
     {
-        if (nodeUpdated)
-            updateId();
-    }
-    
-    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)
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 31b0905ac..acc9db75f 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -9,21 +9,22 @@
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
 : m_softBodies(softBodies)
-, projection(m_softBodies, m_dt)
+, projection(m_softBodies, m_dt, &m_indices)
 , m_backupVelocity(backup_v)
 {
     // TODO: this should really be specified in initialization instead of here
-    btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
-    btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
+//    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);
+//    m_lf.push_back(mass_spring);
+//    m_lf.push_back(gravity);
 }
 
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)
 {
     if(nodeUpdated)
     {
+        updateId();
         projection.setSoftBodies(m_softBodies);
     }
     for (int i = 0; i < m_lf.size(); ++i)
@@ -69,7 +70,7 @@ 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)
     {
-        int i = projection.m_indices[it.first];
+        int i = m_indices[it.first];
         it.first->m_v = m_backupVelocity[i] + dv[i];
     }
 }
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index bfd4ca3af..e22b42b0b 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -28,6 +28,7 @@ public:
     Preconditioner* m_preconditioner;
     btDeformableContactProjection projection;
     const TVStack& m_backupVelocity;
+    std::unordered_map<btSoftBody::Node *, size_t> m_indices;
     
     btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
@@ -95,6 +96,24 @@ public:
         m_world = world;
         projection.setWorld(world);
     }
+    
+    virtual void updateId()
+    {
+        size_t index = 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)
+            {
+                m_indices[&(psb->m_nodes[j])] = index++;
+            }
+        }
+    }
+    
+    std::unordered_map<btSoftBody::Node *, size_t>* getIndices()
+    {
+        return &m_indices;
+    }
 };
 
 #endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 1a9cad120..4d6ea4e84 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -88,26 +88,27 @@ void btDeformableContactProjection::update()
                             const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
                             const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
                             const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
+                            const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
                             deltaV_normal = &c->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] * J_n[k];
+                                vel += local_v[k] * J_n[k];
                             }
                             va = cti.m_normal * vel * m_dt;
 
+                            // add in the tangential components of the va
                             vel = 0.0;
                             for (int k = 0; k < ndof; ++k)
                             {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
+                                vel += local_v[k] * J_t1[k];
                             }
                             va += c->t1 * vel * m_dt;
                             vel = 0.0;
                             for (int k = 0; k < ndof; ++k)
                             {
-                                vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
+                                vel += local_v[k] * J_t2[k];
                             }
                             va += c->t2 * vel * m_dt;
                         }
@@ -177,7 +178,7 @@ void btDeformableContactProjection::update()
                     
                     // the following is equivalent
                     /*
-                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]];
+                        btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)];
                         btScalar dvn = dv.dot(cti.m_normal);
                      */
                     
@@ -196,9 +197,11 @@ void btDeformableContactProjection::update()
                     {
                         if (multibodyLinkCol)
                         {
+                            // apply normal component of the impulse
                             multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal));
                             if (incremental_tangent.norm() > SIMD_EPSILON)
                             {
+                                // apply tangential component of the impulse
                                const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
                                 multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1));
                                 const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
@@ -336,7 +339,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
     for (auto& it : m_constraints)
     {
         const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices[it.first];
+        size_t i = m_indices->at(it.first);
         const btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
@@ -399,7 +402,7 @@ void btDeformableContactProjection::project(TVStack& x)
     for (auto& it : m_constraints)
     {
         const btAlignedObjectArray<DeformableContactConstraint>& constraints = it.second;
-        size_t i = m_indices[it.first];
+        size_t i = m_indices->at(it.first);
         btAlignedObjectArray<DeformableFrictionConstraint>& frictions = m_frictions[it.first];
         btAssert(constraints.size() <= dim);
         btAssert(constraints.size() > 0);
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index e73d8faa4..ea3b00f62 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -18,8 +18,8 @@ public:
     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)
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt, const std::unordered_map<btSoftBody::Node *, size_t>* indices)
+    : btCGProjection(softBodies, dt, indices)
     {
     }
     
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
index d8571fa73..398662e1f 100644
--- a/src/BulletSoftBody/btDeformableGravityForce.h
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -16,7 +16,7 @@ public:
     using TVStack = btDeformableLagrangianForce::TVStack;
     btVector3 m_gravity;
     
-    btDeformableGravityForce(const btAlignedObjectArray<btSoftBody *>& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g)
+    btDeformableGravityForce(const btVector3& g) : m_gravity(g)
     {
         
     }
@@ -39,14 +39,14 @@ public:
     virtual void addScaledGravityForce(btScalar scale, TVStack& force)
     {
         int numNodes = getNumNodes();
-        btAssert(numNodes == force.size())
+        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];
+                size_t id = m_indices->at(&n);
                 btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im;
                 btVector3 scaled_force = scale * m_gravity * mass;
                 force[id] += scaled_force;
@@ -54,7 +54,10 @@ public:
         }
     }
     
-
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_GRAVITY_FORCE;
+    }
     
     
 };
diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
index acb9a28ff..fa4184a14 100644
--- a/src/BulletSoftBody/btDeformableLagrangianForce.h
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -10,16 +10,20 @@
 
 #include "btSoftBody.h"
 #include <unordered_map>
+enum btDeformableLagrangianForceType
+{
+    BT_GRAVITY_FORCE = 1,
+    BT_MASSSPRING_FORCE = 2
+};
 
 class btDeformableLagrangianForce
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
-    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    std::unordered_map<btSoftBody::Node *, size_t> m_indices;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;
+    const std::unordered_map<btSoftBody::Node *, size_t>* m_indices;
     
-    btDeformableLagrangianForce(const btAlignedObjectArray<btSoftBody *>& softBodies)
-    : m_softBodies(softBodies)
+    btDeformableLagrangianForce()
     {
     }
     
@@ -31,23 +35,10 @@ public:
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
     
+    virtual btDeformableLagrangianForceType getForceType() = 0;
+    
     virtual void reinitialize(bool nodeUpdated)
     {
-        if (nodeUpdated)
-            updateId();
-    }
-    
-    virtual void updateId()
-    {
-        size_t index = 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)
-            {
-                m_indices[&(psb->m_nodes[j])] = index++;
-            }
-        }
     }
     
     virtual int getNumNodes()
@@ -59,5 +50,15 @@ public:
         }
         return numNodes;
     }
+    
+    virtual void addSoftBody(btSoftBody* psb)
+    {
+        m_softBodies.push_back(psb);
+    }
+    
+    virtual void setIndices(const std::unordered_map<btSoftBody::Node *, size_t>* indices)
+    {
+        m_indices = indices;
+    }
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 70ad6e69c..c9fca134c 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -14,15 +14,13 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
 public:
     using TVStack = btDeformableLagrangianForce::TVStack;
-    btDeformableMassSpringForce(const btAlignedObjectArray<btSoftBody *>& softBodies) : btDeformableLagrangianForce(softBodies)
+    btDeformableMassSpringForce()
     {
-        
     }
     
     virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
     {
         addScaledDampingForce(scale, force);
-//        addScaledElasticForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -33,7 +31,7 @@ public:
     virtual void addScaledDampingForce(btScalar scale, TVStack& force)
     {
         int numNodes = getNumNodes();
-        btAssert(numNodes == force.size())
+        btAssert(numNodes <= force.size())
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
@@ -42,8 +40,8 @@ public:
                 const auto& link = psb->m_links[j];
                 const auto node1 = link.m_n[0];
                 const auto node2 = link.m_n[1];
-                size_t id1 = m_indices[node1];
-                size_t id2 = m_indices[node2];
+                size_t id1 = m_indices->at(node1);
+                size_t id2 = m_indices->at(node2);
                 
                 // damping force
                 btVector3 v_diff = (node2->m_v - node1->m_v);
@@ -58,7 +56,7 @@ public:
     virtual void addScaledElasticForce(btScalar scale, TVStack& force)
     {
         int numNodes = getNumNodes();
-        btAssert(numNodes == force.size())
+        btAssert(numNodes <= force.size())
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
@@ -69,8 +67,8 @@ public:
                 const auto node2 = link.m_n[1];
                 btScalar kLST = link.Feature::m_material->m_kLST;
                 btScalar r = link.m_rl;
-                size_t id1 = m_indices[node1];
-                size_t id2 = m_indices[node2];
+                size_t id1 = m_indices->at(node1);
+                size_t id2 = m_indices->at(node2);
                 
                 // elastic force
                 // explicit elastic force
@@ -95,8 +93,8 @@ public:
                 const auto node1 = link.m_n[0];
                 const auto node2 = link.m_n[1];
                 btScalar k_damp = psb->m_dampingCoefficient;
-                size_t id1 = m_indices[node1];
-                size_t id2 = m_indices[node2];
+                size_t id1 = m_indices->at(node1);
+                size_t id2 = m_indices->at(node2);
                 btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]);
                 df[id1] += local_scaled_df;
                 df[id2] -= local_scaled_df;
@@ -104,6 +102,10 @@ public:
         }
     }
     
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_MASSSPRING_FORCE;
+    }
     
 };
 
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index f9c9968c9..b91d9d2c4 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -60,13 +60,12 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
                 if (c == nullptr || c->m_node->m_im == 0)
                     continue;
                 const btSoftBody::sCti& cti = c->m_cti;
-                btRigidBody* rigidCol = 0;
                 btVector3 va(0, 0, 0);
                 
                 // grab the velocity of the rigid body
                 if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
                     va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
                 }
                 else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -78,25 +77,25 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
                         const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
                         const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
                         const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                        
+                        const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
                         // 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] * J_n[k];
+                            vel += local_v[k] * J_n[k];
                         }
                         va = cti.m_normal * vel;
                         
                         vel = 0.0;
                         for (int k = 0; k < ndof; ++k)
                         {
-                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k];
+                            vel += local_v[k] * J_t1[k];
                         }
                         va += c->t1 * vel;
                         vel = 0.0;
                         for (int k = 0; k < ndof; ++k)
                         {
-                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k];
+                            vel += local_v[k] * J_t2[k];
                         }
                         va += c->t2 * vel;
                     }
@@ -110,7 +109,6 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
                 if (cti.m_colObj->hasContactResponse())
                 {
                     btScalar dp = cti.m_offset;
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
                     if (friction.m_static[j] == true)
                     {
                         c->m_node->m_v = va;
@@ -214,3 +212,24 @@ void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
     for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i)
         m_beforeSolverCallbacks[i](m_internalTime, this);
 }
+
+void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+    bool added = false;
+    for (int i = 0; i < forces.size(); ++i)
+    {
+        if (forces[i]->getForceType() == force->getForceType())
+        {
+            forces[i]->addSoftBody(psb);
+            added = true;
+            break;
+        }
+    }
+    if (!added)
+    {
+        force->addSoftBody(psb);
+        force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+        forces.push_back(force);
+    }
+}
diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
index f1be4fd85..c6b3dcbc6 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h
@@ -126,6 +126,8 @@ public:
     
     void afterSolverCallbacks(btScalar timeStep);
     
+    void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+    
     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 bd7587c47..c31bdb7ed 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2266,14 +2266,15 @@ btVector3 btSoftBody::evaluateCom() const
 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 							  const btVector3& x,
 							  btScalar margin,
-							  btSoftBody::sCti& cti) const
+							  btSoftBody::sCti& cti, bool predict) const
 {
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
-        const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+    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();
+    const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
+//    const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
     // TODO: get the correct transform for multibody
 
 	btScalar dst =
@@ -2282,11 +2283,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 			shp,
 			nrm,
 			margin);
-	if (dst < 0)
+	if (dst < 0 || !predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
-//        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
         cti.m_offset = dst;
 		return (true);
 	}
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 95249b093..5e33d74d0 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -1005,7 +1005,7 @@ public:
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
-	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
+	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
 	void updateNormals();
 	void updateBounds();
 	void updatePose();
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 8cf0ed3c0..2ec8c1b55 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -945,14 +945,17 @@ struct btSoftColliders
 
 			if (!n.m_battach)
             {
-				if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+                // check for collision at x_{n+1}^*
+                if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
+//                if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false));
                 {
                     const btScalar ima = n.m_im;
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
                     const btScalar ms = ima + imb;
                     if (ms > 0)
                     {
-                        psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti);
+                        // resolve contact at x_n
+                        psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
                         auto& cti = c.m_cti;
                         c.m_node = &n;
                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
@@ -982,8 +985,13 @@ struct btSoftColliders
                                 btVector3 t2 = btCross(normal, t1);
                                 btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
                                 findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
-                                findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
-                                findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
+                                
+                                // findJacobian is hella expensive, avoid calling if possible
+                                if (fc != 0)
+                                {
+                                    findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
+                                    findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
+                                }
                                 
                                 btScalar* J_n = &jacobianData_normal.m_jacobians[0];
                                 btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
@@ -995,16 +1003,7 @@ struct btSoftColliders
                                 
                                 btMatrix3x3 rot(normal, t1, t2); // world frame to local frame
                                 const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                                btVector3 u_dot_J(0,0,0);
-                                for (int i = 0; i < ndof; ++i)
-                                {
-                                    u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]);
-                                }
-                                btVector3 impulse_matrix_diag;
                                 btScalar dt = psb->m_sst.sdt;
-                                impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt));
-                                impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt));
-                                impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt));
                                 btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
                                 c.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
                                 c.jacobianData_normal = jacobianData_normal;