From 98cd9a85e4416686e099969332691c91b60a3468 Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com>
Date: Fri, 12 Jul 2019 10:35:50 -0700
Subject: generalize preconditioner, now supports mass preconditioning

---
 src/BulletSoftBody/btBackwardEulerObjective.cpp | 10 ++--
 src/BulletSoftBody/btBackwardEulerObjective.h   | 78 +++++++++++++++++++++----
 2 files changed, 73 insertions(+), 15 deletions(-)

diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp
index 8801e68df..12c7a7add 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp
@@ -8,14 +8,15 @@
 #include "btBackwardEulerObjective.h"
 
 btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
-: cg(20)
+: cg(50)
 , m_softBodies(softBodies)
-, precondition(DefaultPreconditioner())
 , projection(m_softBodies, m_dt)
 , m_backupVelocity(backup_v)
 {
     // TODO: this should really be specified in initialization instead of here
     btMassSpring* mass_spring = new btMassSpring(m_softBodies);
+//    m_preconditioner = new MassPreconditioner(m_softBodies);
+    m_preconditioner = new DefaultPreconditioner();
     m_lf.push_back(mass_spring);
 }
 
@@ -28,8 +29,9 @@ void btBackwardEulerObjective::reinitialize(bool nodeUpdated)
     for (int i = 0; i < m_lf.size(); ++i)
     {
         m_lf[i]->reinitialize(nodeUpdated);
-        projection.reinitialize(nodeUpdated);
     }
+    projection.reinitialize(nodeUpdated);
+    m_preconditioner->reinitialize(nodeUpdated);
 }
 
 
@@ -64,7 +66,7 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const
 void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt)
 {
     m_dt = dt;
-    btScalar tolerance = std::numeric_limits<float>::epsilon()* 16 * computeNorm(residual);
+    btScalar tolerance = std::numeric_limits<float>::epsilon()* 1024 * computeNorm(residual);
     cg.solve(*this, dv, residual, tolerance);
 }
 
diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h
index 424291700..9b94341b7 100644
--- a/src/BulletSoftBody/btBackwardEulerObjective.h
+++ b/src/BulletSoftBody/btBackwardEulerObjective.h
@@ -15,25 +15,82 @@
 #include "btDeformableRigidDynamicsWorld.h"
 
 class btDeformableRigidDynamicsWorld;
-class btBackwardEulerObjective
+
+class Preconditioner
 {
 public:
     using TVStack = btAlignedObjectArray<btVector3>;
-    struct DefaultPreconditioner
+    virtual void operator()(const TVStack& x, TVStack& b) = 0;
+    virtual void reinitialize(bool nodeUpdated) = 0;
+};
+
+class DefaultPreconditioner : public Preconditioner
+{
+public:
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i];
+    }
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        
+    }
+};
+
+class MassPreconditioner : public Preconditioner
+{
+    btAlignedObjectArray<btScalar> m_inv_mass;
+    const btAlignedObjectArray<btSoftBody *>& m_softBodies;
+public:
+    MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies)
+    : m_softBodies(softBodies)
     {
-        void operator()(const TVStack& x, TVStack& b)
+    }
+    
+    virtual void reinitialize(bool nodeUpdated)
+    {
+        if (nodeUpdated)
         {
-            btAssert(b.size() == x.size());
-            for (int i = 0; i < b.size(); ++i)
-                b[i] = x[i];
+            m_inv_mass.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_inv_mass.push_back(psb->m_nodes[j].m_im);
+            }
         }
-    };
+    }
+    
+    virtual void operator()(const TVStack& x, TVStack& b)
+    {
+        btAssert(b.size() == x.size());
+        btAssert(m_inv_mass.size() == x.size());
+        for (int i = 0; i < b.size(); ++i)
+            b[i] = x[i] * m_inv_mass[i];
+    }
+};
+
+class btBackwardEulerObjective
+{
+public:
+    using TVStack = btAlignedObjectArray<btVector3>;
+//    struct DefaultPreconditioner
+//    {
+//        void operator()(const TVStack& x, TVStack& b)
+//        {
+//            btAssert(b.size() == x.size());
+//            for (int i = 0; i < b.size(); ++i)
+//                b[i] = x[i];
+//        }
+//    };
     btScalar m_dt;
     btConjugateGradient<btBackwardEulerObjective> cg;
     btDeformableRigidDynamicsWorld* m_world;
     btAlignedObjectArray<btLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
-    std::function<void(const TVStack&, TVStack&)> precondition;
+    Preconditioner* m_preconditioner;
     btContactProjection projection;
     const TVStack& m_backupVelocity;
     
@@ -92,10 +149,9 @@ public:
         projection(r);
     }
     
-    template <class Func>
-    void setPreconditioner(Func preconditioner_func)
+    void precondition(const TVStack& x, TVStack& b)
     {
-        precondition = preconditioner_func;
+        m_preconditioner->operator()(x,b);
     }
     
     virtual void setWorld(btDeformableRigidDynamicsWorld* world)
-- 
cgit v1.2.1