allow hinge constraint between two entities

libraries/entities/src/EntityDynamicInterface.h

@@ -42,6 +42,7 @@ public:
     EntityDynamicType getType() const { return _type; }
     virtual bool isAction() const { return false; }
     virtual bool isConstraint() const { return false; }
+    virtual bool isReadyForAdd() const { return true; }
 
     bool isActive() { return _active; }
 

libraries/physics/src/ObjectConstraintHinge.cpp

@@ -11,8 +11,8 @@
 
 #include "QVariantGLM.h"
 
+#include "EntityTree.h"
 #include "ObjectConstraintHinge.h"
-
 #include "PhysicsLogging.h"
 
 
@@ -22,7 +22,7 @@ const uint16_t ObjectConstraintHinge::constraintVersion = 1;
 ObjectConstraintHinge::ObjectConstraintHinge(const QUuid& id, EntityItemPointer ownerEntity) :
     ObjectConstraint(DYNAMIC_TYPE_HINGE, id, ownerEntity),
     _pivotInA(glm::vec3(0.0f)),
-    _axis(glm::vec3(0.0f))
+    _axisInA(glm::vec3(0.0f))
 {
     #if WANT_DEBUG
     qCDebug(physics) << "ObjectConstraintHinge::ObjectConstraintHinge";
@@ -36,8 +36,23 @@ ObjectConstraintHinge::~ObjectConstraintHinge() {
 }
 
 btTypedConstraint* ObjectConstraintHinge::getConstraint() {
-    if (_constraint) {
+    btHingeConstraint* constraint { nullptr };
-        return _constraint;
+    QUuid otherEntityID;
+    glm::vec3 pivotInA;
+    glm::vec3 axisInA;
+    glm::vec3 pivotInB;
+    glm::vec3 axisInB;
+
+    withReadLock([&]{
+        constraint = static_cast<btHingeConstraint*>(_constraint);
+        pivotInA = _pivotInA;
+        axisInA = _axisInA;
+        otherEntityID = _otherEntityID;
+        pivotInB = _pivotInB;
+        axisInB = _axisInB;
+    });
+    if (constraint) {
+        return constraint;
     }
 
     btRigidBody* rigidBodyA = getRigidBody();
@@ -48,22 +63,67 @@ btTypedConstraint* ObjectConstraintHinge::getConstraint() {
 
     bool useReferenceFrameA { false };
 
-    btTransform rigidBodyAFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(_pivotInA));
+    if (!otherEntityID.isNull()) {
+        // This hinge is between two entities... find the other rigid body.
+        EntityItemPointer otherEntity = getEntityByID(otherEntityID);
+        if (!otherEntity) {
+            return nullptr;
+        }
 
-    btHingeConstraint* constraint = new btHingeConstraint(*rigidBodyA, rigidBodyAFrame, useReferenceFrameA);
+        void* otherPhysicsInfo = otherEntity->getPhysicsInfo();
-    // constraint->setAngularOnly(true);
+        if (!otherPhysicsInfo) {
+            return nullptr;
+        }
+        ObjectMotionState* otherMotionState = static_cast<ObjectMotionState*>(otherPhysicsInfo);
+        if (!otherMotionState) {
+            return nullptr;
+        }
+        btRigidBody* rigidBodyB = otherMotionState->getRigidBody();
+        if (!rigidBodyB) {
+            return nullptr;
+        }
 
-    btVector3 axisInA = glmToBullet(_axis);
+        btTransform rigidBodyAFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(pivotInA));
-    constraint->setAxis(axisInA);
+        btTransform rigidBodyBFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(pivotInB));
 
-    _constraint = constraint;
+        constraint = new btHingeConstraint(*rigidBodyA, *rigidBodyB,
-    return constraint;
+                                           rigidBodyAFrame, rigidBodyBFrame,
+                                           useReferenceFrameA);
+
+        btVector3 bulletAxisInA = glmToBullet(axisInA);
+        constraint->setAxis(bulletAxisInA);
+
+        withWriteLock([&]{
+            _otherEntity = otherEntity; // save weak-pointer to the other entity
+            _constraint = constraint;
+        });
+
+        return constraint;
+    } else {
+        // This hinge is between an entity and the world-frame.
+        btTransform rigidBodyAFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(pivotInA));
+
+        constraint = new btHingeConstraint(*rigidBodyA, rigidBodyAFrame, useReferenceFrameA);
+        // constraint->setAngularOnly(true);
+
+        btVector3 bulletAxisInA = glmToBullet(axisInA);
+        constraint->setAxis(bulletAxisInA);
+
+        withWriteLock([&]{
+            _otherEntity.reset();
+            _constraint = constraint;
+        });
+        return constraint;
+    }
 }
 
 
 bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) {
     glm::vec3 pivotInA;
-    glm::vec3 axis;
+    glm::vec3 axisInA;
+    QUuid otherEntityID;
+    glm::vec3 pivotInB;
+    glm::vec3 axisInB;
 
     bool needUpdate = false;
     bool somethingChanged = ObjectDynamic::updateArguments(arguments);
@@ -75,14 +135,35 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) {
         }
 
         ok = true;
