attempt to remove dynamics associated with a rigid-body before removing the rigid-body

libraries/physics/src/ObjectConstraint.cpp

@@ -19,3 +19,7 @@ ObjectConstraint::ObjectConstraint(EntityDynamicType type, const QUuid& id, Enti
     ObjectDynamic(type, id, ownerEntity)
 {
 }
+
+void ObjectConstraint::invalidate() {
+    _constraint = nullptr;
+}

libraries/physics/src/ObjectConstraint.h

@@ -24,6 +24,7 @@ public:
     virtual btTypedConstraint* getConstraint() = 0;
 
     virtual bool isConstraint() const override { return true; }
+    virtual void invalidate() override;
 
 protected:
     btTypedConstraint* _constraint { nullptr };

libraries/physics/src/ObjectDynamic.h

@@ -33,6 +33,8 @@ public:
     virtual EntityItemWeakPointer getOwnerEntity() const override { return _ownerEntity; }
     virtual void setOwnerEntity(const EntityItemPointer ownerEntity) override { _ownerEntity = ownerEntity; }
 
+    virtual void invalidate() {};
+
     virtual bool updateArguments(QVariantMap arguments) override;
     virtual QVariantMap getArguments() override;
 
@@ -71,4 +73,6 @@ private:
     qint64 getEntityServerClockSkew() const;
 };
 
+typedef std::shared_ptr<ObjectDynamic> ObjectDynamicPointer;
+
 #endif // hifi_ObjectDynamic_h

libraries/physics/src/PhysicsEngine.cpp

@@ -142,6 +142,18 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
     motionState->clearIncomingDirtyFlags();
 }
 
+QList<EntityDynamicPointer> PhysicsEngine::removeDynamicsForBody(btRigidBody* body) {
+    // remove dynamics that are attached to this body
+    QList<EntityDynamicPointer> removedDynamics;
+    QMutableSetIterator<EntityDynamicPointer> i(_objectDynamicsByBody[body]);
+    while (i.hasNext()) {
+        EntityDynamicPointer dynamic = i.next();
+        removeDynamic(dynamic->getID());
+        removedDynamics += dynamic;
+    }
+    return removedDynamics;
+}
+
 void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) {
     // bump and prune contacts for all objects in the list
     for (auto object : objects) {
@@ -175,6 +187,7 @@ void PhysicsEngine::removeObjects(const VectorOfMotionStates& objects) {
     for (auto object : objects) {
         btRigidBody* body = object->getRigidBody();
         if (body) {
+            removeDynamicsForBody(body);
             _dynamicsWorld->removeRigidBody(body);
 
             // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
@@ -191,6 +204,7 @@ void PhysicsEngine::removeObjects(const SetOfMotionStates& objects) {
     for (auto object : objects) {
         btRigidBody* body = object->getRigidBody();
         if (body) {
+            removeDynamicsForBody(body);
             _dynamicsWorld->removeRigidBody(body);
 
             // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
@@ -239,10 +253,17 @@ void PhysicsEngine::reinsertObject(ObjectMotionState* object) {
     bumpAndPruneContacts(object);
     btRigidBody* body = object->getRigidBody();
     if (body) {
+        QList<EntityDynamicPointer> removedDynamics = removeDynamicsForBody(body);
         _dynamicsWorld->removeRigidBody(body);
 
         // add it back
         addObjectToDynamicsWorld(object);
+        foreach(EntityDynamicPointer dynamic, removedDynamics) {
+            bool success = addDynamic(dynamic);
+            if (!success) {
+                qCDebug(physics) << "PhysicsEngine::reinsertObject failed to recreate dynamic";
+            }
+        }
     }
 }
 
@@ -587,7 +608,7 @@ bool PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
     if (success) {
         _objectDynamics[dynamicID] = dynamic;
         foreach(btRigidBody* rigidBody, std::static_pointer_cast<ObjectDynamic>(dynamic)->getRigidBodies()) {
-            _objectDynamicsByBody[rigidBody] = dynamic;
+            _objectDynamicsByBody[rigidBody] += dynamic;
         }
     }
     return success;
@@ -595,7 +616,8 @@ bool PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
 
 void PhysicsEngine::removeDynamic(const QUuid dynamicID) {
     if (_objectDynamics.contains(dynamicID)) {
-        EntityDynamicPointer dynamic = _objectDynamics[dynamicID];
+        ObjectDynamicPointer dynamic = std::static_pointer_cast<ObjectDynamic>(_objectDynamics[dynamicID]);
+        QList<btRigidBody*> rigidBodies = dynamic->getRigidBodies();
         if (dynamic->isAction()) {
             ObjectAction* objectAction = static_cast<ObjectAction*>(dynamic.get());
             _dynamicsWorld->removeAction(objectAction);
@@ -605,10 +627,14 @@ void PhysicsEngine::removeDynamic(const QUuid dynamicID) {
             if (constraint) {
                 _dynamicsWorld->removeConstraint(constraint);
             } else {
-                qDebug() << "PhysicsEngine::removeDynamic of constraint failed"; // XXX
+                qCDebug(physics) << "PhysicsEngine::removeDynamic of constraint failed";
             }
         }
         _objectDynamics.remove(dynamicID);
+        foreach(btRigidBody* rigidBody, rigidBodies) {
+            _objectDynamicsByBody[rigidBody].remove(dynamic);
+        }
+        dynamic->invalidate();
     }
 }
 

libraries/physics/src/PhysicsEngine.h

@@ -91,6 +91,7 @@ public:
     void forEachDynamic(std::function<void(EntityDynamicPointer)> actor);
 
 private:
+    QList<EntityDynamicPointer> removeDynamicsForBody(btRigidBody* body);
     void addObjectToDynamicsWorld(ObjectMotionState* motionState);
     void recursivelyHarvestPerformanceStats(CProfileIterator* profileIterator, QString contextName);
 
@@ -112,7 +113,7 @@ private:
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityDynamicPointer> _objectDynamics;
-    QHash<btRigidBody*, EntityDynamicPointer> _objectDynamicsByBody;
+    QHash<btRigidBody*, QSet<EntityDynamicPointer>> _objectDynamicsByBody;
     std::vector<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;