--initial commit; save a ptr in each object to the managed object con…

…taining it.
facebookresearch · Nov 12, 2024 · cad98c3 · cad98c3
1 parent cae5f2d
commit cad98c3
Show file tree

Hide file tree

Showing 6 changed files with 82 additions and 24 deletions.
diff --git a/src/esp/physics/ArticulatedObject.h b/src/esp/physics/ArticulatedObject.h
@@ -29,6 +29,7 @@ class ResourceManager;
 }
 
 namespace physics {
+class ManagedArticulatedObject;
 
 class URDFImporter;
 
@@ -1071,6 +1072,19 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
     return aabb_;
   }
 
+  /**
+   * @brief Get the ManagedArticulatedObject or BulletManagedArticulatedObject
+   * referencing this object.
+   */
+  template <class T>
+  std::shared_ptr<T> getManagedArticulatedObject() const {
+    static_assert(std::is_base_of<ManagedArticulatedObject, T>::value,
+                  "ManagedArticulatedObject must be base class of desired "
+                  "ArticulatedObject's Managed wrapper class.");
+
+    return PhysicsObjectBase::getManagedObjectPtrInternal<T>();
+  }
+
  protected:
   /**
    * @brief Used to synchronize simulator's notion of the object state

diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp
@@ -319,17 +319,17 @@ int PhysicsManager::addObjectInternal(
   }
 
   // derive valid object ID and create new node if necessary
-  int nextObjectID_ = allocateObjectID();
+  int newObjectID = allocateObjectID();
   scene::SceneNode* objectNode = attachmentNode;
   if (attachmentNode == nullptr) {
     objectNode = &staticStageObject_->node().createChild();
   }
-
+  // Attempt to create a new object, initialize it and add to existingObjects_
   objectSuccess =
-      makeAndAddRigidObject(nextObjectID_, objectAttributes, objectNode);
+      makeAndAddRigidObject(newObjectID, objectAttributes, objectNode);
 
   if (!objectSuccess) {
-    deallocateObjectID(nextObjectID_);
+    deallocateObjectID(newObjectID);
     if (attachmentNode == nullptr) {
       delete objectNode;
     }
@@ -341,7 +341,7 @@ int PhysicsManager::addObjectInternal(
 
   // temp non-owning pointer to object
   esp::physics::RigidObject* const obj =
-      (existingObjects_.at(nextObjectID_).get());
+      (existingObjects_.at(newObjectID).get());
 
   obj->visualNodes_.push_back(obj->visualNode_);
 
@@ -358,7 +358,7 @@ int PhysicsManager::addObjectInternal(
   objectSuccess = obj->finalizeObject();
   if (!objectSuccess) {
     // if failed for some reason, remove and return
-    removeObject(nextObjectID_, true, true);
+    removeObject(newObjectID, true, true);
     ESP_ERROR() << "PhysicsManager::finalizeObject unsuccessful, so addObject `"
                 << objectAttributes->getHandle() << "` aborted.";
     return ID_UNDEFINED;
@@ -373,18 +373,21 @@ int PhysicsManager::addObjectInternal(
   ESP_DEBUG() << "Simplified template handle :" << simpleObjectHandle
               << " | newObjectHandle :" << newObjectHandle;
 
-  existingObjects_.at(nextObjectID_)->setObjectName(newObjectHandle);
+  obj->setObjectName(newObjectHandle);
 
   // 2.0 Get wrapper - name is irrelevant, do not register.
   ManagedRigidObject::ptr objWrapper = getRigidObjectWrapper();
 
   // 3.0 Put object in wrapper
-  objWrapper->setObjectRef(existingObjects_.at(nextObjectID_));
+  objWrapper->setObjectRef(existingObjects_.at(newObjectID));
 
   // 4.0 register wrapper in manager
-  rigidObjectManager_->registerObject(std::move(objWrapper), newObjectHandle);
+  rigidObjectManager_->registerObject(objWrapper, newObjectHandle);
+
+  // 4.5 register wrapper with object it contains
+  obj->setManagedObjectPtr(objWrapper);
 
-  return nextObjectID_;
+  return newObjectID;
 }  // PhysicsManager::addObject
 
 /////////////////////////////////

diff --git a/src/esp/physics/PhysicsObjectBase.h b/src/esp/physics/PhysicsObjectBase.h
@@ -608,7 +608,32 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
   /** @brief Return the local axis-aligned bounding box of the this object.*/
   virtual const Mn::Range3D& getAabb() { return node().getCumulativeBB(); }
 
+  /** @brief Set the managed object used to reference this object externally
+   * (i.e. via python)*/
+  template <class T>
+  void setManagedObjectPtr(std::shared_ptr<T> managedObjPtr) {
+    _managedObject = std::move(managedObjPtr);
+  }
+
  protected:
