diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index 0236d9941..67d04eb4b 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -50,7 +50,8 @@ public:
         A.precondition(r, z);
         A.project(z);
         btScalar r_dot_z = dot(z,r);
-        btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance);
+//        btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance);
+        btScalar local_tolerance = tolerance;
         if (std::sqrt(r_dot_z) <= local_tolerance) {
             if (verbose)
             {
@@ -66,7 +67,7 @@ public:
             A.multiply(p, temp);
             A.project(temp);
             // alpha = r^T * z / (p^T * A * p)
-            if (dot(p,temp) < 0)
+            if (dot(p,temp) < SIMD_EPSILON)
             {
                 if (verbose)
                     std::cout << "Encountered negative direction in CG!"<<std::endl;
@@ -92,7 +93,8 @@ public:
                 }
                 return k;
             }
-            btScalar beta = r_dot_z_new/ r_dot_z;
+
+            btScalar beta = r_dot_z_new/r_dot_z;
             p = multAndAdd(beta, p, z);
         }
         if (verbose)
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 29801ab1d..49232725a 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -19,7 +19,7 @@
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
 : m_softBodies(softBodies)
-, projection(m_softBodies, m_dt)
+, m_projection(softBodies)
 , m_backupVelocity(backup_v)
 , m_implicit(false)
 {
@@ -46,8 +46,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
     {
         m_lf[i]->reinitialize(nodeUpdated);
     }
-    projection.reinitialize(nodeUpdated);
-    projection.setIndices(getIndices());
+    m_projection.reinitialize(nodeUpdated);
     m_preconditioner->reinitialize(nodeUpdated);
 }
 
@@ -85,13 +84,6 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
 
 void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
-//    // only the velocity of the constrained nodes needs to be updated during contact solve
-//    for (int i = 0; i < projection.m_constraints.size(); ++i)
-//    {
-//        int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
-//        m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
-//    }
-    
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
@@ -137,7 +129,7 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
             m_lf[i]->addScaledDampingForce(dt, residual);
         }
     }
-    projection.project(residual);
+    m_projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@@ -150,12 +142,12 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
     return std::sqrt(mag);
 }
 
-btScalar btDeformableBackwardEulerObjective::totalEnergy()
+btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
 {
     btScalar e = 0;
     for (int i = 0; i < m_lf.size(); ++i)
     {
-        e += m_lf[i]->totalElasticEnergy();
+        e += m_lf[i]->totalEnergy(dt);
     }
     return e;
 }
@@ -164,7 +156,7 @@ void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 {
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        m_softBodies[i]->updateDeformation();
+        m_softBodies[i]->advanceDeformation();
     }
     
     for (int i = 0; i < m_lf.size(); ++i)
@@ -191,10 +183,10 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 //set constraints as projections
 void btDeformableBackwardEulerObjective::setConstraints()
 {
-    projection.setConstraints();
+    m_projection.setConstraints();
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
 {
-     projection.applyDynamicFriction(r);
+     m_projection.applyDynamicFriction(r);
 }
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index cc6d118b7..8265d61d5 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
@@ -34,7 +34,7 @@ public:
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     Preconditioner* m_preconditioner;
-    btDeformableContactProjection projection;
+    btDeformableContactProjection m_projection;
     const TVStack& m_backupVelocity;
     btAlignedObjectArray<btSoftBody::Node* > m_nodes;
     bool m_implicit;
@@ -71,6 +71,7 @@ public:
     
     void setDt(btScalar dt);
     
+    // add friction force to residual
     void applyDynamicFriction(TVStack& r);
     
     // add dv to velocity
@@ -83,7 +84,7 @@ public:
     void project(TVStack& r)
     {
         BT_PROFILE("project");
-        projection.project(r);
+        m_projection.project(r);
     }
     
     // perform precondition M^(-1) x = b
@@ -124,7 +125,7 @@ public:
         m_implicit = implicit;
     }
 
-    btScalar totalEnergy();
+    btScalar totalEnergy(btScalar dt);
 };
 
 #endif /* btBackwardEulerObjective_h */
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 0df93534c..d2add96db 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -22,11 +22,11 @@
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
 , m_cg(20)
