add relative tolerance for linear solver and newton with line search

src/BulletSoftBody/btConjugateGradient.h

@@ -26,16 +26,18 @@ class btConjugateGradient
     typedef btAlignedObjectArray<btVector3> TVStack;
     TVStack r,p,z,temp;
     int max_iterations;
+    btScalar tolerance;
 public:
     btConjugateGradient(const int max_it_in)
     : max_iterations(max_it_in)
     {
+       tolerance = 1024 * std::numeric_limits<btScalar>::epsilon();
     }
     
     virtual ~btConjugateGradient(){}
     
     // return the number of iterations taken
-    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false)
+    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar relative_tolerance, bool verbose = false)
     {
         BT_PROFILE("CGSolve");
         btAssert(x.size() == b.size());
@@ -48,7 +50,8 @@ public:
         A.precondition(r, z);
         A.project(z);
         btScalar r_dot_z = dot(z,r);
-        if (dot(z,z) < tolerance) {
+        btScalar local_tolerance = btMin(relative_tolerance * std::sqrt(r_dot_z), tolerance);
+        if (std::sqrt(r_dot_z) < local_tolerance) {
             if (verbose)
             {
                 std::cout << "Iteration = 0" << std::endl;
@@ -58,11 +61,21 @@ public:
         }
         p = z;
         btScalar r_dot_z_new = r_dot_z;
-        for (int k = 1; k < max_iterations; k++) {
+        for (int k = 1; k <= max_iterations; k++) {
             // temp = A*p
             A.multiply(p, temp);
             A.project(temp);
             // alpha = r^T * z / (p^T * A * p)
+            if (dot(p,temp) < 0)
+            {
+                if (verbose)
+                    std::cout << "Encountered negative direction in CG!"<<std::endl;
+                if (k == 1)
+                {
+                    x = b;
+                }
+              return k;
+            }
             btScalar alpha = r_dot_z_new / dot(p, temp);
             //  x += alpha * p;
             multAndAddTo(alpha, p, x);
@@ -72,7 +85,7 @@ public:
             A.precondition(r, z);
             r_dot_z = r_dot_z_new;
             r_dot_z_new = dot(r,z);
-            if (r_dot_z_new < tolerance) {
+            if (std::sqrt(r_dot_z_new) < local_tolerance) {
                 if (verbose)
                 {
                     std::cout << "ConjugateGradient iterations " << k << std::endl;

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -134,9 +134,19 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual
     btScalar mag = 0;
     for (int i = 0; i < residual.size(); ++i)
     {
-        mag += residual[i].length();
+        mag += residual[i].length2();
     }
-    return mag;
+    return std::sqrt(mag);
+}
+
+btScalar btDeformableBackwardEulerObjective::totalEnergy()
+{
+    btScalar e = 0;
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        e += m_lf[i]->totalElasticEnergy();
+    }
+    return e;
 }
 
 void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -124,6 +124,8 @@ public:
     {
         m_implicit = implicit;
     }
+
+    btScalar totalEnergy();
 };
 
 #endif /* btBackwardEulerObjective_h */

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -23,6 +23,11 @@ btDeformableBodySolver::btDeformableBodySolver()
 , m_cg(20)
 , m_maxNewtonIterations(5)
 , m_newtonTolerance(1e-4)
+//, m_lineSearch(false)
+//, m_cg(10)
+//, m_maxNewtonIterations(5)
+//, m_newtonTolerance(1e-3)
+, m_lineSearch(true)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
@@ -63,13 +68,37 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             }
             
             m_objective->computeResidual(solverdt, m_residual);
-            if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
+            if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
             {
                 break;
             }
             m_objective->applyDynamicFriction(m_residual);
+            if (m_lineSearch)
+            {
+                btScalar inner_product = computeDescentStep(m_ddv,m_residual);
+                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;
+                btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2;
+                backupDv();
+                do {
+                    scale *= beta;
+                    if (scale < 1e-8) {
+                        //std::cout << "Could not find sufficient descent!" << std::endl;
+                        return;
+                    }
+                    updateEnergy(scale);
+                    f1 = m_objective->totalEnergy()+kineticEnergy();
+                    f2 = f0 - alpha * scale * inner_product;
+                } while (!(f1 < f2)); // if anything here is nan then the search continues
+                revertDv();
+                updateDv(scale);
+            }
+            else
+            {
                 computeStep(m_ddv, m_residual);
                 updateDv();
+            }
+
             for (int j = 0; j < m_numNodes; ++j)
             {
                 m_ddv[j].setZero();
@@ -79,26 +108,99 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
     }
 }
 
+btScalar btDeformableBodySolver::kineticEnergy()
+{
+    btScalar ke = 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)
+        {
+            btSoftBody::Node& node = psb->m_nodes[j];
+            if (node.m_im > 0)
+            {
+                ke += m_dv[node.index].length2() * 0.5 / node.m_im;
+            }
+        }
+    }
+    return ke;
+}
+
+void btDeformableBodySolver::backupDv()
+{
+    m_backup_dv.resize(m_dv.size());
+    for (int i = 0; i<m_backup_dv.size(); ++i)
+    {
+        m_backup_dv[i] = m_dv[i];
+    }
+}
+
+void btDeformableBodySolver::revertDv()
+{
+    for (int i = 0; i<m_backup_dv.size(); ++i)
+    {
+        m_dv[i] = m_backup_dv[i];
+    }
+}
+
+void btDeformableBodySolver::updateEnergy(btScalar scale)
+{
+    for (int i = 0; i<m_dv.size(); ++i)
+    {
+        m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
+    }
+    updateState();
+}
+
+
+btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual)
+{
+    btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
+    m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
+    btScalar inner_product = m_cg.dot(residual, m_ddv);
+    btScalar tol = 1e-5 * m_objective->computeNorm(residual) * m_objective->computeNorm(m_ddv);
+    if (inner_product < -tol)
+    {
+        std::cout << "Looking backwards!" << std::endl;
+        for (int i = 0; i < m_ddv.size();++i)
+        {
+            m_ddv[i] = -m_ddv[i];
+        }
+        inner_product = -inner_product;
+    }
+    else if (std::abs(inner_product) < tol)
+    {
+        std::cout << "Gradient Descent!" << std::endl;
+        btScalar res_norm = m_objective->computeNorm(residual);
+        btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
+        for (int i = 0; i < m_ddv.size();++i)
+        {
+            m_ddv[i] = scale * residual[i];
+        }
+        inner_product = scale * res_norm * res_norm;
+    }
+    return inner_product;
+}
+
 void btDeformableBodySolver::updateState()
 {
     updateVelocity();
     updateTempPosition();
-    
 }
 
