Improved bullet EntityManagementFeatures GetEntities

bullet/src/Base.hh

@@ -248,9 +248,9 @@ class Base : public Implements3d<FeatureList<Feature>>
   public: using CollisionInfoPtr = std::shared_ptr<CollisionInfo>;
   public: using JointInfoPtr  = std::shared_ptr<JointInfo>;
 
-  public: std::unordered_map<std::size_t, WorldInfoPtr> worlds;
-  public: std::unordered_map<std::size_t, ModelInfoPtr> models;
-  public: std::unordered_map<std::size_t, LinkInfoPtr> links;
+  public: std::map<std::size_t, WorldInfoPtr> worlds;
+  public: std::map<std::size_t, ModelInfoPtr> models;
+  public: std::map<std::size_t, LinkInfoPtr> links;
   public: std::unordered_map<std::size_t, CollisionInfoPtr> collisions;
   public: std::unordered_map<std::size_t, JointInfoPtr> joints;
 

bullet/src/EntityManagementFeatures.cc

@@ -53,6 +53,40 @@ Identity EntityManagementFeatures::ConstructEmptyWorld(
     {_name, collisionConfiguration, dispatcher, broadphase, solver, world});
 }
 
+Identity EntityManagementFeatures::ConstructEmptyModel(
+  const Identity &_worldID, const std::string &_name)
+{
+  return this->AddModel(_worldID, {_name, _worldID, false, math::Pose3d(), {}});
+}
+
+Identity EntityManagementFeatures::ConstructEmptyLink(
+  const Identity &_modelID, const std::string &_name)
+{
+  double mass = 1;
+  btVector3 linkInertiaDiag(1, 1, 1);
+  // Fixed links have 0 mass and inertia
+  if (this->models.at(_modelID)->fixed)
+  {
+    mass = 0;
+    linkInertiaDiag = btVector3(0, 0, 0);
+  }
+
+  auto myMotionState = std::make_shared<btDefaultMotionState>(btTransform());
+  auto collisionShape = std::make_shared<btCompoundShape>();
+  btRigidBody::btRigidBodyConstructionInfo
+    rbInfo(mass, myMotionState.get(), collisionShape.get(), linkInertiaDiag);
+
+  auto body = std::make_shared<btRigidBody>(rbInfo);
+  body.get()->setActivationState(DISABLE_DEACTIVATION);
+
+  return this->AddLink(
+    _modelID,
+    {_name, _modelID, math::Pose3d(), math::Pose3d(),
+     mass, linkInertiaDiag,
+     myMotionState, collisionShape, body});
+}
+
+
 /////////////////////////////////////////////////
 bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
 {
@@ -167,6 +201,7 @@ bool EntityManagementFeatures::RemoveModelByName(
 
   return false;
 }
+
 const std::string &EntityManagementFeatures::GetEngineName(
   const Identity &) const
 {
@@ -185,21 +220,51 @@ std::size_t EntityManagementFeatures::GetWorldCount(const Identity &) const
 }
 
 Identity EntityManagementFeatures::GetWorld(
-    const Identity &, std::size_t) const
+    const Identity &, std::size_t _worldIndex) const
 {
-    return this->GenerateIdentity(0);
+  std::size_t index = 0;
+  std::size_t worldEntity = 0;
+  WorldInfoPtr worldPtr = nullptr;
+
+  for (auto itWorld = this->worlds.begin();
+       itWorld != this->worlds.end();
+       ++itWorld)
+  {
+    if (index++ == _worldIndex)
+    {
+      worldPtr = itWorld->second;
+      worldEntity = itWorld->first;
+      break;
+    }
+  }
+
+  return this->GenerateIdentity(worldEntity, worldPtr);
 }
 
 Identity EntityManagementFeatures::GetWorld(
-    const Identity &, const std::string &) const
+    const Identity &, const std::string &_worldName) const
 {
-  return this->GenerateIdentity(0);
+  std::size_t worldEntity = 0;
+  WorldInfoPtr worldPtr = nullptr;
+  for (auto itWorld = this->worlds.begin();
+       itWorld != this->worlds.end();
+       ++itWorld)
+  {
+    if (itWorld->second->name == _worldName)
+    {
+      worldPtr = itWorld->second;
+      worldEntity = itWorld->first;
+      break;
+    }
+  }
+
+  return this->GenerateIdentity(worldEntity, worldPtr);
 }
 
 const std::string &EntityManagementFeatures::GetWorldName(
-    const Identity &) const
+    const Identity &_worldID) const
 {
-  static const std::string worldName = "bullet";
+  static const std::string worldName = this->worlds.at(_worldID)->name;
   return worldName;
 }
 
