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

@@ -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)

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;

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<int, int>(&Simulator::addObject),
-           "object_lib_index"_a, "scene_id"_a = 0)
-      .def("add_object",
-           py::overload_cast<int, const std::string&, int>(
-               &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,

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<assets::CollisionMeshData>& 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<physics::RigidObject>(&newNode);
+      std::make_unique<physics::RigidObject>(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);
   }

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<assets::CollisionMeshData>& 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*/

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();
 }
 

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. */

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -75,11 +75,18 @@ bool BulletPhysicsManager::addScene(
 
 int BulletPhysicsManager::makeRigidObject(
     const std::vector<assets::CollisionMeshData>& 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<BulletRigidObject>(&newNode);
+
+  scene::SceneNode* objectNode = attachmentNode;
+  if (attachmentNode == nullptr) {
+    objectNode = &staticSceneObject_->node().createChild();
+  }
+
+  existingObjects_[newObjectID] =
+      std::make_unique<BulletRigidObject>(objectNode);
 
   const assets::MeshMetaData& metaData = resourceManager_->getMeshMetaData(
       physicsObjectAttributes.getString("collisionMeshHandle"));
@@ -92,18 +99,21 @@ 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;
 }
 
 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) {

src/esp/physics/bullet/BulletPhysicsManager.h

@@ -84,9 +84,26 @@ class BulletPhysicsManager : public PhysicsManager {
       const assets::PhysicsSceneAttributes& physicsSceneAttributes,
       const std::vector<assets::CollisionMeshData>& 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<assets::CollisionMeshData>& meshGroup,
-      assets::PhysicsObjectAttributes physicsObjectAttributes) override;
+  int makeRigidObject(const std::vector<assets::CollisionMeshData>& meshGroup,
+                      assets::PhysicsObjectAttributes physicsObjectAttributes,
+                      scene::SceneNode* attachmentNode = nullptr) override;
 
   ESP_SMART_POINTERS(BulletPhysicsManager)
 

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++) {