From eb700cd7eb1162ca8c701507ec0725b25341ce8d Mon Sep 17 00:00:00 2001 From: Alexander Clegg Date: Wed, 4 Mar 2020 11:03:56 -0800 Subject: [PATCH] Attach SceneNode to simulation (#492) * Add SceneNode attachment option to addObject and makeRigidObject. * Added RigidObject::visualNode_ to clearly separate Drawable branch of a physical object's SceneNode hierarchy. * Enabled optional deletion of visualNode_ and object SceneNode hierarchy when removing objects from PhysicsManager. * Added python bindings and unit tests in python and C++. --- habitat_sim/simulator.py | 15 ++++-- src/esp/assets/ResourceManager.cpp | 2 - src/esp/bindings/SimBindings.cpp | 10 ++-- src/esp/physics/PhysicsManager.cpp | 43 +++++++++++----- src/esp/physics/PhysicsManager.h | 23 +++++++-- src/esp/physics/RigidObject.cpp | 10 ++-- src/esp/physics/RigidObject.h | 5 ++ .../physics/bullet/BulletPhysicsManager.cpp | 20 ++++++-- src/esp/physics/bullet/BulletPhysicsManager.h | 26 ++++++++-- src/esp/physics/bullet/BulletRigidObject.cpp | 6 +-- src/esp/sim/Simulator.cpp | 15 +++--- src/esp/sim/Simulator.h | 11 +++-- src/tests/PhysicsTest.cpp | 49 +++++++++++++++++++ src/tests/SimTest.cpp | 8 +-- tests/test_physics.py | 10 ++++ 15 files changed, 190 insertions(+), 63 deletions(-) diff --git a/habitat_sim/simulator.py b/habitat_sim/simulator.py index a837947622..fea0a82be6 100644 --- a/habitat_sim/simulator.py +++ b/habitat_sim/simulator.py @@ -274,14 +274,21 @@ def __del__(self): self.close() # --- physics functions --- - def add_object(self, object_lib_index, light_setup_key=DEFAULT_LIGHTING_KEY): - return self._sim.add_object(object_lib_index, light_setup_key=light_setup_key) + def add_object( + self, + object_lib_index, + attachment_node=None, + light_setup_key=DEFAULT_LIGHTING_KEY, + ): + return self._sim.add_object(object_lib_index, attachment_node, light_setup_key) def get_physics_object_library_size(self): return self._sim.get_physics_object_library_size() - def remove_object(self, object_id): - self._sim.remove_object(object_id) + def remove_object( + self, object_id, delete_object_node=True, delete_visual_node=True + ): + self._sim.remove_object(object_id, delete_object_node, delete_visual_node) def get_existing_object_ids(self, scene_id=0): return self._sim.get_existing_object_ids(scene_id) diff --git a/src/esp/assets/ResourceManager.cpp b/src/esp/assets/ResourceManager.cpp index 9cd7d91309..a3ea05a595 100644 --- a/src/esp/assets/ResourceManager.cpp +++ b/src/esp/assets/ResourceManager.cpp @@ -444,8 +444,6 @@ int ResourceManager::loadObject(const std::string& objPhysConfigFilename, addComponent(meshMetaData, scalingNode, lightSetup, drawables, meshMetaData.root); - // compute the full BB hierarchy for the new tree. - parent->computeCumulativeBB(); } return objectID; diff --git a/src/esp/bindings/SimBindings.cpp b/src/esp/bindings/SimBindings.cpp index ed1a2556e0..cc5e4e02b7 100644 --- a/src/esp/bindings/SimBindings.cpp +++ b/src/esp/bindings/SimBindings.cpp @@ -72,16 +72,12 @@ void initSimBindings(py::module& m) { &Simulator::setFrustumCullingEnabled, R"(Enable or disable the frustum culling)") /* --- Physics functions --- */ - .def("add_object", py::overload_cast(&Simulator::addObject), - "object_lib_index"_a, "scene_id"_a = 0) - .def("add_object", - py::overload_cast( - &Simulator::addObject), - "object_lib_index"_a, "light_setup_key"_a, "scene_id"_a = 0) + .def("add_object", &Simulator::addObject, "object_lib_index"_a, + "attachment_node"_a, "light_setup_key"_a, "scene_id"_a = 0) .def("get_physics_object_library_size", &Simulator::getPhysicsObjectLibrarySize) .def("remove_object", &Simulator::removeObject, "object_id"_a, - "sceneID"_a = 0) + "delete_object_node"_a, "delete_visual_node"_a, "sceneID"_a = 0) .def("get_object_motion_type", &Simulator::getObjectMotionType, "object_id"_a, "sceneID"_a = 0) .def("set_object_motion_type", &Simulator::setObjectMotionType, diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp index ab0372612d..54091b6a72 100644 --- a/src/esp/physics/PhysicsManager.cpp +++ b/src/esp/physics/PhysicsManager.cpp @@ -48,6 +48,7 @@ bool PhysicsManager::addScene( int PhysicsManager::addObject(const int objectLibIndex, DrawableGroup* drawables, + scene::SceneNode* attachmentNode, const Magnum::ResourceKey& lightSetup) { const std::string configFile = resourceManager_->getObjectConfig(objectLibIndex); @@ -59,7 +60,8 @@ int PhysicsManager::addObject(const int objectLibIndex, resourceManager_->getPhysicsObjectAttributes(configFile); //! Instantiate with mesh pointer - int nextObjectID_ = makeRigidObject(meshGroup, physicsObjectAttributes); + int nextObjectID_ = + makeRigidObject(meshGroup, physicsObjectAttributes, attachmentNode); if (nextObjectID_ < 0) { LOG(ERROR) << "makeRigidObject unsuccessful"; return ID_UNDEFINED; @@ -68,8 +70,9 @@ int PhysicsManager::addObject(const int objectLibIndex, //! Draw object via resource manager //! Render node as child of physics node resourceManager_->loadObject(configFile, - &existingObjects_.at(nextObjectID_)->node(), + existingObjects_.at(nextObjectID_)->visualNode_, drawables, lightSetup); + existingObjects_.at(nextObjectID_)->node().computeCumulativeBB(); if (physicsObjectAttributes.existsAs(assets::DataType::BOOL, "COM_provided")) { @@ -88,21 +91,27 @@ int PhysicsManager::addObject(const int objectLibIndex, int PhysicsManager::addObject(const std::string& configFile, DrawableGroup* drawables, + scene::SceneNode* attachmentNode, const Magnum::ResourceKey& lightSetup) { int resObjectID = resourceManager_->getObjectID(configFile); //! Invoke resourceManager to draw object - int physObjectID = addObject(resObjectID, drawables, lightSetup); + int physObjectID = + addObject(resObjectID, drawables, attachmentNode, lightSetup); return physObjectID; } void PhysicsManager::removeObject(const int physObjectID, - bool deleteSceneNode) { + bool deleteObjectNode, + bool deleteVisualNode) { assertIDValidity(physObjectID); scene::SceneNode* objectNode = &existingObjects_.at(physObjectID)->node(); + scene::SceneNode* visualNode = existingObjects_.at(physObjectID)->visualNode_; existingObjects_.erase(physObjectID); deallocateObjectID(physObjectID); - if (deleteSceneNode) { + if (deleteObjectNode) { delete objectNode; + } else if (deleteVisualNode && visualNode) { + delete visualNode; } } @@ -135,11 +144,15 @@ int PhysicsManager::deallocateObjectID(int physObjectID) { //! Create and initialize rigid object int PhysicsManager::makeRigidObject( const std::vector& meshGroup, - assets::PhysicsObjectAttributes physicsObjectAttributes) { + assets::PhysicsObjectAttributes physicsObjectAttributes, + scene::SceneNode* attachmentNode /* = nullptr */) { int newObjectID = allocateObjectID(); - scene::SceneNode& newNode = staticSceneObject_->node().createChild(); + scene::SceneNode* objectNode = attachmentNode; + if (attachmentNode == nullptr) { + objectNode = &staticSceneObject_->node().createChild(); + } existingObjects_[newObjectID] = - std::make_unique(&newNode); + std::make_unique(objectNode); //! Instantiate with mesh pointer bool objectSuccess = @@ -148,7 +161,8 @@ int PhysicsManager::makeRigidObject( if (!objectSuccess) { deallocateObjectID(newObjectID); existingObjects_.erase(newObjectID); - delete &newNode; + if (attachmentNode == nullptr) + delete objectNode; return ID_UNDEFINED; } return newObjectID; @@ -459,15 +473,18 @@ void PhysicsManager::setObjectBBDraw(int physObjectID, // destroy the node delete existingObjects_[physObjectID]->BBNode_; existingObjects_[physObjectID]->BBNode_ = nullptr; - } else if (drawBB) { + } else if (drawBB && existingObjects_[physObjectID]->visualNode_) { // add a new BBNode Magnum::Vector3 scale = - existingObjects_[physObjectID]->node().getCumulativeBB().size() / 2.0; + existingObjects_[physObjectID]->visualNode_->getCumulativeBB().size() / + 2.0; existingObjects_[physObjectID]->BBNode_ = - &existingObjects_[physObjectID]->node().createChild(); + &existingObjects_[physObjectID]->visualNode_->createChild(); existingObjects_[physObjectID]->BBNode_->MagnumObject::setScaling(scale); existingObjects_[physObjectID]->BBNode_->MagnumObject::setTranslation( - existingObjects_[physObjectID]->node().getCumulativeBB().center()); + existingObjects_[physObjectID] + ->visualNode_->getCumulativeBB() + .center()); resourceManager_->addPrimitiveToDrawables( 0, *existingObjects_[physObjectID]->BBNode_, drawables); } diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index cc66d18762..e3f0b10c82 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -143,28 +143,33 @@ class PhysicsManager { * esp::assets::ResourceManager::physicsObjectLibrary_ * @param drawables Reference to the scene graph drawables group to enable * rendering of the newly initialized object. + * @param attachmentNode If supplied, attach the new physical object to an + * existing SceneNode. * @return the instanced object's ID, mapping to it in @ref * PhysicsManager::existingObjects_ if successful, or @ref esp::ID_UNDEFINED. */ int addObject(const std::string& configFile, DrawableGroup* drawables, + scene::SceneNode* attachmentNode = nullptr, const Magnum::ResourceKey& lightSetup = Magnum::ResourceKey{ assets::ResourceManager::DEFAULT_LIGHTING_KEY}); /** @brief Instance a physical object from an object properties template in * the @ref esp::assets::ResourceManager::physicsObjectLibrary_ by object - * library index. Queries the properties filename and calls @ref - * addObject(const std::string& configFile, DrawableGroup* drawables). + * library index. * @param objectLibIndex The index of the object's template in @ref * esp::assets::ResourceManager::physicsObjectLibrary_ * @param drawables Reference to the scene graph drawables group to enable * rendering of the newly initialized object. + * @param attachmentNode If supplied, attach the new physical object to an + * existing SceneNode. * @return the instanced object's ID, mapping to it in @ref * PhysicsManager::existingObjects_ if successful, or @ref esp::ID_UNDEFINED. */ virtual int addObject( const int objectLibIndex, DrawableGroup* drawables, + scene::SceneNode* attachmentNode = nullptr, const Magnum::ResourceKey& lightSetup = Magnum::ResourceKey{ assets::ResourceManager::DEFAULT_LIGHTING_KEY}); @@ -173,11 +178,15 @@ class PhysicsManager { * PhysicsManager::existingObjects_. * @param physObjectID The ID (key) of the object instance in @ref * PhysicsManager::existingObjects_. - * @param deleteSceneNode If true, deletes the object's scene node. Otherwise + * @param deleteObjectNode If true, deletes the object's scene node. Otherwise * detaches the object from simulation. + * @param deleteVisualNode If true, deletes the object's visual node. + * Otherwise detaches the object from simulation. Is not considered if + * deleteObjectNode==true. */ virtual void removeObject(const int physObjectID, - bool deleteSceneNode = true); + bool deleteObjectNode = true, + bool deleteVisualNode = true); /** @brief Get the number of objects mapped in @ref * PhysicsManager::existingObjects_. @@ -820,10 +829,14 @@ class PhysicsManager { * @param meshGroup The object's mesh. * @param physicsObjectAttributes The physical object's template defining its * physical parameters. + * @param attachmentNode If supplied, attach the new physical object to an + * existing SceneNode. + * @return The id of the newly allocated object in @ref existingObjects_ */ virtual int makeRigidObject( const std::vector& meshGroup, - assets::PhysicsObjectAttributes physicsObjectAttributes); + assets::PhysicsObjectAttributes physicsObjectAttributes, + scene::SceneNode* attachmentNode = nullptr); /** @brief A pointer to a @ref esp::assets::ResourceManager which holds assets * that can be accessed by this @ref PhysicsManager*/ diff --git a/src/esp/physics/RigidObject.cpp b/src/esp/physics/RigidObject.cpp index 7f5ddb0bcf..67417c0042 100644 --- a/src/esp/physics/RigidObject.cpp +++ b/src/esp/physics/RigidObject.cpp @@ -9,7 +9,8 @@ namespace esp { namespace physics { RigidObject::RigidObject(scene::SceneNode* rigidBodyNode) - : Magnum::SceneGraph::AbstractFeature3D(*rigidBodyNode) {} + : Magnum::SceneGraph::AbstractFeature3D(*rigidBodyNode), + visualNode_(&rigidBodyNode->createChild()) {} bool RigidObject::initializeScene( const assets::PhysicsSceneAttributes&, @@ -64,10 +65,9 @@ bool RigidObject::setMotionType(MotionType mt) { } void RigidObject::shiftOrigin(const Magnum::Vector3& shift) { - // shift each child node - for (auto& child : node().children()) { - child.translate(shift); - } + // shift visual components + if (visualNode_) + visualNode_->translate(shift); node().computeCumulativeBB(); } diff --git a/src/esp/physics/RigidObject.h b/src/esp/physics/RigidObject.h index f12a76e47a..a5ad292e34 100644 --- a/src/esp/physics/RigidObject.h +++ b/src/esp/physics/RigidObject.h @@ -507,6 +507,11 @@ class RigidObject : public Magnum::SceneGraph::AbstractFeature3D { //! drawing is off. See @ref toggleBBDraw(). scene::SceneNode* BBNode_ = nullptr; + /** + * @brief All Drawable components are children of this node. + */ + scene::SceneNode* visualNode_ = nullptr; + protected: /** @brief The @ref MotionType of the object. Determines what operations can * be performed on this object. */ diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index ba426797f6..8327316470 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -75,11 +75,18 @@ bool BulletPhysicsManager::addScene( int BulletPhysicsManager::makeRigidObject( const std::vector& meshGroup, - assets::PhysicsObjectAttributes physicsObjectAttributes) { + assets::PhysicsObjectAttributes physicsObjectAttributes, + scene::SceneNode* attachmentNode) { //! Create new physics object (child node of staticSceneObject_) int newObjectID = allocateObjectID(); - scene::SceneNode& newNode = staticSceneObject_->node().createChild(); - existingObjects_[newObjectID] = std::make_unique(&newNode); + + scene::SceneNode* objectNode = attachmentNode; + if (attachmentNode == nullptr) { + objectNode = &staticSceneObject_->node().createChild(); + } + + existingObjects_[newObjectID] = + std::make_unique(objectNode); const assets::MeshMetaData& metaData = resourceManager_->getMeshMetaData( physicsObjectAttributes.getString("collisionMeshHandle")); @@ -92,7 +99,8 @@ int BulletPhysicsManager::makeRigidObject( LOG(ERROR) << "Object load failed"; deallocateObjectID(newObjectID); existingObjects_.erase(newObjectID); - delete &newNode; + if (attachmentNode == nullptr) + delete objectNode; return ID_UNDEFINED; } return newObjectID; @@ -100,10 +108,12 @@ int BulletPhysicsManager::makeRigidObject( int BulletPhysicsManager::addObject(const int objectLibIndex, DrawableGroup* drawables, + scene::SceneNode* attachmentNode, const Magnum::ResourceKey& lightSetup) { // Do default load first (adds the SceneNode to the SceneGraph and computes // the cumulativeBB_) - int objID = PhysicsManager::addObject(objectLibIndex, drawables, lightSetup); + int objID = PhysicsManager::addObject(objectLibIndex, drawables, + attachmentNode, lightSetup); // Then set the collision shape to the cumulativeBB_ if necessary if (objID != ID_UNDEFINED) { diff --git a/src/esp/physics/bullet/BulletPhysicsManager.h b/src/esp/physics/bullet/BulletPhysicsManager.h index a4ccb920ce..6d57579bc4 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.h +++ b/src/esp/physics/bullet/BulletPhysicsManager.h @@ -84,9 +84,26 @@ class BulletPhysicsManager : public PhysicsManager { const assets::PhysicsSceneAttributes& physicsSceneAttributes, const std::vector& meshGroup) override; + /** + * @brief Override of @ref PhysicsManager::addObject() to handle primitive + * collision shapes requiring a SceneNode with bounding box to be + * pre-computed. + * + * @param objectLibIndex The index of the object's template in @ref + * esp::assets::ResourceManager::physicsObjectLibrary_ + * @param drawables Reference to the scene graph drawables group to enable + * rendering of the newly initialized object. + * @param attachmentNode If supplied, attach the new physical object to an + * existing SceneNode. + * @param lightSetup The identifier of the lighting configuration to use when + * rendering the new object. + * @return the instanced object's ID, mapping to it in @ref + * PhysicsManager::existingObjects_ if successful, or @ref esp::ID_UNDEFINED. + */ virtual int addObject( const int objectLibIndex, DrawableGroup* drawables, + scene::SceneNode* attachmentNode = nullptr, const Magnum::ResourceKey& lightSetup = Magnum::ResourceKey{ assets::ResourceManager::DEFAULT_LIGHTING_KEY}) override; @@ -227,10 +244,13 @@ class BulletPhysicsManager : public PhysicsManager { * @param meshGroup The object's mesh. * @param physicsObjectAttributes The physical object's template defining its * physical parameters. + * @param attachmentNode If supplied, attach the new physical object to an + * existing SceneNode. + * @return The id of the newly allocated object in @ref existingObjects_ */ - int makeRigidObject( - const std::vector& meshGroup, - assets::PhysicsObjectAttributes physicsObjectAttributes) override; + int makeRigidObject(const std::vector& meshGroup, + assets::PhysicsObjectAttributes physicsObjectAttributes, + scene::SceneNode* attachmentNode = nullptr) override; ESP_SMART_POINTERS(BulletPhysicsManager) diff --git a/src/esp/physics/bullet/BulletRigidObject.cpp b/src/esp/physics/bullet/BulletRigidObject.cpp index aa151ac27f..c7b95f31df 100644 --- a/src/esp/physics/bullet/BulletRigidObject.cpp +++ b/src/esp/physics/bullet/BulletRigidObject.cpp @@ -333,10 +333,8 @@ bool BulletRigidObject::setMotionType(MotionType mt) { void BulletRigidObject::shiftOrigin(const Magnum::Vector3& shift) { Corrade::Utility::Debug() << "shiftOrigin: " << shift; - // shift each child node - for (auto& child : node().children()) { - child.translate(shift); - } + if (visualNode_) + visualNode_->translate(shift); // shift all children of the parent collision shape for (int i = 0; i < bObjectShape_->getNumChildShapes(); i++) { diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index 4a7d0ecfd6..ef3500985c 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -233,12 +233,8 @@ bool operator!=(const SimulatorConfiguration& a, // === Physics Simulator Functions === -int Simulator::addObject(int objectLibIndex, int sceneID) { - return addObject(objectLibIndex, - assets::ResourceManager::DEFAULT_LIGHTING_KEY, sceneID); -} - int Simulator::addObject(int objectLibIndex, + scene::SceneNode* attachmentNode, const std::string& lightSetupKey, int sceneID) { if (sceneHasPhysics(sceneID)) { @@ -247,7 +243,7 @@ int Simulator::addObject(int objectLibIndex, auto& sceneGraph_ = sceneManager_.getSceneGraph(activeSceneID_); auto& drawables = sceneGraph_.getDrawables(); return physicsManager_->addObject(objectLibIndex, &drawables, - lightSetupKey); + attachmentNode, lightSetupKey); } return ID_UNDEFINED; } @@ -267,9 +263,12 @@ std::vector Simulator::getExistingObjectIDs(const int sceneID) { } // remove object objectID instance in sceneID -void Simulator::removeObject(const int objectID, const int sceneID) { +void Simulator::removeObject(const int objectID, + bool deleteObjectNode, + bool deleteVisualNode, + const int sceneID) { if (sceneHasPhysics(sceneID)) { - physicsManager_->removeObject(objectID); + physicsManager_->removeObject(objectID, deleteObjectNode, deleteVisualNode); } } diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index d0eeb8bc6f..14c9d36a29 100644 --- a/src/esp/sim/Simulator.h +++ b/src/esp/sim/Simulator.h @@ -101,11 +101,13 @@ class Simulator { * esp::physics::PhysicsManager::existingObjects_ or @ref esp::ID_UNDEFINED if * instancing fails. */ - int addObject(int objectLibIndex, int sceneID = 0); + // int addObject(int objectLibIndex, int sceneID = 0); /** @overload */ int addObject(int objectLibIndex, - const std::string& lightSetupKey, + scene::SceneNode* attachmentNode = nullptr, + const std::string& lightSetupKey = + assets::ResourceManager::DEFAULT_LIGHTING_KEY, int sceneID = 0); /** @@ -124,7 +126,10 @@ class Simulator { * @param sceneID !! Not used currently !! Specifies which physical scene to * remove the object from. */ - void removeObject(const int objectID, const int sceneID = 0); + void removeObject(const int objectID, + bool deleteObjectNode = true, + bool deleteVisualNode = true, + const int sceneID = 0); /** * @brief Get the IDs of the physics objects instanced in a physical scene. diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index bbe1619d58..92fc67e156 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -408,3 +408,52 @@ TEST_F(PhysicsManagerTest, TestVelocityControl) { ASSERT_EQ(physicsManager_->getAngularVelocity(objectId), Magnum::Vector3{}); } } + +TEST_F(PhysicsManagerTest, TestSceneNodeAttachment) { + // test attaching/detaching existing SceneNode to/from physical simulation + LOG(INFO) << "Starting physics test: TestSceneNodeAttachment"; + + std::string objectFile = Cr::Utility::Directory::join( + dataDir, "test_assets/objects/transform_box.glb"); + + std::string sceneFile = + Cr::Utility::Directory::join(dataDir, "test_assets/scenes/plane.glb"); + + initScene(sceneFile); + + esp::assets::PhysicsObjectAttributes physicsObjectAttributes; + physicsObjectAttributes.setString("renderMeshHandle", objectFile); + resourceManager_.loadObject(physicsObjectAttributes, objectFile); + + esp::scene::SceneNode& root = + sceneManager_.getSceneGraph(sceneID_).getRootNode(); + esp::scene::SceneNode* newNode = &root.createChild(); + ASSERT_EQ(root.children().last(), newNode); + + auto& drawables = sceneManager_.getSceneGraph(sceneID_).getDrawables(); + + // Test attaching newNode to a RigidBody + int objectId = physicsManager_->addObject(objectFile, &drawables, newNode); + ASSERT_EQ(&physicsManager_->getObjectSceneNode(objectId), newNode); + + // Test updating newNode position with PhysicsManager + Magnum::Vector3 newPos{1.0, 3.0, 0.0}; + physicsManager_->setTranslation(objectId, newPos); + ASSERT_EQ(physicsManager_->getTranslation(objectId), newPos); + ASSERT_EQ(newNode->translation(), newPos); + + // Test leaving newNode without visualNode_ after destroying the RigidBody + physicsManager_->removeObject(objectId, false, true); + ASSERT(newNode->children().isEmpty()); + + // Test leaving the visualNode attached to newNode after destroying the + // RigidBody + objectId = physicsManager_->addObject(objectFile, &drawables, newNode); + physicsManager_->removeObject(objectId, false, false); + ASSERT(!newNode->children().isEmpty()); + + // Test destroying newNode with the RigidBody + objectId = physicsManager_->addObject(objectFile, &drawables, newNode); + physicsManager_->removeObject(objectId, true, true); + ASSERT_NE(root.children().last(), newNode); +} diff --git a/src/tests/SimTest.cpp b/src/tests/SimTest.cpp index 04bee2c86d..3240b8fc5a 100644 --- a/src/tests/SimTest.cpp +++ b/src/tests/SimTest.cpp @@ -197,7 +197,7 @@ void SimTest::getDefaultLightingRGBAObservation() { void SimTest::getCustomLightingRGBAObservation() { auto simulator = getSimulator(vangogh); - int objectID = simulator->addObject(0, "custom_lighting_1"); + int objectID = simulator->addObject(0, nullptr, "custom_lighting_1"); CORRADE_VERIFY(objectID != esp::ID_UNDEFINED); simulator->setTranslation({1.0f, 0.5f, -0.5f}, objectID); @@ -222,7 +222,7 @@ void SimTest::updateLightSetupRGBAObservation() { simulator->removeObject(objectID); // update custom lighting - objectID = simulator->addObject(0, "custom_lighting_1"); + objectID = simulator->addObject(0, nullptr, "custom_lighting_1"); CORRADE_VERIFY(objectID != esp::ID_UNDEFINED); simulator->setTranslation({1.0f, 0.5f, -0.5f}, objectID); @@ -258,11 +258,11 @@ void SimTest::multipleLightingSetupsRGBAObservation() { auto simulator = getSimulator(planeScene); // make sure updates apply to all objects using the light setup - int objectID = simulator->addObject(0, "custom_lighting_1"); + int objectID = simulator->addObject(0, nullptr, "custom_lighting_1"); CORRADE_VERIFY(objectID != esp::ID_UNDEFINED); simulator->setTranslation({0.0f, 0.5f, -0.5f}, objectID); - int otherObjectID = simulator->addObject(0, "custom_lighting_1"); + int otherObjectID = simulator->addObject(0, nullptr, "custom_lighting_1"); CORRADE_VERIFY(otherObjectID != esp::ID_UNDEFINED); simulator->setTranslation({2.0f, 0.5f, -0.5f}, otherObjectID); diff --git a/tests/test_physics.py b/tests/test_physics.py index 10b5b6b13a..e5a15f38af 100644 --- a/tests/test_physics.py +++ b/tests/test_physics.py @@ -90,6 +90,16 @@ def test_kinematics(sim): assert sim.get_world_time() > prev_time prev_time = sim.get_world_time() + sim.remove_object(object_id) + + # test attaching/dettaching an Agent to/from physics simulation + agent_node = sim.agents[0].scene_node + sim.add_object(0, agent_node) + sim.set_translation(np.random.rand(3), object_id) + assert np.allclose(agent_node.translation, sim.get_translation(object_id)) + sim.remove_object(object_id, False) # don't delete the agent's node + assert agent_node.translation + @pytest.mark.skipif( not osp.exists("data/scene_datasets/habitat-test-scenes/skokloster-castle.glb")