keep track of objectDynamics per rigidBody

libraries/physics/src/ObjectConstraintHinge.cpp

@@ -35,6 +35,19 @@ ObjectConstraintHinge::~ObjectConstraintHinge() {
     #endif
 }
 
+QList<btRigidBody*> ObjectConstraintHinge::getRigidBodies() {
+    QList<btRigidBody*> result;
+    result += getRigidBody();
+    QUuid otherEntityID;
+    withReadLock([&]{
+        otherEntityID = _otherEntityID;
+    });
+    if (!otherEntityID.isNull()) {
+        result += getOtherRigidBody(otherEntityID);
+    }
+    return result;
+}
+
 btTypedConstraint* ObjectConstraintHinge::getConstraint() {
     btHingeConstraint* constraint { nullptr };
     QUuid otherEntityID;
@@ -65,20 +78,7 @@ btTypedConstraint* ObjectConstraintHinge::getConstraint() {
 
     if (!otherEntityID.isNull()) {
         // This hinge is between two entities... find the other rigid body.
-        EntityItemPointer otherEntity = getEntityByID(otherEntityID);
+        btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
-        if (!otherEntity) {
-            return nullptr;
-        }
-
-        void* otherPhysicsInfo = otherEntity->getPhysicsInfo();
-        if (!otherPhysicsInfo) {
-            return nullptr;
-        }
-        ObjectMotionState* otherMotionState = static_cast<ObjectMotionState*>(otherPhysicsInfo);
-        if (!otherMotionState) {
-            return nullptr;
-        }
-        btRigidBody* rigidBodyB = otherMotionState->getRigidBody();
         if (!rigidBodyB) {
             return nullptr;
         }
@@ -94,7 +94,6 @@ btTypedConstraint* ObjectConstraintHinge::getConstraint() {
         constraint->setAxis(bulletAxisInA);
 
         withWriteLock([&]{
-            _otherEntity = otherEntity; // save weak-pointer to the other entity
             _constraint = constraint;
         });
 
@@ -110,7 +109,6 @@ btTypedConstraint* ObjectConstraintHinge::getConstraint() {
         constraint->setAxis(bulletAxisInA);
 
         withWriteLock([&]{
-            _otherEntity.reset();
             _constraint = constraint;
         });
         return constraint;

libraries/physics/src/ObjectConstraintHinge.h

@@ -27,6 +27,7 @@ public:
     virtual QByteArray serialize() const override;
     virtual void deserialize(QByteArray serializedArguments) override;
 
+    virtual QList<btRigidBody*> getRigidBodies() override;
     virtual btTypedConstraint* getConstraint() override;
 
 protected:
@@ -35,8 +36,7 @@ protected:
     glm::vec3 _pivotInA;
     glm::vec3 _axisInA;
 
-    QUuid _otherEntityID;
+    EntityItemID _otherEntityID;
-    EntityItemWeakPointer _otherEntity;
     glm::vec3 _pivotInB;
     glm::vec3 _axisInB;
 };

libraries/physics/src/ObjectDynamic.cpp

@@ -271,3 +271,28 @@ quint64 ObjectDynamic::serverTimeToLocalTime(quint64 timeValue) const {
 
     return timeValue - serverClockSkew;
 }
+
+btRigidBody* ObjectDynamic::getOtherRigidBody(EntityItemID otherEntityID) {
+    EntityItemPointer otherEntity = getEntityByID(otherEntityID);
+    if (!otherEntity) {
+        return nullptr;
+    }
+
+    void* otherPhysicsInfo = otherEntity->getPhysicsInfo();
+    if (!otherPhysicsInfo) {
+        return nullptr;
+    }
+
+    ObjectMotionState* otherMotionState = static_cast<ObjectMotionState*>(otherPhysicsInfo);
+    if (!otherMotionState) {
+        return nullptr;
+    }
+
+    return otherMotionState->getRigidBody();
+}
+
+QList<btRigidBody*> ObjectDynamic::getRigidBodies() {
+    QList<btRigidBody*> result;
+    result += getRigidBody();
+    return result;
+}

libraries/physics/src/ObjectDynamic.h

@@ -43,10 +43,13 @@ public:
     virtual bool lifetimeIsOver() override;
     virtual quint64 getExpires() override { return _expires; }
 
+    virtual QList<btRigidBody*> getRigidBodies();
+
 protected:
     quint64 localTimeToServerTime(quint64 timeValue) const;
     quint64 serverTimeToLocalTime(quint64 timeValue) const;
 
+    btRigidBody* getOtherRigidBody(EntityItemID otherEntityID);
     EntityItemPointer getEntityByID(EntityItemID entityID) const;
     virtual btRigidBody* getRigidBody();
     virtual glm::vec3 getPosition() override;

libraries/physics/src/PhysicsEngine.cpp

@@ -570,25 +570,27 @@ bool PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
         removeDynamic(dynamic->getID());
     }
 
-    _objectDynamics[dynamicID] = dynamic;
+    bool success { false };
-
-    // bullet needs a pointer to the dynamic, but it doesn't use shared pointers.
-    // is there a way to bump the reference count?
     if (dynamic->isAction()) {
         ObjectAction* objectAction = static_cast<ObjectAction*>(dynamic.get());
         _dynamicsWorld->addAction(objectAction);
-        return true;
+        success = true;
     } else if (dynamic->isConstraint()) {
         ObjectConstraint* objectConstraint = static_cast<ObjectConstraint*>(dynamic.get());
         btTypedConstraint* constraint = objectConstraint->getConstraint();
         if (constraint) {
             _dynamicsWorld->addConstraint(constraint);
-            return true;
+            success = true;
-        } else {
+        } // else perhaps not all the rigid bodies are available, yet
-            // perhaps not all the rigid bodies are available, yet
+    }
-            return false;
+
+    if (success) {
+        _objectDynamics[dynamicID] = dynamic;
+        foreach(btRigidBody* rigidBody, std::static_pointer_cast<ObjectDynamic>(dynamic)->getRigidBodies()) {
+            _objectDynamicsByBody[rigidBody] = dynamic;
         }
     }
+    return success;
 }
 
 void PhysicsEngine::removeDynamic(const QUuid dynamicID) {

libraries/physics/src/PhysicsEngine.h

@@ -112,6 +112,7 @@ private:
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityDynamicPointer> _objectDynamics;
+    QHash<btRigidBody*, EntityDynamicPointer> _objectDynamicsByBody;
     std::vector<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;
@@ -123,6 +124,7 @@ private:
 
     bool _dumpNextStats = false;
     bool _hasOutgoingChanges = false;
+
 };
 
 typedef std::shared_ptr<PhysicsEngine> PhysicsEnginePointer;