add newton solver

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -20,6 +20,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 : m_softBodies(softBodies)
 , projection(m_softBodies, m_dt)
 , m_backupVelocity(backup_v)
+, m_implicit(false)
 {
     m_preconditioner = new DefaultPreconditioner();
 }
@@ -72,7 +73,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
     for (int i = 0; i < m_lf.size(); ++i)
     {
         // add damping matrix
-        m_lf[i]->addScaledForceDifferential(-m_dt, x, b);
+        m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b);
+        if (m_implicit)
+        {
+             m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b);
+        }
     }
 }
 
@@ -105,14 +110,22 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero
     }
 }
 
-void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const
+void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual)
 {
     BT_PROFILE("computeResidual");
     // add implicit force
     for (int i = 0; i < m_lf.size(); ++i)
     {
-        m_lf[i]->addScaledImplicitForce(dt, residual);
+        if (m_implicit)
+        {
+            m_lf[i]->addScaledForces(dt, residual);
+        }
+        else
+        {
+            m_lf[i]->addScaledDampingForce(dt, residual);
+        }
     }
+    projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
@@ -122,7 +135,7 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
     {
         norm_squared += residual[i].length2();
     }
-    return std::sqrt(norm_squared+SIMD_EPSILON);
+    return std::sqrt(norm_squared);
 }
 
 void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -37,6 +37,8 @@ public:
     btDeformableContactProjection projection;
     const TVStack& m_backupVelocity;
     btAlignedObjectArray<btSoftBody::Node* > m_nodes;
+    bool m_implicit;
+
     btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v);
     
     virtual ~btDeformableBackwardEulerObjective();
@@ -44,7 +46,7 @@ public:
     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;
+    void computeResidual(btScalar dt, TVStack& residual);
     
     // add explicit force to the velocity
     void applyExplicitForce(TVStack& force);
@@ -117,6 +119,11 @@ public:
     {
         return &m_nodes;
     }
+    
+    void setImplicit(bool implicit)
+    {
+        m_implicit = implicit;
+    }
 };
 
 #endif /* btBackwardEulerObjective_h */

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -21,6 +21,8 @@
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
 , m_cg(50)
+, m_maxNewtonIterations(5)
+, m_newtonTolerance(1e-10)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
@@ -33,17 +35,70 @@ btDeformableBodySolver::~btDeformableBodySolver()
 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 {
     BT_PROFILE("solveConstraints");
-    m_objective->computeResidual(solverdt, m_residual);
+    if (!m_implicit)
-    m_objective->applyDynamicFriction(m_residual);
+    {
-    computeStep(m_dv, m_residual);
+        m_objective->computeResidual(solverdt, m_residual);
-    
+        m_objective->applyDynamicFriction(m_residual);
-    updateVelocity();
+        computeStep(m_dv, m_residual);
+        updateVelocity();
+    }
+    else
+    {
+        for (int i = 0; i < m_maxNewtonIterations; ++i)
+        {
+            updateState();
+            // add the inertia term in the residual
+            int counter = 0;
+            for (int k = 0; k < m_softBodySet.size(); ++k)
+            {
+                btSoftBody* psb = m_softBodySet[k];
+                for (int j = 0; j < psb->m_nodes.size(); ++j)
+                {
+                    if (psb->m_nodes[j].m_im > 0)
+                    {
+                        m_residual[counter] = (-1./psb->m_nodes[j].m_im) *  m_dv[counter];
+                    }
+                    ++counter;
+                }
+            }
+            
+            m_objective->computeResidual(solverdt, m_residual);
+            if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
+            {
+                break;
+            }
+//            m_objective->applyDynamicFriction(m_residual);
+            computeStep(m_ddv, m_residual);
+            updateDv();
+            for (int j = 0; j < m_numNodes; ++j)
+            {
+                m_ddv[j].setZero();
+                m_residual[j].setZero();
+            }
+        }
+    }
 }
 
-void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual)
+void btDeformableBodySolver::updateState()
 {
-    btScalar tolerance = std::numeric_limits<float>::epsilon() * 16 * m_objective->computeNorm(residual);
+    updateVelocity();
-    m_cg.solve(*m_objective, dv, residual, tolerance);
+    updateTempPosition();
+    
+}
+
+void btDeformableBodySolver::updateDv()
+{
+    for (int i = 0; i < m_numNodes; ++i)
+    {
+        m_dv[i] += m_ddv[i];
+    }
+}
+
+void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
+{
+//    btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
+    btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
+    m_cg.solve(*m_objective, ddv, residual, tolerance);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
@@ -54,6 +109,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     if (nodeUpdated)
     {
         m_dv.resize(m_numNodes, btVector3(0,0,0));
+        m_ddv.resize(m_numNodes, btVector3(0,0,0));
         m_residual.resize(m_numNodes, btVector3(0,0,0));
         m_backupVelocity.resize(m_numNodes, btVector3(0,0,0));
     }
@@ -62,9 +118,11 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     for (int i = 0; i < m_numNodes; ++i)
     {
         m_dv[i].setZero();
+        m_ddv[i].setZero();
         m_residual[i].setZero();
     }
     
+    m_dt = dt;
     m_objective->reinitialize(nodeUpdated, dt);
 }
 
