From 6e27b4d1c5946fd946a394a5dd8e4371349d5fd1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 2 May 2019 15:40:26 -0700
Subject: [PATCH] remove bodies of deleted entities

---
 .../physics/src/PhysicalEntitySimulation.cpp  | 31 ++++++++++++++++---
 1 file changed, 26 insertions(+), 5 deletions(-)

diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp
index f1f356cef7..4e73b6f966 100644
--- a/libraries/physics/src/PhysicalEntitySimulation.cpp
+++ b/libraries/physics/src/PhysicalEntitySimulation.cpp
@@ -344,9 +344,29 @@ void PhysicalEntitySimulation::buildMotionStatesForEntitiesThatNeedThem() {
 
 void PhysicalEntitySimulation::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
     QMutexLocker lock(&_mutex);
+    // entities being removed
+    for (auto entity : _entitiesToRemoveFromPhysics) {
+        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
+        if (motionState) {
+            transaction.objectsToRemove.push_back(motionState);
+            _incomingChanges.remove(motionState);
+        }
+        if (_shapeRequests.size() > 0) {
+            ShapeRequest shapeRequest(entity);
+            ShapeRequests::iterator  requestItr = _shapeRequests.find(shapeRequest);
+            if (requestItr == _shapeRequests.end()) {
+                _shapeRequests.erase(requestItr);
+            }
+        }
+    }
+    _entitiesToRemoveFromPhysics.clear();
+
+    // entities to add
     buildMotionStatesForEntitiesThatNeedThem();
+
+    // motionStates with changed entities: delete, add, or change
     for (auto& object : _incomingChanges) {
-        uint32_t flags = object->getIncomingDirtyFlags();
+        uint32_t unhandledFlags = object->getIncomingDirtyFlags();
 
         uint32_t handledFlags = EASY_DIRTY_PHYSICS_FLAGS;
         bool isInPhysicsSimulation = object->isInPhysicsSimulation();
@@ -402,16 +422,17 @@ void PhysicalEntitySimulation::buildPhysicsTransaction(PhysicsEngine::Transactio
             } else {
                 transaction.objectsToAdd.push_back(object);
                 handledFlags = DIRTY_PHYSICS_FLAGS;
+                unhandledFlags = 0;
             }
         }
 
-        if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-            object->handleEasyChanges(flags);
+        if (unhandledFlags & EASY_DIRTY_PHYSICS_FLAGS) {
+            object->handleEasyChanges(unhandledFlags);
         }
-        if ((flags & (Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_COLLISION_GROUP)) || (handledFlags & Simulation::DIRTY_SHAPE)) {
+        if (unhandledFlags & (Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_COLLISION_GROUP | (handledFlags & Simulation::DIRTY_SHAPE))) {
             transaction.objectsToReinsert.push_back(object);
             handledFlags |= (Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_COLLISION_GROUP);
-        } else if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && object->getRigidBody()->isStaticObject()) {
+        } else if (unhandledFlags & Simulation::DIRTY_PHYSICS_ACTIVATION && object->getRigidBody()->isStaticObject()) {
             transaction.activeStaticObjects.push_back(object);
         }
         object->clearIncomingDirtyFlags(handledFlags);