-, m_maxNewtonIterations(10)
+, m_maxNewtonIterations(3)
 , m_newtonTolerance(1e-4)
 , m_lineSearch(true)
 {
-    m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+    m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
@@ -51,9 +51,9 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             updateState();
             // add the inertia term in the residual
             int counter = 0;
-            for (int k = 0; k < m_softBodySet.size(); ++k)
+            for (int k = 0; k < m_softBodies.size(); ++k)
             {
-                btSoftBody* psb = m_softBodySet[k];
+                btSoftBody* psb = m_softBodies[k];
                 for (int j = 0; j < psb->m_nodes.size(); ++j)
                 {
                     if (psb->m_nodes[j].m_im > 0)
@@ -77,7 +77,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
                 btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
                 btScalar scale = 2;
                 // todo xuchenhan@: add damping energy to f0 and f1
-                btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2;
+                btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
                 backupDv();
                 do {
                     scale *= beta;
@@ -85,9 +85,9 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
                         return;
                     }
                     updateEnergy(scale);
-                    f1 = m_objective->totalEnergy()+kineticEnergy();
+                    f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
                     f2 = f0 - alpha * scale * inner_product;
-                } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
+                } while (!(f1 < f2)); // if anything here is nan then the search continues
                 revertDv();
                 updateDv(scale);
             }
