From 5330396c70fe671c01162c3e1a1dc2420ba60705 Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan2015@u.northwestern.edu>
Date: Thu, 29 Aug 2019 11:12:35 -0700
Subject: [PATCH] enabled mass preconditioner

---
 src/BulletSoftBody/btConjugateGradient.h                  | 2 ++
 src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 3 ++-
 src/BulletSoftBody/btDeformableBodySolver.cpp             | 4 ++--
 src/BulletSoftBody/btDeformableContactProjection.cpp      | 8 +++++---
 src/BulletSoftBody/btPreconditioner.h                     | 2 ++
 5 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h
index 6fad0e60a..c20a8ae5d 100644
--- a/src/BulletSoftBody/btConjugateGradient.h
+++ b/src/BulletSoftBody/btConjugateGradient.h
@@ -104,7 +104,9 @@ public:
         TVStack c;
         c.resize(a.size());
         for (int i = 0; i < a.size(); ++i)
+        {
             c[i] = a[i] - b[i];
+        }
         return c;
     }
     
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index b66a02aff..4ca5187c4 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -14,6 +14,7 @@
  */
 
 #include "btDeformableBackwardEulerObjective.h"
+#include "btPreconditioner.h"
 #include "LinearMath/btQuickprof.h"
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
@@ -22,7 +23,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 , m_backupVelocity(backup_v)
 , m_implicit(false)
 {
-    m_preconditioner = new DefaultPreconditioner();
+    m_preconditioner = new MassPreconditioner(m_softBodies);
 }
 
 btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective()
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 7fc8aa99e..a902322ce 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -20,9 +20,9 @@
 
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
-, m_cg(50)
+, m_cg(20)
 , m_maxNewtonIterations(5)
-, m_newtonTolerance(1e-10)
+, m_newtonTolerance(1e-4)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index ffd91d5df..404eda494 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -236,12 +236,14 @@ void btDeformableContactProjection::setConstraints()
                         bool single_contact = true;
                         if (single_contact)
                         {
-                          constraints.m_contact[0]->m_cti.m_offset > cti.m_offset;
-                          constraints.replace(c);
+                            if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset)
+                            {
+                                constraints.replace(c);
+                            }
                         }
                         else
                         {
-                          constraints.append(c);
+                            constraints.append(c);
                         }
                     }
                 }
diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h
index 32d697a63..d71242038 100644
--- a/src/BulletSoftBody/btPreconditioner.h
+++ b/src/BulletSoftBody/btPreconditioner.h
@@ -70,7 +70,9 @@ public:
         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];
+        }
     }
 };