-void btDeformableBodySolver::updateDv()
+void btDeformableBodySolver::updateDv(btScalar scale)
 {
     for (int i = 0; i < m_numNodes; ++i)
     {
-        m_dv[i] += m_ddv[i];
+        m_dv[i] += scale * 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();
+    btScalar relative_tolerance = btMin(0.5, std::sqrt(btMax(m_objective->computeNorm(residual), m_newtonTolerance)));
-    m_cg.solve(*m_objective, ddv, residual, tolerance);
+    m_cg.solve(*m_objective, ddv, residual, relative_tolerance, false);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)

src/BulletSoftBody/btDeformableBodySolver.h

@@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver
 protected:
     int m_numNodes;
     TVStack m_dv;
+    TVStack m_backup_dv;
     TVStack m_ddv;
     TVStack m_residual;
     btAlignedObjectArray<btSoftBody *> m_softBodySet;
@@ -45,6 +46,7 @@ protected:
     bool m_implicit;
     int m_maxNewtonIterations;
     btScalar m_newtonTolerance;
+    bool m_lineSearch;
     
 public:
     btDeformableBackwardEulerObjective* m_objective;
@@ -82,6 +84,7 @@ public:
     bool updateNodes();
     
     void computeStep(TVStack& dv, const TVStack& residual);
+    btScalar computeDescentStep(TVStack& ddv, const TVStack& residual);
                      
     virtual void predictMotion(btScalar solverdt);
 
@@ -103,9 +106,13 @@ public:
     
     void updateState();
     
-    void updateDv();
+    void updateDv(btScalar scale = 1);
     
     void updateTempPosition();
+    void backupDv();
+    void revertDv();
+    void updateEnergy(btScalar scale);
+    btScalar kineticEnergy();
 };
 
 #endif /* btDeformableBodySolver_h */

src/BulletSoftBody/btDeformableGravityForce.h

@@ -73,6 +73,24 @@ public:
         return BT_GRAVITY_FORCE;
     }
 
+    virtual double totalElasticEnergy()
+    {
+        double e = 0;
+        for (int i = 0; i<m_softBodies.size();++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                const btSoftBody::Node& node = psb->m_nodes[j];
+                if (node.m_im > 0)
+                {
+                    e -= m_gravity.dot(node.m_q)/node.m_im;
+                }
+            }
+        }
+        return e;
+    }
+    
     
 };
 #endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */