diff --git a/libraries/entities/src/EntityDynamicInterface.h b/libraries/entities/src/EntityDynamicInterface.h index f6dba84fc5..f4b7efc7a6 100644 --- a/libraries/entities/src/EntityDynamicInterface.h +++ b/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; } diff --git a/libraries/physics/src/ObjectConstraintHinge.cpp b/libraries/physics/src/ObjectConstraintHinge.cpp index ec27ead647..b6c19e96ce 100644 --- a/libraries/physics/src/ObjectConstraintHinge.cpp +++ b/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) { - return _constraint; + btHingeConstraint* constraint { nullptr }; + QUuid otherEntityID; + glm::vec3 pivotInA; + glm::vec3 axisInA; + glm::vec3 pivotInB; + glm::vec3 axisInB; + + withReadLock([&]{ + constraint = static_cast(_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); - // constraint->setAngularOnly(true); + void* otherPhysicsInfo = otherEntity->getPhysicsInfo(); + if (!otherPhysicsInfo) { + return nullptr; + } + ObjectMotionState* otherMotionState = static_cast(otherPhysicsInfo); + if (!otherMotionState) { + return nullptr; + } + btRigidBody* rigidBodyB = otherMotionState->getRigidBody(); + if (!rigidBodyB) { + return nullptr; + } - btVector3 axisInA = glmToBullet(_axis); - constraint->setAxis(axisInA); + btTransform rigidBodyAFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(pivotInA)); + btTransform rigidBodyBFrame(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f), glmToBullet(pivotInB)); - _constraint = constraint; - return constraint; + constraint = new btHingeConstraint(*rigidBodyA, *rigidBodyB, + 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; diff --git a/libraries/physics/src/ObjectConstraintHinge.h b/libraries/physics/src/ObjectConstraintHinge.h index ba451937ca..7823b85758 100644 --- a/libraries/physics/src/ObjectConstraintHinge.h +++ b/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 diff --git a/libraries/physics/src/ObjectDynamic.cpp b/libraries/physics/src/ObjectDynamic.cpp index f887cc42dc..4831c44705 100644 --- a/libraries/physics/src/ObjectDynamic.cpp +++ b/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([&]{ diff --git a/libraries/physics/src/ObjectDynamic.h b/libraries/physics/src/ObjectDynamic.h index 3f81cbc037..1ffc13239c 100644 --- a/libraries/physics/src/ObjectDynamic.h +++ b/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; diff --git a/libraries/physics/src/PhysicalEntitySimulation.cpp b/libraries/physics/src/PhysicalEntitySimulation.cpp index 3ba1744abc..5081f981d4 100644 --- a/libraries/physics/src/PhysicalEntitySimulation.cpp +++ b/libraries/physics/src/PhysicalEntitySimulation.cpp @@ -346,6 +346,7 @@ void PhysicalEntitySimulation::addDynamic(EntityDynamicPointer dynamic) { } void PhysicalEntitySimulation::applyDynamicChanges() { + QList 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); + } } diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index fd82b05fa5..3cd6abb839 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/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(dynamic.get()); _dynamicsWorld->addAction(objectAction); + return true; } else if (dynamic->isConstraint()) { ObjectConstraint* objectConstraint = static_cast(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; } } } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index ec36286dd8..d9f70efc3e 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/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 actor);