@@ -108,9 +108,9 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 btScalar btDeformableBodySolver::kineticEnergy()
 {
     btScalar ke = 0;
-    for (int i = 0; i < m_softBodySet.size();++i)
+    for (int i = 0; i < m_softBodies.size();++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size();++j)
         {
             btSoftBody::Node& node = psb->m_nodes[j];
@@ -152,11 +152,13 @@ void btDeformableBodySolver::updateEnergy(btScalar scale)
 
 btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual)
 {
-    btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
+//    btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
+    btScalar relative_tolerance = 0.5;
     m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
     btScalar inner_product = m_cg.dot(residual, m_ddv);
-    btScalar tol = 1e-3 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv);
     btScalar res_norm = m_objective->computeNorm(residual);
+    btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
+    
     if (inner_product < -tol)
     {
         std::cout << "Looking backwards!" << std::endl;
@@ -196,13 +198,14 @@ void btDeformableBodySolver::updateDv(btScalar scale)
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
     //btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
-    btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
+//    btScalar relative_tolerance = btMin(btScalar(0.5), std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
+    btScalar relative_tolerance = 0.5;
     m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
 {
-    m_softBodySet.copyFromArray(softBodies);
+    m_softBodies.copyFromArray(softBodies);
     bool nodeUpdated = updateNodes();
     
     if (nodeUpdated)
@@ -234,7 +237,7 @@ void btDeformableBodySolver::setConstraints()
 btScalar btDeformableBodySolver::solveContactConstraints()
 {
     BT_PROFILE("setConstraint");
-    btScalar maxSquaredResidual = m_objective->projection.update();
+    btScalar maxSquaredResidual = m_objective->m_projection.update();
     return maxSquaredResidual;
 }
 
@@ -242,9 +245,9 @@ btScalar btDeformableBodySolver::solveContactConstraints()
 void btDeformableBodySolver::updateVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             // set NaN to zero;
@@ -261,9 +264,9 @@ void btDeformableBodySolver::updateVelocity()
 void btDeformableBodySolver::updateTempPosition()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
@@ -276,9 +279,9 @@ void btDeformableBodySolver::updateTempPosition()
 void btDeformableBodySolver::backupVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
@@ -289,9 +292,9 @@ void btDeformableBodySolver::backupVelocity()
 void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             if (implicit)
@@ -307,9 +310,9 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 void btDeformableBodySolver::revertVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[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++];
@@ -320,8 +323,8 @@ void btDeformableBodySolver::revertVelocity()
 bool btDeformableBodySolver::updateNodes()
 {
     int numNodes = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
-        numNodes += m_softBodySet[i]->m_nodes.size();
+    for (int i = 0; i < m_softBodies.size(); ++i)
+        numNodes += m_softBodies[i]->m_nodes.size();
     if (numNodes != m_numNodes)
     {
         m_numNodes = numNodes;
@@ -333,9 +336,9 @@ bool btDeformableBodySolver::updateNodes()
 
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody *psb = m_softBodySet[i];
+        btSoftBody *psb = m_softBodies[i];
         
         if (psb->isActive())
         {
@@ -417,9 +420,9 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 
 void btDeformableBodySolver::updateSoftBodies()
 {
-    for (int i = 0; i < m_softBodySet.size(); i++)
+    for (int i = 0; i < m_softBodies.size(); i++)
     {
-        btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
+        btSoftBody *psb = (btSoftBody *)m_softBodies[i];
         if (psb->isActive())
         {
             psb->updateNormals(); // normal is updated here
diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h
index 31fe01934..ffce06937 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.h
+++ b/src/BulletSoftBody/btDeformableBodySolver.h
@@ -37,7 +37,7 @@ protected:
     TVStack m_backup_dv;
     TVStack m_ddv;
     TVStack m_residual;
-    btAlignedObjectArray<btSoftBody *> m_softBodySet;
+    btAlignedObjectArray<btSoftBody *> m_softBodies;
    
     btAlignedObjectArray<btVector3> m_backupVelocity;
     btScalar m_dt;
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 408f73b88..3943e695f 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -40,7 +40,6 @@ btScalar btDeformableContactProjection::update()
         residualSquare = btMax(residualSquare, localResidualSquare);
     }
     
-    // todo xuchenhan@: deformable/deformable constraints
     return residualSquare;
 }
 
@@ -401,7 +400,6 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 
 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
-    btCGProjection::reinitialize(nodeUpdated);
     m_staticConstraints.clear();
     m_nodeRigidConstraints.clear();
     m_faceRigidConstraints.clear();
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index 57a7ecd94..6c91fd584 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -22,9 +22,14 @@
 #include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
 #include <vector>
-class btDeformableContactProjection : public btCGProjection
+class btDeformableContactProjection
 {
 public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    typedef btAlignedObjectArray<btAlignedObjectArray<btVector3> > TVArrayStack;
+    typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
+    btAlignedObjectArray<btSoftBody *>& m_softBodies;
+    
     // map from node index to static constraint
     btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
     // map from node index to node rigid constraint
@@ -40,8 +45,8 @@ public:
     // map from node index to projection directions
     btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
     
-    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
-    : btCGProjection(softBodies, dt)
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
+    : m_softBodies(softBodies)
     {
     }
     
diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h
index f5b4c2de9..cc935abeb 100644
--- a/src/BulletSoftBody/btDeformableGravityForce.h
+++ b/src/BulletSoftBody/btDeformableGravityForce.h
@@ -73,7 +73,7 @@ public:
         return BT_GRAVITY_FORCE;
     }
 
-    virtual double totalElasticEnergy()
+    virtual double totalEnergy(btScalar dt)
     {
         double e = 0;
         for (int i = 0; i<m_softBodies.size();++i)
diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
index 7ce72a7fd..53f06e4ea 100644
--- a/src/BulletSoftBody/btDeformableLagrangianForce.h
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -28,6 +28,11 @@ enum btDeformableLagrangianForceType
     BT_NEOHOOKEAN_FORCE = 4
 };
 
+static inline double randomDouble(double low, double high)
+{
+    return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+}
+
 class btDeformableLagrangianForce
 {
 public:
@@ -176,7 +181,7 @@ public:
                 psb->updateDeformation();
             }
             counter = 0;
-            double f1 = totalElasticEnergy();
+            double f1 = totalElasticEnergy(0);
             
             for (int i = 0; i<m_softBodies.size();++i)
             {
@@ -190,7 +195,7 @@ public:
             }
             counter = 0;
             
-            double f2 = totalElasticEnergy();
+            double f2 = totalElasticEnergy(0);
             
             //restore m_q
             for (int i = 0; i<m_softBodies.size();++i)
@@ -337,14 +342,22 @@ public:
         }
     }
     
-    virtual double totalElasticEnergy()
+    //
+    virtual double totalElasticEnergy(btScalar dt)
     {
         return 0;
     }
     
-    double randomDouble(double low, double high)
+    //
+    virtual double totalDampingEnergy(btScalar dt)
     {
-        return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+        return 0;
+    }
+    
+    // total Energy takes dt as input because certain energies depend on dt
+    virtual double totalEnergy(btScalar dt)
+    {
+        return totalElasticEnergy(dt) + totalDampingEnergy(dt);
     }
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 73d7efb42..2f71d1d4a 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -129,7 +129,7 @@ public:
             }
         }
     }
-    virtual double totalElasticEnergy()
+    virtual double totalElasticEnergy(btScalar dt)
     {
         double energy = 0;
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -144,7 +144,7 @@ public:
 
                 // elastic force
                 btVector3 dir = (node2->m_q - node1->m_q);
-                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r );
+                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r);
             }
         }
         return energy;
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 5cd72f597..f4191bf76 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -194,7 +194,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
     // set up constraints among multibodies and between multibodies and deformable bodies
     setupConstraints();
     solveMultiBodyRelatedConstraints();