@@ -216,36 +281,99 @@ Identity EntityManagementFeatures::GetEngineOfWorld(const Identity &) const
 std::size_t EntityManagementFeatures::GetModelCount(
     const Identity &) const
 {
-  return 0;
+  return this->models.size();
 }
 
 Identity EntityManagementFeatures::GetModel(
-    const Identity &, std::size_t) const
+    const Identity &, std::size_t _modelIndex) const
 {
-  return this->GenerateIdentity(0);
+  std::size_t index = 0;
+  std::size_t modelEntity = 0;
+  ModelInfoPtr modelPtr = nullptr;
+
+  for (auto itModel = this->models.begin();
+       itModel != this->models.end();
+       ++itModel)
+  {
+    if (index++ == _modelIndex)
+    {
+      modelPtr = itModel->second;
+      modelEntity = itModel->first;
+      break;
+    }
+  }
+
+  return this->GenerateIdentity(modelEntity, modelPtr);
 }
 
 Identity EntityManagementFeatures::GetModel(
-    const Identity &, const std::string &) const
+    const Identity &, const std::string &_modelName) const
 {
-  return this->GenerateIdentity(0);
+  std::size_t modelEntity = 0;
+  ModelInfoPtr modelPtr = nullptr;
+  for (auto itModel = this->models.begin();
+       itModel != this->models.end();
+       ++itModel)
+  {
+    if (itModel->second->name == _modelName)
+    {
+      modelPtr = itModel->second;
+      modelEntity = itModel->first;
+      break;
+    }
+  }
+
+  return this->GenerateIdentity(modelEntity, modelPtr);
 }
 
 const std::string &EntityManagementFeatures::GetModelName(
-    const Identity &) const
+    const Identity &_modelID) const
 {
-  static const std::string modelName = "bulletModel";
+  // Check if the model exists
+  if (this->models.find(_modelID.id) == this->models.end())
+  {
+    static const std::string modelName = "";
+    return modelName;
+  }
+
+  static const std::string modelName = this->models.at(_modelID)->name;
   return modelName;
 }
 
-std::size_t EntityManagementFeatures::GetModelIndex(const Identity &) const
+std::size_t EntityManagementFeatures::GetModelIndex(
+  const Identity &_modelId) const
 {
+  if (this->models.find(_modelId.id) == this->models.end())
+  {
+    return 0;
+  }
+
+  std::size_t index = 0;
+  ModelInfoPtr modelPtr = nullptr;
+
+  for (const auto& [key, value] : this->models)
+  {
+    if (key == _modelId.id)
+    {
+      return index;
+    }
+    index++;
+  }
+
   return 0;
 }
 