@@ -103,6 +161,21 @@ void btDeformableBodySolver::updateVelocity()
     }
 }
 
+void btDeformableBodySolver::updateTempPosition()
+{
+    int counter = 0;
+    for (int i = 0; i < m_softBodySet.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodySet[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v;
+            ++counter;
+        }
+        psb->updateDeformation();
+    }
+}
+
 void btDeformableBodySolver::backupVelocity()
 {
     int counter = 0;
@@ -209,3 +282,9 @@ void btDeformableBodySolver::updateSoftBodies()
         }
     }
 }
+
+void btDeformableBodySolver::setImplicit(bool implicit)
+{
+    m_implicit = implicit;
+    m_objective->setImplicit(implicit);
+}

src/BulletSoftBody/btDeformableBodySolver.h

@@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver
 protected:
     int m_numNodes;
     TVStack m_dv;
+    TVStack m_ddv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
    
@@ -41,7 +42,9 @@ protected:
     btScalar m_dt;
     btScalar m_contact_iterations;
     btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
-    
+    bool m_implicit;
+    int m_maxNewtonIterations;
+    btScalar m_newtonTolerance;
     
 public:
     btDeformableBackwardEulerObjective* m_objective;
@@ -94,7 +97,14 @@ public:
     virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
     
     virtual bool checkInitialized(){return true;}
-
+    
+    void setImplicit(bool implicit);
+    
+    void updateState();
+    
+    void updateDv();
+    
+    void updateTempPosition();
 };
 
 #endif /* btDeformableBodySolver_h */

src/BulletSoftBody/btDeformableCorotatedForce.h

@@ -39,8 +39,9 @@ public:
     {
     }
     
-    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    virtual void addScaledForces(btScalar scale, TVStack& force)
     {
+        addScaledElasticForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -105,7 +106,11 @@ public:
         }
     }
     
-    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+    }
+    
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
     }
     

src/BulletSoftBody/btDeformableGravityForce.h

@@ -28,8 +28,9 @@ public:
     {
     }
     
-    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    virtual void addScaledForces(btScalar scale, TVStack& force)
     {
+        addScaledGravityForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -37,9 +38,16 @@ public:
         addScaledGravityForce(scale, force);
     }
     
-    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+    }
+    
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+    }
+    
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
-        
     }
     
     virtual void addScaledGravityForce(btScalar scale, TVStack& force)

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -18,6 +18,7 @@
 
 #include "btSoftBody.h"
 #include <LinearMath/btHashMap.h>