-        axis = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "axis", ok, false);
+        axisInA = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "axis", ok, false);
         if (!ok) {
-            axis = _axis;
+            axisInA = _axisInA;
+        }
+
+        ok = true;
+        otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("hinge constraint",
+                                                                            arguments, "otherEntityID", ok, false));
+        if (!ok) {
+            otherEntityID = QUuid();
+        }
+
+        ok = true;
+        pivotInB = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "otherPivot", ok, false);
+        if (!ok) {
+            pivotInB = _pivotInB;
+        }
+
+        ok = true;
+        axisInB = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "otherAxis", ok, false);
+        if (!ok) {
+            axisInB = _axisInB;
         }
 
         if (somethingChanged ||
             pivotInA != _pivotInA ||
-            axis != _axis) {
+            axisInA != _axisInA ||
+            pivotInB != _pivotInB ||
+            axisInB != _axisInB) {
             // something changed
             needUpdate = true;
         }
@@ -91,7 +172,11 @@ bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) {
     if (needUpdate) {
         withWriteLock([&] {
             _pivotInA = pivotInA;
-            _axis = axis;
+            _axisInA = axisInA;
+            _otherEntityID = otherEntityID;
+            _pivotInB = pivotInB;
+            _axisInB = axisInB;
+
             _active = true;
 
             auto ownerEntity = _ownerEntity.lock();
@@ -110,7 +195,10 @@ QVariantMap ObjectConstraintHinge::getArguments() {
     QVariantMap arguments = ObjectDynamic::getArguments();
     withReadLock([&] {
         arguments["pivot"] = glmToQMap(_pivotInA);
-        arguments["axis"] = glmToQMap(_axis);
+        arguments["axis"] = glmToQMap(_axisInA);
+        arguments["otherEntityID"] = _otherEntityID;
+        arguments["otherPivot"] = glmToQMap(_pivotInB);
+        arguments["otherAxis"] = glmToQMap(_axisInB);
     });
     return arguments;
 }
@@ -125,7 +213,10 @@ QByteArray ObjectConstraintHinge::serialize() const {
 
     withReadLock([&] {
         dataStream << _pivotInA;
-        dataStream << _axis;
+        dataStream << _axisInA;
+        dataStream << _otherEntityID;
+        dataStream << _pivotInB;
+        dataStream << _axisInB;
         dataStream << localTimeToServerTime(_expires);
         dataStream << _tag;
     });
@@ -153,7 +244,10 @@ void ObjectConstraintHinge::deserialize(QByteArray serializedArguments) {
 
     withWriteLock([&] {
         dataStream >> _pivotInA;
-        dataStream >> _axis;
+        dataStream >> _axisInA;
+        dataStream >> _otherEntityID;
+        dataStream >> _pivotInB;
+        dataStream >> _axisInB;
 
         quint64 serverExpires;
         dataStream >> serverExpires;

libraries/physics/src/ObjectConstraintHinge.h

@@ -33,7 +33,12 @@ protected:
     static const uint16_t constraintVersion;
 
     glm::vec3 _pivotInA;
-    glm::vec3 _axis;
+    glm::vec3 _axisInA;
+
+    QUuid _otherEntityID;
+    EntityItemWeakPointer _otherEntity;
+    glm::vec3 _pivotInB;
+    glm::vec3 _axisInB;
 };
 
 #endif // hifi_ObjectConstraintHinge_h

libraries/physics/src/ObjectDynamic.cpp

@@ -106,6 +106,20 @@ void ObjectDynamic::removeFromSimulation(EntitySimulationPointer simulation) con
     simulation->removeDynamic(myID);
 }
 
+EntityItemPointer ObjectDynamic::getEntityByID(EntityItemID entityID) const {
+    EntityItemPointer ownerEntity;
+    withReadLock([&]{
+        ownerEntity = _ownerEntity.lock();
+    });
+    EntityTreeElementPointer element = ownerEntity ? ownerEntity->getElement() : nullptr;
+    EntityTreePointer tree = element ? element->getTree() : nullptr;
+    if (!tree) {
+        return nullptr;
+    }
+    return tree->findEntityByID(entityID);
+}
+
+
 btRigidBody* ObjectDynamic::getRigidBody() {
     ObjectMotionState* motionState = nullptr;
     withReadLock([&]{

libraries/physics/src/ObjectDynamic.h

@@ -47,6 +47,7 @@ protected:
     quint64 localTimeToServerTime(quint64 timeValue) const;
     quint64 serverTimeToLocalTime(quint64 timeValue) const;
 
+    EntityItemPointer getEntityByID(EntityItemID entityID) const;
     virtual btRigidBody* getRigidBody();
     virtual glm::vec3 getPosition() override;
     virtual void setPosition(glm::vec3 position) override;

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -346,6 +346,7 @@ void PhysicalEntitySimulation::addDynamic(EntityDynamicPointer dynamic) {
 }
 
 void PhysicalEntitySimulation::applyDynamicChanges() {
+    QList<EntityDynamicPointer> dynamicsFailedToAdd;
     if (_physicsEngine) {
         // FIXME put fine grain locking into _physicsEngine
         QMutexLocker lock(&_mutex);
@@ -354,9 +355,16 @@ void PhysicalEntitySimulation::applyDynamicChanges() {
         }
         foreach (EntityDynamicPointer dynamicToAdd, _dynamicsToAdd) {
             if (!_dynamicsToRemove.contains(dynamicToAdd->getID())) {
-                _physicsEngine->addDynamic(dynamicToAdd);
+                if (!_physicsEngine->addDynamic(dynamicToAdd)) {
+                    dynamicsFailedToAdd += dynamicToAdd;
+                }
             }
         }
     }
+    // applyDynamicChanges will clear _dynamicsToRemove and _dynamicsToAdd
     EntitySimulation::applyDynamicChanges();
+    // put back the ones that couldn't yet be added
+    foreach (EntityDynamicPointer dynamicFailedToAdd, dynamicsFailedToAdd) {
+        addDynamic(dynamicFailedToAdd);
+    }
 }

libraries/physics/src/PhysicsEngine.cpp

@@ -555,12 +555,17 @@ EntityDynamicPointer PhysicsEngine::getDynamicByID(const QUuid& dynamicID) const
     return nullptr;
 }
 
-void PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
+bool PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
     assert(dynamic);
+
+    if (!dynamic->isReadyForAdd()) {
+        return false;
+    }
+
     const QUuid& dynamicID = dynamic->getID();
     if (_objectDynamics.contains(dynamicID)) {
         if (_objectDynamics[dynamicID] == dynamic) {
-            return;
+            return true;
         }
         removeDynamic(dynamic->getID());
     }
@@ -572,13 +577,16 @@ void PhysicsEngine::addDynamic(EntityDynamicPointer dynamic) {
     if (dynamic->isAction()) {
         ObjectAction* objectAction = static_cast<ObjectAction*>(dynamic.get());
         _dynamicsWorld->addAction(objectAction);
+        return true;
     } else if (dynamic->isConstraint()) {
         ObjectConstraint* objectConstraint = static_cast<ObjectConstraint*>(dynamic.get());
         btTypedConstraint* constraint = objectConstraint->getConstraint();
         if (constraint) {
             _dynamicsWorld->addConstraint(constraint);
+            return true;
         } else {
-            qDebug() << "PhysicsEngine::addDynamic of constraint failed"; // XXX
+            // perhaps not all the rigid bodies are available, yet
+            return false;
         }
     }
 }

libraries/physics/src/PhysicsEngine.h

@@ -86,7 +86,7 @@ public:
     void dumpNextStats() { _dumpNextStats = true; }
 
     EntityDynamicPointer getDynamicByID(const QUuid& dynamicID) const;
-    void addDynamic(EntityDynamicPointer dynamic);
+    bool addDynamic(EntityDynamicPointer dynamic);
     void removeDynamic(const QUuid dynamicID);
     void forEachDynamic(std::function<void(EntityDynamicPointer)> actor);