From b4da2ddd52b67a18997e123af7eaaab6b228ea48 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 5 Feb 2015 09:13:32 -0800
Subject: [PATCH] fix physics crash on disconnect from domainserver

---
 libraries/physics/src/PhysicsEngine.cpp | 15 ++++++++++-----
 libraries/physics/src/PhysicsEngine.h   |  6 +++---
 2 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 7e40d13cad..461d15114c 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -86,7 +86,7 @@ void PhysicsEngine::removeEntityInternal(EntityItem* entity) {
     if (physicsInfo) {
         EntityMotionState* motionState = static_cast<EntityMotionState*>(physicsInfo);
         if (motionState->getRigidBody()) {
-            removeObject(motionState);
+            removeObjectFromBullet(motionState);
         } else {
             // only need to hunt in this list when there is no RigidBody
             _nonPhysicalKinematicObjects.remove(motionState);
@@ -130,7 +130,7 @@ void PhysicsEngine::clearEntitiesInternal() {
     // For now we assume this would only be called on shutdown in which case we can just let the memory get lost.
     QSet<EntityMotionState*>::const_iterator stateItr = _entityMotionStates.begin();
     for (stateItr = _entityMotionStates.begin(); stateItr != _entityMotionStates.end(); ++stateItr) {
-        removeObject(*stateItr);
+        removeObjectFromBullet(*stateItr);
         delete (*stateItr);
     }
     _entityMotionStates.clear();
@@ -211,6 +211,9 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
         if (removeMotionState) {
             // if we get here then there is no need to keep this motionState around (no physics or kinematics)
             _outgoingPackets.remove(motionState);
+            if (motionState->getType() == MOTION_STATE_TYPE_ENTITY) {
+                _entityMotionStates.remove(static_cast<EntityMotionState*>(motionState));
+            }
             // NOTE: motionState will clean up its own backpointers in the Object
             delete motionState;
             continue;
@@ -455,7 +458,7 @@ void PhysicsEngine::addObject(const ShapeInfo& shapeInfo, btCollisionShape* shap
     _dynamicsWorld->addRigidBody(body);
 }
 
-void PhysicsEngine::removeObject(ObjectMotionState* motionState) {
+void PhysicsEngine::removeObjectFromBullet(ObjectMotionState* motionState) {
     assert(motionState);
     btRigidBody* body = motionState->getRigidBody();
     if (body) {
@@ -464,8 +467,9 @@ void PhysicsEngine::removeObject(ObjectMotionState* motionState) {
         ShapeInfoUtil::collectInfoFromShape(shape, shapeInfo);
         _dynamicsWorld->removeRigidBody(body);
         _shapeManager.releaseShape(shapeInfo);
-        delete body;
+        // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
         motionState->setRigidBody(NULL);
+        delete body;
         motionState->setKinematic(false, _numSubsteps);
 
         removeContacts(motionState);
@@ -492,8 +496,9 @@ bool PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
             // FAIL! we are unable to support these changes!
             _shapeManager.releaseShape(oldShape);
 
-            delete body;
+            // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
             motionState->setRigidBody(NULL);
+            delete body;
             motionState->setKinematic(false, _numSubsteps);
             removeContacts(motionState);
             return false;
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index 993b4aeade..2a7baa3ec6 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -79,13 +79,13 @@ public:
     /// \return true if Object added
     void addObject(const ShapeInfo& shapeInfo, btCollisionShape* shape, ObjectMotionState* motionState);
 
-    /// \param motionState pointer to Object's MotionState
-    void removeObject(ObjectMotionState* motionState);
-
     /// process queue of changed from external sources
     void relayIncomingChangesToSimulation();
 
 private:
+    /// \param motionState pointer to Object's MotionState
+    void removeObjectFromBullet(ObjectMotionState* motionState);
+
     void removeContacts(ObjectMotionState* motionState);
 
     // return 'true' of update was successful