+#include <iostream>
 
 enum btDeformableLagrangianForceType
 {
@@ -40,12 +41,21 @@ public:
     
     virtual ~btDeformableLagrangianForce(){}
     
-    virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0;
+    // add all forces
+    virtual void addScaledForces(btScalar scale, TVStack& force) = 0;
     
-    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
+    // add damping df
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0;
     
+    // add elastic df
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0;
+    
+    // add all forces that are explicit in explicit solve
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0;
     
+    // add all damping forces 
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0;
+    
     virtual btDeformableLagrangianForceType getForceType() = 0;
     
     virtual void reinitialize(bool nodeUpdated)
@@ -71,5 +81,261 @@ public:
     {
         m_nodes = nodes;
     }
+    
+    virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
+    {
+        btVector3 c1 = dx[id1] - dx[id0];
+        btVector3 c2 = dx[id2] - dx[id0];
+        btVector3 c3 = dx[id3] - dx[id0];
+        btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
+                       c1.getY(), c2.getY(), c3.getY(),
+                       c1.getZ(), c2.getZ(), c3.getZ());
+        return dF;
+    }
+    
+    virtual void testDerivative()
+    {
+        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_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+            }
+            psb->updateDeformation();
+        }
+        
+        
+        TVStack dx;
+        dx.resize(getNumNodes());
+        TVStack dphi_dx;
+        dphi_dx.resize(dx.size());
+        for (int i =0; i < dphi_dx.size();++i)
+        {
+            dphi_dx[i].setZero();
+        }
+        addScaledForces(-1, dphi_dx);
+        
+        // write down the current position
+        TVStack x;
+        x.resize(dx.size());
+        int counter = 0;
+        for (int i = 0; i<m_softBodies.size();++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                x[counter] = psb->m_nodes[j].m_q;
+                counter++;
+            }
+        }
+        counter = 0;
+        
+        // populate dx with random vectors
+        for (int i = 0; i < dx.size(); ++i)
+        {
+            dx[i].setX(randomDouble(-1, 1));
+            dx[i].setY(randomDouble(-1, 1));
+            dx[i].setZ(randomDouble(-1, 1));
+        }
+        
+        btAlignedObjectArray<double> errors;
+        double h = 1;
+        for (int it = 0; it < 10; ++it)
+        {
+            for (int i = 0; i < dx.size(); ++i)
+            {
+                dx[i] *= 0.5;
+            }
+            
+            // get dphi/dx * dx
+            double dphi = 0;
+            for (int i = 0; i < dx.size(); ++i)
+            {
+                dphi += dphi_dx[i].dot(dx[i]);
+            }
+            
+
+            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_q = x[counter] + dx[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            double f1 = totalElasticEnergy();
+            
+            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_q = x[counter] - dx[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            
+            double f2 = totalElasticEnergy();
+            
+            //restore m_q
+            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_q = x[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            double error = f1-f2-2*dphi;
+            errors.push_back(error);
+            std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl;
+        }
+        for (int i = 1; i < errors.size(); ++i)
+        {
+            std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
+        }
+    }
+    
+    virtual void testHessian()
+    {
+        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_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1));
+            }
+            psb->updateDeformation();
+        }
+        
+        
+        TVStack dx;
+        dx.resize(getNumNodes());
+        TVStack df;
+        df.resize(dx.size());
+        TVStack f1;
+        f1.resize(dx.size());
+        TVStack f2;
+        f2.resize(dx.size());
+        
+        
+        // write down the current position
+        TVStack x;
+        x.resize(dx.size());
+        int counter = 0;
+        for (int i = 0; i<m_softBodies.size();++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                x[counter] = psb->m_nodes[j].m_q;
+                counter++;
+            }
+        }
+        counter = 0;
+        
+        // populate dx with random vectors
+        for (int i = 0; i < dx.size(); ++i)
+        {
+            dx[i].setX(randomDouble(-1, 1));
+            dx[i].setY(randomDouble(-1, 1));
+            dx[i].setZ(randomDouble(-1, 1));
+        }
+        
+        btAlignedObjectArray<double> errors;
+        for (int it = 0; it < 10; ++it)
+        {
+            for (int i = 0; i < dx.size(); ++i)
+            {
+                dx[i] *= 0.5;
+            }
+            
+            // get df
+            for (int i =0; i < df.size();++i)
+            {
+                df[i].setZero();
+                f1[i].setZero();
+                f2[i].setZero();
+            }
+
+            //set df
+            addScaledElasticForceDifferential(-1, dx, df);
+            
+            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_q = x[counter] + dx[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            
+            //set f1
+            addScaledForces(-1, f1);
+            
+            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_q = x[counter] - dx[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            
+            //set f2
+            addScaledForces(-1, f2);
+            
+            //restore m_q
+            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_q = x[counter];
+                    counter++;
+                }
+                psb->updateDeformation();
+            }
+            counter = 0;
+            double error = 0;
+            for (int i = 0; i < df.size();++i)
+            {
+                btVector3 error_vector = f1[i]-f2[i]-2*df[i];
+                error += error_vector.length2();
+            }
+            error = btSqrt(error);
+            errors.push_back(error);
+            std::cout << "Iteration = " << it << ", error = " << error << std::endl;
+        }
+        for (int i = 1; i < errors.size(); ++i)
+        {
+            std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl;
+        }
+    }
+    
+    virtual double totalElasticEnergy()
+    {
+        return 0;
+    }
+    
+    double randomDouble(double low, double high)
+    {
+        return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+    }
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

src/BulletSoftBody/btDeformableMassSpringForce.h

@@ -31,9 +31,10 @@ public:
     {
     }
     
-    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    virtual void addScaledForces(btScalar scale, TVStack& force)
     {
         addScaledDampingForce(scale, force);
+        addScaledElasticForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -61,9 +62,9 @@ public:
                 btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
                 if (m_momentum_conserving)
                 {
-                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
                     {
-                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
+                        btVector3 dir = (node2->m_q - node1->m_q).normalized();
                         scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
                     }
                 }
@@ -91,7 +92,7 @@ public:
                 
                 // elastic force
                 // explicit elastic force
-                btVector3 dir = (node2->m_x - node1->m_x);
+                btVector3 dir = (node2->m_q - node1->m_q);
                 btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
                 btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
                 force[id1] += scaled_force;
@@ -100,7 +101,7 @@ public:
         }
     }
     