-Identity EntityManagementFeatures::GetWorldOfModel(const Identity &) const
+Identity EntityManagementFeatures::GetWorldOfModel(
+  const Identity &_modelId) const
 {
-  return this->GenerateIdentity(0);
+  if (this->models.find(_modelId.id) == this->models.end())
+  {
+    return GenerateIdentity(0);
+  }
+
+  auto worldID = this->models.at(_modelId)->world;
+
+  return this->GenerateIdentity(worldID, this->worlds.at(worldID));
 }
 
 std::size_t EntityManagementFeatures::GetNestedModelCount(
@@ -268,19 +396,55 @@ Identity EntityManagementFeatures::GetNestedModel(
 
 std::size_t EntityManagementFeatures::GetLinkCount(const Identity &) const
 {
-  return 0;
+  return this->links.size();
 }
 
 Identity EntityManagementFeatures::GetLink(
-    const Identity &, std::size_t) const
+    const Identity &_modelID, std::size_t _linkIndex) const
 {
+  if (this->models.find(_modelID.id) == this->models.end())
+  {
+    return GenerateIdentity(0);
+  }
+
+  std::size_t index = 0;
+  auto model = this->models.at(_modelID);
+
+  for (const auto & link : model->links)
+  {
+    if (this->links.find(link) != this->links.end())
+    {
+      if (index++ == _linkIndex)
+      {
+        auto linkFound = this->links.at(link);
+        return this->GenerateIdentity(link, linkFound);
+      }
+    }
+  }
   return this->GenerateIdentity(0);
 }
 
 Identity EntityManagementFeatures::GetLink(
-    const Identity &, const std::string &) const
-
+    const Identity &_modelID, const std::string &_linkName) const
 {
+  if (this->models.find(_modelID.id) == this->models.end())
+  {
+    return GenerateIdentity(0);
+  }
+
+  auto model = this->models.at(_modelID);
+  for (const auto & link : model->links)
+  {
+    if (this->links.find(link) != this->links.end())
+    {
+        auto linkFound = this->links.at(link);
+        if (linkFound->name == _linkName)
+        {
+          return this->GenerateIdentity(link, linkFound);
+        }
+    }
+  }
+
   return this->GenerateIdentity(0);
 }
 
@@ -302,20 +466,49 @@ Identity EntityManagementFeatures::GetJoint(
 }
 
 const std::string &EntityManagementFeatures::GetLinkName(
-    const Identity &) const
+    const Identity &_linkId) const
 {
-  static const std::string linkName = "bulletLink";
+  if (this->links.find(_linkId.id) == this->links.end())
+  {
+    static const std::string linkName = "";
+    return linkName;
+  }
+
+  static const std::string linkName = this->links.at(_linkId)->name;;
   return linkName;
 }
 
-std::size_t EntityManagementFeatures::GetLinkIndex(const Identity &) const
+std::size_t EntityManagementFeatures::GetLinkIndex(
+  const Identity &_linkId) const
 {
+  if (this->links.find(_linkId.id) == this->links.end())
+  {
+    return 0;
+  }
+
+  std::size_t index = 0;
+  for (const auto& [key, value] : this->links)
+  {
+    if (key == _linkId.id)
+    {
+      return index;
+    }
+    index++;
+  }
+
   return 0;
 }
 
-Identity EntityManagementFeatures::GetModelOfLink(const Identity &) const
+Identity EntityManagementFeatures::GetModelOfLink(
+  const Identity &_linkId) const
 {
-  return this->GenerateIdentity(0);
+  if (this->links.find(_linkId.id) == this->links.end())
+  {
+    return GenerateIdentity(0);
+  }
+
+  auto modelID = this->links.at(_linkId)->model;
+  return this->GenerateIdentity(modelID, this->models.at(modelID));
 }
 
 std::size_t EntityManagementFeatures::GetShapeCount(const Identity &) const

bullet/src/EntityManagementFeatures.hh

@@ -34,7 +34,10 @@ namespace bullet {
 struct EntityManagementFeatureList : gz::physics::FeatureList<
   GetEntities,
   RemoveModelFromWorld,
-  ConstructEmptyWorldFeature
+  ConstructEmptyWorldFeature,
+  ConstructEmptyModelFeature,
+  ConstructEmptyLinkFeature,
+  GetModelFromWorld
 > { };
 
 class EntityManagementFeatures :
@@ -58,6 +61,12 @@ class EntityManagementFeatures :
   public: Identity ConstructEmptyWorld(
       const Identity &_engineID, const std::string & _name) override;
 
+  public: Identity ConstructEmptyModel(
+    const Identity &_worldID, const std::string &_name) override;
+
+  public: Identity ConstructEmptyLink(
+    const Identity &_modelID, const std::string &_name) override;
+
   // ----- Get entities -----
   public: const std::string &GetEngineName(const Identity &) const override;
   public: std::size_t GetEngineIndex(const Identity &) const override;

test/CMakeLists.txt

@@ -47,6 +47,7 @@ ExternalProject_Add(
 add_subdirectory(benchmark)
 add_subdirectory(common_test)
 add_subdirectory(plugins)
+add_subdirectory(helpers)
 add_subdirectory(integration)
 add_subdirectory(performance)
 add_subdirectory(regression)