From 3bf3b66fb72cb228d4ff8cc61b16c689efbc0b2d Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan2015@u.northwestern.edu>
Date: Mon, 19 Aug 2019 19:18:02 -0700
Subject: [PATCH] add method to remove softbody

---
 src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 5 ++++-
 src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp | 8 ++++++++
 src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h   | 2 ++
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 244fbb6ce..bf88ae550 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -27,7 +27,10 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
 void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt)
 {
     BT_PROFILE("reinitialize");
-    setDt(dt);
+    if (dt > 0)
+    {
+        setDt(dt);
+    }
     if(nodeUpdated)
     {
         updateId();
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 60e3bbbbd..05e1c1ac9 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -269,3 +269,11 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
         forces.push_back(force);
     }
 }
+
+void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+    m_softBodies.remove(body);
+    btCollisionWorld::removeCollisionObject(body);
+    // force a reinitialize so that node indices get updated.
+    m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
+}
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
index bd886d4dc..f8d368c5f 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h
@@ -136,6 +136,8 @@ public:
     
     void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
     
+    void removeSoftBody(btSoftBody* body);
+    
     int getDrawFlags() const { return (m_drawFlags); }
     void setDrawFlags(int f) { m_drawFlags = f; }
 };