-    m_deformableBodySolver->m_objective->projection.setProjection();
+    m_deformableBodySolver->m_objective->m_projection.setProjection();
     
     // for explicit scheme, m_backupVelocity = v_{n+1}^*
     // for implicit scheme, m_backupVelocity = v_n
diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
index 37803e6e3..842855644 100644
--- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h
+++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h
@@ -26,12 +26,12 @@ public:
     btScalar m_mu_damp, m_lambda_damp;
     btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
     {
-        btScalar damping = 0.005;
+        btScalar damping = 0.05;
         m_mu_damp = damping * m_mu;
         m_lambda_damp = damping * m_lambda;
     }
     
-    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda)
+    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda)
     {
         m_mu_damp = damping * m_mu;
         m_lambda_damp = damping * m_lambda;
@@ -71,7 +71,7 @@ public:
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
-                firstPiolaDampingDifferential(psb->m_tetraScratches[j], dF, dP);
+                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
                 btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 
@@ -85,7 +85,7 @@ public:
         }
     }
     
-    virtual double totalElasticEnergy()
+    virtual double totalElasticEnergy(btScalar dt)
     {
         double energy = 0;
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -101,6 +101,35 @@ public:
         return energy;
     }
     
+    virtual double totalDampingEnergy(btScalar dt)
+    {
+        double energy = 0;
+        int sz = 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)
+            {
+                sz = btMax(sz, psb->m_nodes[j].index);
+            }
+        }
+        TVStack dampingForce;
+        dampingForce.resize(sz+1);
+        for (int i = 0; i < dampingForce.size(); ++i)
+            dampingForce[i].setZero();
+        addScaledDampingForce(0.5, dampingForce);
+        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 btSoftBody::Node& node = psb->m_nodes[j];
+                energy -= dampingForce[node.index].dot(node.m_v) / dt;
+            }
+        }
+        return energy;
+    }
+    
     double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
     {
         double density = 0;
@@ -168,7 +197,7 @@ public:
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
-                firstPiolaDampingDifferential(psb->m_tetraScratches[j], dF, dP);
+                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
                 btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
                 btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose();
 
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 6a2f005b1..95f2c4860 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2914,6 +2914,15 @@ void btSoftBody::updateDeformation()
         s.m_cofF = t.m_F.adjoint().transpose();
     }
 }
+
+void btSoftBody::advanceDeformation()
+{
+    updateDeformation();
+    for (int i = 0; i < m_tetras.size(); ++i)
+    {
+        m_tetraScratchesTn[i] = m_tetraScratches[i];
+    }
+}
 //
 void btSoftBody::Joint::Prepare(btScalar dt, int)
 {
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 67c0d3448..1016fe9f6 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -770,6 +770,7 @@ public:
     tFaceArray m_renderFaces;                // Faces
 	tTetraArray m_tetras;              // Tetras
     btAlignedObjectArray<TetraScratch> m_tetraScratches;
+    btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
 	tAnchorArray m_anchors;            // Anchors
 	tRContactArray m_rcontacts;        // Rigid contacts
     btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
@@ -1100,6 +1101,7 @@ public:
     void setSpringStiffness(btScalar k);
     void initializeDmInverse();
     void updateDeformation();
+    void advanceDeformation();
 	void applyForces();
     void interpolateRenderMesh();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
@@ -1114,8 +1116,6 @@ public:
   
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
-
-	//virtual void serializeSingleObject(class btSerializer* serializer) const;
 };
 
 #endif  //_BT_SOFT_BODY_H
diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp
index ed99ce943..5e58fd89e 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.cpp
+++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -1234,6 +1234,7 @@ if(face&&face[0])
 	}
     psb->initializeDmInverse();
     psb->m_tetraScratches.resize(psb->m_tetras.size());
+    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
 	printf("Nodes:  %u\r\n", psb->m_nodes.size());
 	printf("Links:  %u\r\n", psb->m_links.size());
 	printf("Faces:  %u\r\n", psb->m_faces.size());
@@ -1376,6 +1377,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
     
     psb->initializeDmInverse();
     psb->m_tetraScratches.resize(psb->m_tetras.size());
+    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
     printf("Nodes:  %u\r\n", psb->m_nodes.size());
     printf("Links:  %u\r\n", psb->m_links.size());
     printf("Faces:  %u\r\n", psb->m_faces.size());