+  /**
+   * @brief Accessed Internally. Get the Managed Object that references this
+   * object.
+   */
+  template <class T>
+  std::shared_ptr<T> getManagedObjectPtrInternal() const {
+    if (!_managedObject) {
+      return nullptr;
+    }
+    static_assert(
+        std::is_base_of<core::managedContainers::AbstractManagedObject,
+                        T>::value,
+        "AbstractManagedObject must be base class of desired Managed Object "
+        "class.");
+
+    return std::static_pointer_cast<T>(_managedObject);
+  }
+
   /**
    * @brief Used Internally on object creation. Set whether or not this object
    * is articulated.
@@ -641,7 +666,7 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
    * @ref metadata::attributes::SceneObjectInstanceAttributes used to
    * create and place the object within the scene, appropriately cast for
    * object type.
-   * @return A copy of the initialization template used to create this object
+   * @return The initialization template used to create this object
    * instance or nullptr if no template exists.
    */
   template <class T>
@@ -778,6 +803,11 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
   }
 
  private:
+  /**
+   * @brief The managed wrapper-based object referencing this object.
+   */
+  core::managedContainers::AbstractManagedObject::ptr _managedObject = nullptr;
+
   /**
    * @brief This object's instancing attributes, if any were used during its
    * creation.

diff --git a/src/esp/physics/RigidObject.h b/src/esp/physics/RigidObject.h
@@ -29,6 +29,7 @@ namespace assets {
 class ResourceManager;
 }  // namespace assets
 namespace physics {
+class ManagedRigidObject;
 
 /**@brief Convenience struct for applying constant velocity control to a rigid
  * body. */
@@ -135,6 +136,19 @@ class RigidObject : public RigidBase {
         metadata::attributes::ObjectAttributes>();
   };
 
+  /**
+   * @brief Get the ManagedRigidObject or BulletManagedRigidObject referencing
+   * this object.
+   */
+  template <class T>
+  std::shared_ptr<T> getManagedRigidObject() const {
+    static_assert(std::is_base_of<ManagedRigidObject, T>::value,
+                  "ManagedRigidObject must be base class of desired "
+                  "RigidObject's Managed wrapper class.");
+
+    return PhysicsObjectBase::getManagedObjectPtrInternal<T>();
+  }
+
  private:
   /**
    * @brief Finalize the initialization of this @ref esp::physics::RigidObject's

diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp
@@ -233,10 +233,6 @@ void BulletArticulatedObject::updateNodes(bool force) {
   }
 }
 
-////////////////////////////
-// BulletArticulatedLink
-////////////////////////////
-
 std::shared_ptr<metadata::attributes::SceneAOInstanceAttributes>
 BulletArticulatedObject::getCurrentStateInstanceAttr() {
   // get mutable copy of initialization SceneAOInstanceAttributes for this AO

diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp
@@ -214,9 +214,6 @@ int BulletPhysicsManager::addArticulatedObjectInternal(
   collisionObjToObjIds_->emplace(
       articulatedObject->btMultiBody_->getBaseCollider(), articulatedObjectID);
 
-  existingArticulatedObjects_.emplace(articulatedObjectID,
-                                      std::move(articulatedObject));
-
   // get a simplified name of the handle for the object
   std::string simpleArtObjHandle = artObjAttributes->getSimplifiedHandle();
 
@@ -226,19 +223,23 @@ int BulletPhysicsManager::addArticulatedObjectInternal(
   ESP_DEBUG() << "simpleArtObjHandle :" << simpleArtObjHandle
               << " | newArtObjectHandle :" << newArtObjectHandle;
 
-  existingArticulatedObjects_.at(articulatedObjectID)
-      ->setObjectName(newArtObjectHandle);
+  articulatedObject->setObjectName(newArtObjectHandle);
 
   // 2.0 Get wrapper - name is irrelevant, do not register on create.
   ManagedArticulatedObject::ptr AObjWrapper = getArticulatedObjectWrapper();
 
   // 3.0 Put articulated object in wrapper
-  AObjWrapper->setObjectRef(
-      existingArticulatedObjects_.at(articulatedObjectID));
+  AObjWrapper->setObjectRef(articulatedObject);
 
   // 4.0 register wrapper in manager
-  articulatedObjectManager_->registerObject(std::move(AObjWrapper),
-                                            newArtObjectHandle);
+  articulatedObjectManager_->registerObject(AObjWrapper, newArtObjectHandle);
+
+  // 4.5 register wrapper with object it contains
+  articulatedObject->setManagedObjectPtr(AObjWrapper);
+
+  // 5.0 move object into object library
+  existingArticulatedObjects_.emplace(articulatedObjectID,
+                                      std::move(articulatedObject));
 
   return articulatedObjectID;