From 5826492020885fd1d87887e260d42376be7e74b8 Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan2015@u.northwestern.edu>
Date: Tue, 27 Aug 2019 21:43:33 -0700
Subject: [PATCH] add elastic force differential for mass spring

---
 .../btDeformableLagrangianForce.h             |  2 -
 .../btDeformableMassSpringForce.h             | 45 ++++++++++++++-----
 2 files changed, 35 insertions(+), 12 deletions(-)

diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h
index 3a5f5a5b1..0e349e5be 100644
--- a/src/BulletSoftBody/btDeformableLagrangianForce.h
+++ b/src/BulletSoftBody/btDeformableLagrangianForce.h
@@ -105,7 +105,6 @@ public:
             psb->updateDeformation();
         }
         
-        
         TVStack dx;
         dx.resize(getNumNodes());
         TVStack dphi_dx;
@@ -140,7 +139,6 @@ public:
         }
         
         btAlignedObjectArray<double> errors;
-        double h = 1;
         for (int it = 0; it < 10; ++it)
         {
             for (int i = 0; i < dx.size(); ++i)
diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h
index 824f8a485..a85dd547a 100644
--- a/src/BulletSoftBody/btDeformableMassSpringForce.h
+++ b/src/BulletSoftBody/btDeformableMassSpringForce.h
@@ -91,7 +91,6 @@ public:
                 size_t id2 = node2->index;
                 
                 // elastic force
-                // explicit elastic force
                 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);
@@ -106,7 +105,7 @@ public:
         // implicit damping force differential
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
-            const btSoftBody* psb = m_softBodies[i];
+            btSoftBody* psb = m_softBodies[i];
             btScalar scaled_k_damp = m_dampingStiffness * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
@@ -130,6 +129,26 @@ public:
             }
         }
     }
+    virtual double totalElasticEnergy()
+    {
+        double energy = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            const btSoftBody* psb = m_softBodies[i];
+            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];
+                btScalar r = link.m_rl;
+
+                // elastic force
+                btVector3 dir = (node2->m_q - node1->m_q);
+                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r );
+            }
+        }
+        return energy;
+    }
     
     virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
     {
@@ -137,7 +156,7 @@ public:
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             const btSoftBody* psb = m_softBodies[i];
-            btScalar scaled_k_damp = m_dampingStiffness * scale;
+            btScalar scaled_k = m_elasticStiffness * scale;
             for (int j = 0; j < psb->m_links.size(); ++j)
             {
                 const btSoftBody::Link& link = psb->m_links[j];
@@ -147,14 +166,20 @@ public:
                 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);
+                btVector3 dir = (node1->m_q - node2->m_q);
+                btScalar dir_norm = dir.norm();
+                btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0);
+                btVector3 dx_diff = dx[id1] - dx[id2];
+                btVector3 scaled_df = btVector3(0,0,0);
+                if (dir_norm > SIMD_EPSILON)
+                {
+                    scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;
+                    scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized;
+                    scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff;
+                }
                 
-                df[id1] += local_scaled_df;
-                df[id2] -= local_scaled_df;
+                df[id1] += scaled_df;
+                df[id2] -= scaled_df;
             }
         }
     }