slightly simpler EntityMotionState delete logic

interface/src/Application.cpp

@@ -5274,11 +5274,13 @@ void Application::update(float deltaTime) {
             {
                 PROFILE_RANGE(simulation_physics, "PreStep");
                 PerformanceTimer perfTimer("preStep)");
-                static VectorOfMotionStates motionStates;
+                {
-                _entitySimulation->getObjectsToRemoveFromPhysics(motionStates);
+                    const VectorOfMotionStates& motionStates = _entitySimulation->getObjectsToRemoveFromPhysics();
-                _physicsEngine->removeObjects(motionStates);
+                    _physicsEngine->removeObjects(motionStates);
-                _entitySimulation->deleteObjectsRemovedFromPhysics();
+                    _entitySimulation->deleteObjectsRemovedFromPhysics();
+                }
 
+                VectorOfMotionStates motionStates;
                 getEntities()->getTree()->withReadLock([&] {
                     _entitySimulation->getObjectsToAddToPhysics(motionStates);
                     _physicsEngine->addObjects(motionStates);
@@ -5292,7 +5294,7 @@ void Application::update(float deltaTime) {
 
                 _entitySimulation->applyDynamicChanges();
 
-                 avatarManager->getObjectsToRemoveFromPhysics(motionStates);
+                avatarManager->getObjectsToRemoveFromPhysics(motionStates);
                 _physicsEngine->removeObjects(motionStates);
                 avatarManager->getObjectsToAddToPhysics(motionStates);
                 _physicsEngine->addObjects(motionStates);

libraries/physics/src/EntityMotionState.cpp

@@ -49,8 +49,7 @@ bool entityTreeIsLocked() {
 
 EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer entity) :
     ObjectMotionState(nullptr),
-    _entityPtr(entity),
+    _entity(entity),
-    _entity(entity.get()),
     _serverPosition(0.0f),
     _serverRotation(),
     _serverVelocity(0.0f),
@@ -80,8 +79,11 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer
 }
 
 EntityMotionState::~EntityMotionState() {
-    assert(_entity);
+    if (_entity) {
-    _entity = nullptr;
+        assert(_entity->getPhysicsInfo() == this);
+        _entity->setPhysicsInfo(nullptr);
+        _entity.reset();
+    }
 }
 
 void EntityMotionState::updateServerPhysicsVariables() {

libraries/physics/src/EntityMotionState.h

@@ -70,7 +70,7 @@ public:
     virtual QUuid getSimulatorID() const override;
     virtual void bump(uint8_t priority) override;
 
-    EntityItemPointer getEntity() const { return _entityPtr.lock(); }
+    const EntityItemPointer& getEntity() const { return _entity; }
 
     void resetMeasuredBodyAcceleration();
     void measureBodyAcceleration();
@@ -104,9 +104,7 @@ protected:
     // recursive dependency).  Instead we keep a EntityItemWeakPointer to break that dependency while
     // still granting us the capability to generate EntityItemPointers as necessary (for external data
     // structures that use the MotionState to get to the EntityItem).
-    EntityItemWeakPointer _entityPtr;
+    EntityItemPointer _entity;
-    // Meanwhile we also keep a raw EntityItem* for internal stuff where the pointer is guaranteed valid.
-    EntityItem* _entity;
 
     bool _serverVariablesSet { false };
     glm::vec3 _serverPosition;    // in simulation-frame (not world-frame)

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -137,7 +137,6 @@ void PhysicalEntitySimulation::clearEntitiesInternal() {
 
     // clear all other lists specific to this derived class
     _entitiesToRemoveFromPhysics.clear();
-    _entitiesToForget.clear();
     _entitiesToAddToPhysics.clear();
     _pendingChanges.clear();
     _outgoingChanges.clear();
@@ -153,42 +152,37 @@ void PhysicalEntitySimulation::prepareEntityForDelete(EntityItemPointer entity)
 }
 // end EntitySimulation overrides
 
-void PhysicalEntitySimulation::getObjectsToRemoveFromPhysics(VectorOfMotionStates& result) {
+const VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToRemoveFromPhysics() {
-    result.clear();
     QMutexLocker lock(&_mutex);
     for (auto entity: _entitiesToRemoveFromPhysics) {
-        // make sure it isn't on any side lists
-        _entitiesToAddToPhysics.remove(entity);
-
         EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
-        if (motionState) {
+        assert(motionState);
-            _pendingChanges.remove(motionState);
+
-            _outgoingChanges.remove(motionState);
+        _entitiesToAddToPhysics.remove(entity);
-            _physicalObjects.remove(motionState);
+        if (entity->isDead() && entity->getElement()) {
-            result.push_back(motionState);
-            _entitiesToForget.insert(entity);
-        } else if (entity->isDead() && entity->getElement()) {
             _deadEntities.insert(entity);
         }
+
+        _pendingChanges.remove(motionState);
+        _outgoingChanges.remove(motionState);
+        _physicalObjects.remove(motionState);
+
+        // remember this motionState and delete it later (after removing its RigidBody from the PhysicsEngine)
+        _objectsToDelete.push_back(motionState);
     }
     _entitiesToRemoveFromPhysics.clear();
+    return _objectsToDelete;
 }
 
 void PhysicalEntitySimulation::deleteObjectsRemovedFromPhysics() {
     QMutexLocker lock(&_mutex);
-    for (auto entity: _entitiesToForget) {
+    for (auto motionState : _objectsToDelete) {
-        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
-        assert(motionState);
-        entity->setPhysicsInfo(nullptr);
         // someday when we invert the entities/physics lib dependencies we can let EntityItem delete its own PhysicsInfo
         // until then we must do it here
+        // NOTE: a reference to the EntityItemPointer is released in the EntityMotionState::dtor
         delete motionState;
-
-        if (entity->isDead() && entity->getElement()) {
-            _deadEntities.insert(entity);
-        }
     }
-    _entitiesToForget.clear();
+    _objectsToDelete.clear();
 }
 
 void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& result) {

libraries/physics/src/PhysicalEntitySimulation.h

@@ -54,8 +54,9 @@ protected: // only called by EntitySimulation
 public:
     virtual void prepareEntityForDelete(EntityItemPointer entity) override;
 
-    void getObjectsToRemoveFromPhysics(VectorOfMotionStates& result);
+    const VectorOfMotionStates& getObjectsToRemoveFromPhysics();
     void deleteObjectsRemovedFromPhysics();
+
     void getObjectsToAddToPhysics(VectorOfMotionStates& result);
     void setObjectsToChange(const VectorOfMotionStates& objectsToChange);
     void getObjectsToChange(VectorOfMotionStates& result);
@@ -67,9 +68,10 @@ public:
     EntityEditPacketSender* getPacketSender() { return _entityPacketSender; }
 
 private:
-    SetOfEntities _entitiesToRemoveFromPhysics;
-    SetOfEntities _entitiesToForget;
     SetOfEntities _entitiesToAddToPhysics;
+    SetOfEntities _entitiesToRemoveFromPhysics;
+
+    VectorOfMotionStates _objectsToDelete;
 
     SetOfEntityMotionStates _pendingChanges; // EntityMotionStates already in PhysicsEngine that need their physics changed
     SetOfEntityMotionStates _outgoingChanges; // EntityMotionStates for which we may need to send updates to entity-server