From f5a5369055f53a50f71436e62d07c21638f67356 Mon Sep 17 00:00:00 2001 From: Seth Alves Date: Fri, 14 Apr 2017 07:06:53 -0700 Subject: [PATCH] attempt to remove dynamics associated with a rigid-body before removing the rigid-body --- libraries/physics/src/ObjectConstraint.cpp | 4 +++ libraries/physics/src/ObjectConstraint.h | 1 + libraries/physics/src/ObjectDynamic.h | 4 +++ libraries/physics/src/PhysicsEngine.cpp | 32 ++++++++++++++++++++-- libraries/physics/src/PhysicsEngine.h | 3 +- 5 files changed, 40 insertions(+), 4 deletions(-) diff --git a/libraries/physics/src/ObjectConstraint.cpp b/libraries/physics/src/ObjectConstraint.cpp index e8f1a6d9fd..54fd4777e0 100644 --- a/libraries/physics/src/ObjectConstraint.cpp +++ b/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; +} diff --git a/libraries/physics/src/ObjectConstraint.h b/libraries/physics/src/ObjectConstraint.h index 9136b5b38c..13aa871d98 100644 --- a/libraries/physics/src/ObjectConstraint.h +++ b/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 }; diff --git a/libraries/physics/src/ObjectDynamic.h b/libraries/physics/src/ObjectDynamic.h index c8e809cfe1..6ea90bdba0 100644 --- a/libraries/physics/src/ObjectDynamic.h +++ b/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 ObjectDynamicPointer; + #endif // hifi_ObjectDynamic_h diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp index 8a9a5edfb2..432f4287ea 100644 --- a/libraries/physics/src/PhysicsEngine.cpp +++ b/libraries/physics/src/PhysicsEngine.cpp @@ -142,6 +142,18 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) { motionState->clearIncomingDirtyFlags(); } +QList PhysicsEngine::removeDynamicsForBody(btRigidBody* body) { + // remove dynamics that are attached to this body + QList removedDynamics; + QMutableSetIterator 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 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(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(_objectDynamics[dynamicID]); + QList rigidBodies = dynamic->getRigidBodies(); if (dynamic->isAction()) { ObjectAction* objectAction = static_cast(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(); } } diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h index ba30375e73..33d60997a8 100644 --- a/libraries/physics/src/PhysicsEngine.h +++ b/libraries/physics/src/PhysicsEngine.h @@ -91,6 +91,7 @@ public: void forEachDynamic(std::function actor); private: + QList removeDynamicsForBody(btRigidBody* body); void addObjectToDynamicsWorld(ObjectMotionState* motionState); void recursivelyHarvestPerformanceStats(CProfileIterator* profileIterator, QString contextName); @@ -112,7 +113,7 @@ private: ContactMap _contactMap; CollisionEvents _collisionEvents; QHash _objectDynamics; - QHash _objectDynamicsByBody; + QHash> _objectDynamicsByBody; std::vector _activeStaticBodies; glm::vec3 _originOffset;