-    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
         // implicit damping force differential
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -118,9 +119,9 @@ public:
                 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
                 if (m_momentum_conserving)
                 {
-                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
+                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
                     {
-                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
+                        btVector3 dir = (node2->m_q - node1->m_q).normalized();
                         local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
                     }
                 }
@@ -130,6 +131,34 @@ public:
         }
     }
     
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+        // implicit damping force differential
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            btScalar scaled_k_damp = m_dampingStiffness * scale;
+            for (int j = 0; j < psb->m_links.size(); ++j)
+            {
+                const btSoftBody::Link& link = psb->m_links[j];
+                btSoftBody::Node* node1 = link.m_n[0];
+                btSoftBody::Node* node2 = link.m_n[1];
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                btScalar r = link.m_rl;
+
+                // todo @xuchenhan: find df
+                btVector3 dir = (node2->m_q - node1->m_q);
+                btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
+                btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r);
+                btVector3 local_scaled_df = btVector3(0,0,0);
+                
+                df[id1] += local_scaled_df;
+                df[id2] -= local_scaled_df;
+            }
+        }
+    }
+    
     virtual btDeformableLagrangianForceType getForceType()
     {
         return BT_MASSSPRING_FORCE;

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -167,6 +167,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
                 }
             }
             node.m_x  =  node.m_x + timeStep * node.m_v;
+            node.m_q = node.m_x;
         }
     }
     m_deformableBodySolver->revertVelocity();
@@ -174,12 +175,23 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-    // save v_{n+1}^* velocity after explicit forces
+    if (!m_implicit)
-    m_deformableBodySolver->backupVelocity();
+    {
+        // save v_{n+1}^* velocity after explicit forces
+        m_deformableBodySolver->backupVelocity();
+    }
     
     // set up constraints among multibodies and between multibodies and deformable bodies
     setupConstraints();
     solveMultiBodyRelatedConstraints();
+    
+    if (m_implicit)
+    {
+    // revert to v_n after collisions are registered
+        m_deformableBodySolver->revertVelocity();
+        // todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now.
+    }
+    // At this point, dv is golden for nodes in contact
     m_deformableBodySolver->solveDeformableConstraints(timeStep);
 }
 
@@ -273,7 +285,6 @@ void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints()
         }
     }
     
-    // todo : may not be necessary
     for (int i = 0; i < this->m_multiBodies.size(); i++)
     {
         btMultiBody* bod = m_multiBodies[i];
@@ -304,7 +315,13 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
 void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 {
     m_internalTime += timeStep;
+    m_deformableBodySolver->setImplicit(m_implicit);
     m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
+    if (m_implicit)
+    {
+        // backup v_n velocity
+        m_deformableBodySolver->backupVelocity();
+    }
     btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
     dispatchInfo.m_timeStep = timeStep;
     dispatchInfo.m_stepCount = 0;

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -47,6 +47,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
     btSoftBodyWorldInfo m_sbi;
     btScalar m_internalTime;
     int m_contact_iterations;
+    bool m_implicit;
     
     typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
     btSolverCallback m_solverCallback;
@@ -72,7 +73,7 @@ public:
         m_sbi.m_broadphase = pairCache;
         m_sbi.m_dispatcher = dispatcher;
         m_sbi.m_sparsesdf.Initialize();
-        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
+        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025);
         m_sbi.m_sparsesdf.Reset();
         
         m_sbi.air_density = (btScalar)1.2;
@@ -81,6 +82,7 @@ public:
         m_sbi.water_normal = btVector3(0, 0, 0);
         m_sbi.m_gravity.setValue(0, -10, 0);
         m_internalTime = 0.0;
+        m_implicit = false;
     }
 
     void setSolverCallback(btSolverCallback cb)
@@ -153,6 +155,11 @@ public:
     void solveMultiBodyRelatedConstraints();
     
     void sortConstraints();
+    
+    void setImplicit(bool implicit)
+    {
+        m_implicit = implicit;
+    }
 };
 
 #endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H

src/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -25,16 +25,16 @@ public:
     btScalar m_mu, m_lambda;
     btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
     {
-        
     }
     
     btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda)
     {
     }
     
-    virtual void addScaledImplicitForce(btScalar scale, TVStack& force)
+    virtual void addScaledForces(btScalar scale, TVStack& force)
     {
         addScaledDampingForce(scale, force);
+        addScaledElasticForce(scale, force);
     }
     
     virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
