From 532f0683e8ab206d332fa0db31f75b4c4a3721b3 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 9 Feb 2016 11:32:58 -0800
Subject: [PATCH] remove all contact info before removing objects

---
 libraries/physics/src/PhysicsEngine.cpp | 42 +++++++++++++++----------
 libraries/physics/src/PhysicsEngine.h   |  7 ++---
 2 files changed, 29 insertions(+), 20 deletions(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 0a7ef606ba..f950d0c017 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -136,23 +136,19 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
     motionState->clearIncomingDirtyFlags();
 }
 
-// private
-void PhysicsEngine::removeObjectFromDynamicsWorld(ObjectMotionState* object) {
-    // wake up anything touching this object
-    bump(object);
-    removeContacts(object);
-
-    btRigidBody* body = object->getRigidBody();
-    assert(body);
-    _dynamicsWorld->removeRigidBody(body);
-}
-
 void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) {
+    // first bump and prune contacts for all objects in the list
     for (auto object : objects) {
-        removeObjectFromDynamicsWorld(object);
+        bumpAndPruneContacts(object);
+    }
+
+    // then remove them
+    for (auto object : objects) {
+        btRigidBody* body = object->getRigidBody();
+        assert(body);
+        _dynamicsWorld->removeRigidBody(body);
 
         // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
-        btRigidBody* body = object->getRigidBody();
         object->setRigidBody(nullptr);
         body->setMotionState(nullptr);
         delete body;
@@ -163,7 +159,8 @@ void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) {
 void PhysicsEngine::removeObjects(const SetOfMotionStates& objects) {
     for (auto object : objects) {
         btRigidBody* body = object->getRigidBody();
-        removeObjectFromDynamicsWorld(object);
+        assert(body);
+        _dynamicsWorld->removeRigidBody(body);
 
         // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
         object->setRigidBody(nullptr);
@@ -200,7 +197,13 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob
 }
 
 void PhysicsEngine::reinsertObject(ObjectMotionState* object) {
-    removeObjectFromDynamicsWorld(object);
+    // remove object from DynamicsWorld
+    bumpAndPruneContacts(object);
+    btRigidBody* body = object->getRigidBody();
+    assert(body);
+    _dynamicsWorld->removeRigidBody(body);
+
+    // add it back
     addObjectToDynamicsWorld(object);
 }
 
@@ -402,13 +405,14 @@ void PhysicsEngine::dumpStatsIfNecessary() {
 // CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
 // CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
 
-void PhysicsEngine::bump(ObjectMotionState* motionState) {
+void PhysicsEngine::bumpAndPruneContacts(ObjectMotionState* motionState) {
     // Find all objects that touch the object corresponding to motionState and flag the other objects
     // for simulation ownership by the local simulation.
 
     assert(motionState);
     btCollisionObject* object = motionState->getRigidBody();
 
+    std::vector<btPersistentManifold*> staleManifolds;
     int numManifolds = _collisionDispatcher->getNumManifolds();
     for (int i = 0; i < numManifolds; ++i) {
         btPersistentManifold* contactManifold =  _collisionDispatcher->getManifoldByIndexInternal(i);
@@ -423,6 +427,7 @@ void PhysicsEngine::bump(ObjectMotionState* motionState) {
                         objectA->setActivationState(ACTIVE_TAG);
                     }
                 }
+                staleManifolds.push_back(contactManifold);
             } else if (objectA == object) {
                 if (!objectB->isStaticOrKinematicObject()) {
                     ObjectMotionState* motionStateB = static_cast<ObjectMotionState*>(objectB->getUserPointer());
@@ -431,9 +436,14 @@ void PhysicsEngine::bump(ObjectMotionState* motionState) {
                         objectB->setActivationState(ACTIVE_TAG);
                     }
                 }
+                staleManifolds.push_back(contactManifold);
             }
         }
     }
+    for (auto manifold : staleManifolds) {
+        _collisionDispatcher->releaseManifold(manifold);
+    }
+    removeContacts(motionState);
 }
 
 void PhysicsEngine::setCharacterController(CharacterController* character) {
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index 16c8456e55..f644d6f5b2 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -78,9 +78,6 @@ public:
     /// \return position of simulation origin in domain-frame
     const glm::vec3& getOriginOffset() const { return _originOffset; }
 
-    /// \brief call bump on any objects that touch the object corresponding to motionState
-    void bump(ObjectMotionState* motionState);
-
     void setCharacterController(CharacterController* character);
 
     void dumpNextStats() { _dumpNextStats = true; }
@@ -94,7 +91,9 @@ public:
 
 private:
     void addObjectToDynamicsWorld(ObjectMotionState* motionState);
-    void removeObjectFromDynamicsWorld(ObjectMotionState* motionState);
+
+    /// \brief bump any objects that touch this one, then remove contact info
+    void bumpAndPruneContacts(ObjectMotionState* motionState);
 
     void removeContacts(ObjectMotionState* motionState);