From e124c62a705a8e9e8137daeb9e59fb0f54f234ad Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan@google.com>
Date: Thu, 29 Aug 2019 10:08:34 -0700
Subject: [PATCH] each node only allows a single contact; initial guess for
 newton set to x = x_n + dt*v_n

---
 src/BulletSoftBody/btCGProjection.h                 | 10 ++++++++++
 src/BulletSoftBody/btDeformableBodySolver.cpp       | 13 +++++++++++--
 .../btDeformableContactProjection.cpp               | 13 ++++++++++++-
 src/BulletSoftBody/btSoftBodyInternals.h            |  2 +-
 4 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h
index d074ca6ed..9055ad5ce 100644
--- a/src/BulletSoftBody/btCGProjection.h
+++ b/src/BulletSoftBody/btCGProjection.h
@@ -49,6 +49,16 @@ struct DeformableContactConstraint
         m_static.push_back(false);
         m_can_be_dynamic.push_back(true);
     }
+
+    void replace(const btSoftBody::RContact& rcontact)
+    {
+        m_contact.clear();
+        m_total_normal_dv.clear();
+        m_total_tangent_dv.clear();
+        m_static.clear();
+        m_can_be_dynamic.clear();
+        append(rcontact);
+    }
     
     ~DeformableContactConstraint()
     {
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 600dbafa4..7fc8aa99e 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -96,7 +96,7 @@ void btDeformableBodySolver::updateDv()
 
 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_objective->computeNorm(residual);
     btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
     m_cg.solve(*m_objective, ddv, residual, tolerance);
 }
@@ -197,7 +197,16 @@ void btDeformableBodySolver::backupVn()
         btSoftBody* psb = m_softBodySet[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
-            m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
+            // Here:
+            // dv = 0 for nodes not in constraints
+            // dv = v_{n+1} - v_{n+1}^* for nodes in constraints
+            if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL)
+            {
+                m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
+            }
+            // Now:
+            // dv = 0 for nodes not in constraints
+            // dv = v_{n+1} - v_n for nodes in constraints
             m_backupVelocity[counter++] = psb->m_nodes[j].m_vn;
         }
     }
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 33feb4bc6..ffd91d5df 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -20,6 +20,7 @@
 btScalar btDeformableContactProjection::update()
 {
     btScalar residualSquare = 0;
+    btScalar max_impulse = 0;
     // loop through constraints to set constrained values
     for (int index = 0; index < m_constraints.size(); ++index)
     {
@@ -128,6 +129,7 @@ btScalar btDeformableContactProjection::update()
                     }
                 }
                 impulse = impulse_normal + impulse_tangent;
+                max_impulse = btMax(impulse.length2(), max_impulse);
                 
                 // dn is the normal component of velocity diffrerence. Approximates the residual.
                 residualSquare = btMax(residualSquare, dn*dn);
@@ -231,7 +233,16 @@ void btDeformableContactProjection::setConstraints()
                     else
                     {
                         DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
-                        constraints.append(c);
+                        bool single_contact = true;
+                        if (single_contact)
+                        {
+                          constraints.m_contact[0]->m_cti.m_offset > cti.m_offset;
+                          constraints.replace(c);
+                        }
+                        else
+                        {
+                          constraints.append(c);
+                        }
                     }
                 }
             }
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index fc4c6b556..8ce114fa8 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -998,7 +998,7 @@ struct btSoftColliders
 			if (!n.m_battach)
             {
                 // check for collision at x_{n+1}^*
-                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
+                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;