@@ -46,6 +46,34 @@ public:
     {
     }
     
+    virtual double totalElasticEnergy()
+    {
+        double energy = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                energy += tetra.m_element_measure * elasticEnergyDensity(tetra);
+            }
+        }
+        return energy;
+    }
+    
+    double elasticEnergyDensity(const btSoftBody::Tetra& t)
+    {
+        double density = 0;
+        btMatrix3x3 F = t.m_F;
+        btMatrix3x3 C = F.transpose()*F;
+        double J = F.determinant();
+        double trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        density += m_mu * 0.5 * (trace - 3.);
+        density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda);
+        density -= m_mu * 0.5 * log(trace+1);
+        return density;
+    }
+    
     virtual void addScaledElasticForce(btScalar scale, TVStack& force)
     {
         int numNodes = getNumNodes();
@@ -72,7 +100,6 @@ public:
                 size_t id3 = node3->index;
                 
                 // elastic force
-                // explicit elastic force
                 btScalar scale1 = scale * tetra.m_element_measure;
                 force[id0] -= scale1 * force_on_node0;
                 force[id1] -= scale1 * force_on_node123.getColumn(0);
@@ -82,25 +109,62 @@ public:
         }
     }
     
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+    }
+    
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+        int numNodes = getNumNodes();
+        btAssert(numNodes <= df.size())
+        btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            {
+                btSoftBody::Tetra& tetra = psb->m_tetras[j];
+                btSoftBody::Node* node0 = tetra.m_n[0];
+                btSoftBody::Node* node1 = tetra.m_n[1];
+                btSoftBody::Node* node2 = tetra.m_n[2];
+                btSoftBody::Node* node3 = tetra.m_n[3];
+                size_t id0 = node0->index;
+                size_t id1 = node1->index;
+                size_t id2 = node2->index;
+                size_t id3 = node3->index;
+                btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
+                btMatrix3x3 dP;
+                firstPiolaDifferential(tetra.m_F, 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();
+                
+                // elastic force differential
+                btScalar scale1 = scale * tetra.m_element_measure;
+                df[id0] -= scale1 * df_on_node0;
+                df[id1] -= scale1 * df_on_node123.getColumn(0);
+                df[id2] -= scale1 * df_on_node123.getColumn(1);
+                df[id3] -= scale1 * df_on_node123.getColumn(2);
+            }
+        }
+    }
+    
     void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
     {
         btMatrix3x3 C = F.transpose()*F;
         btScalar J = F.determinant();
         btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
-        P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1) - 0.75 * m_mu);
+        P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu);
-    }
-    
-    virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
-    {
     }
     
     void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF,  btMatrix3x3& dP)
     {
         btScalar J = F.determinant();
-        addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1) - 0.75*m_mu, dP);
+        btMatrix3x3 C = F.transpose()*F;
-        dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
+        btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
         
-        //todo @xuchenhan: add the dP of the m_mu term.
+        addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP);
+        dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
     }
     
     btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
@@ -108,25 +172,26 @@ public:
         btScalar ans = 0;
         for (int i = 0; i < 3; ++i)
         {
-            for (int j = 0; j < 3; ++j)
+            ans += A[i].dot(B[i]);
-            {
+//            for (int j = 0; j < 3; ++j)
-                ans += A[i][j] * B[i][j];
+//            {
-            }
+//                ans += A[i][j] * B[i][j];
+//            }
         }
         return ans;
     }
     
     void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
     {
-        M[0][0] = scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
+        M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);
-        M[1][0] = scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
+        M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]);
-        M[2][0] = scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
+        M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]);
-        M[0][1] = scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
+        M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]);
-        M[1][1] = scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
+        M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]);
-        M[2][1] = scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
+        M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]);
-        M[0][2] = scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
+        M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]);
-        M[1][2] = scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
+        M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]);
-        M[2][2] = scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
+        M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]);
     }
     
     virtual btDeformableLagrangianForceType getForceType()

src/BulletSoftBody/btSoftBody.cpp

@@ -2834,12 +2834,10 @@ void btSoftBody::updateDeformation()
 {
     for (int i = 0; i < m_tetras.size(); ++i)
     {
-        // updateDeformation is called before predictMotion where m_q is sync'd.
-        // So m_x is the current position of the node.
         btSoftBody::Tetra& t = m_tetras[i];
-        btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x;
+        btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q;
-        btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x;
+        btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q;
-        btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x;
+        btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q;
         btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(),
                        c1.getY(), c2.getY(), c3.getY(),
                        c1.getZ(), c2.getZ(), c3.getZ());