diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index eaec716b8..813db13c5 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -34,6 +34,12 @@ class ignition::physics::tpelib::ModelPrivate /// \brief First inserted link id; public: std::size_t firstLinkId = kNullEntityId; + + /// \brief Links in the model + public: std::vector linkIds; + + /// \brief Nested models + public: std::vector nestedModelIds; }; using namespace ignition; @@ -64,29 +70,43 @@ Entity &Model::AddLink() { std::size_t linkId = Entity::GetNextId(); - if (this->GetChildren().empty()) + if (this->GetLinkCount() == 0u) this->dataPtr->firstLinkId = linkId; const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); + this->dataPtr->linkIds.push_back(linkId); it->second->SetParent(this); this->ChildrenChanged(); return *it->second.get(); } +////////////////////////////////////////////////// +std::size_t Model::GetLinkCount() const +{ + return this->dataPtr->linkIds.size(); +} + ////////////////////////////////////////////////// Entity &Model::AddModel() { std::size_t modelId = Entity::GetNextId(); const auto[it, success] = this->GetChildren().insert( {modelId, std::make_shared(modelId)}); + this->dataPtr->nestedModelIds.push_back(modelId); it->second->SetParent(this); this->ChildrenChanged(); return *it->second.get(); } +////////////////////////////////////////////////// +std::size_t Model::GetModelCount() const +{ + return this->dataPtr->nestedModelIds.size(); +} + ////////////////////////////////////////////////// void Model::SetCanonicalLink(std::size_t linkId) { diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh index 72d9fb852..3639b58dc 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -48,10 +48,18 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity /// \return Newly created Link public: Entity &AddLink(); + /// \brief Get the number of links in the model + /// \return Number of links in the model + public: std::size_t GetLinkCount() const; + /// \brief Add a nested model /// \return Newly created nested model public: Entity &AddModel(); + /// \brief Get the number of nested models in the model + /// \return Number of nested mdoels in the model + public: std::size_t GetModelCount() const; + /// \brief Set the canonical link of model public: void SetCanonicalLink( std::size_t linkId = kNullEntityId); diff --git a/tpe/plugin/src/Base.hh b/tpe/plugin/src/Base.hh index b8355b676..6b9e58eb6 100644 --- a/tpe/plugin/src/Base.hh +++ b/tpe/plugin/src/Base.hh @@ -88,26 +88,38 @@ class Base : public Implements3d> return -1; } - public: inline std::size_t indexInContainerToId( - const std::size_t _containerId, const std::size_t _index) const + public: template + inline std::pair indexInContainerToId( + const std::size_t _containerId, + const std::size_t _index, + const std::map &_idMap) const { std::size_t counter = 0; auto it = this->childIdToParentId.begin(); while (counter <= _index && it != this->childIdToParentId.end()) { - if (it->second == _containerId && counter == _index) + if (it->second == _containerId) { - return it->first; - } - else if (it->second == _containerId) - { - ++counter; + auto idMapIt = _idMap.find(it->first); + // only count if the entity is found in the idMap. This makes sure we're + // counting only the entities with the correct EntityType + if (idMapIt != _idMap.end()) + { + if (counter == _index) + { + return *idMapIt; + } + else + { + ++counter; + } + } } ++it; } // return invalid id if entity not found - return -1; + return {INVALID_ENTITY_ID, nullptr}; } diff --git a/tpe/plugin/src/Base_TEST.cc b/tpe/plugin/src/Base_TEST.cc index 551fe0241..3438833be 100644 --- a/tpe/plugin/src/Base_TEST.cc +++ b/tpe/plugin/src/Base_TEST.cc @@ -158,24 +158,28 @@ TEST(BaseClass, AddEntities) // check indices std::size_t modelInd1 = base.idToIndexInContainer(modelId1); EXPECT_EQ(0u, modelInd1); - EXPECT_EQ(modelId1, base.indexInContainerToId(worldId, 0u)); + EXPECT_EQ(modelId1, + base.indexInContainerToId(worldId, 0u, base.models).first); std::size_t modelInd2 = base.idToIndexInContainer(modelId2); EXPECT_EQ(1u, modelInd2); - EXPECT_EQ(modelId2, base.indexInContainerToId(worldId, 1u)); + EXPECT_EQ(modelId2, + base.indexInContainerToId(worldId, 1u, base.models).first); std::size_t linkInd1 = base.idToIndexInContainer(linkId1); EXPECT_EQ(0u, linkInd1); - EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u)); + EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u, base.links).first); std::size_t linkInd2 = base.idToIndexInContainer(linkId2); EXPECT_EQ(0u, linkInd2); - EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u)); + EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u, base.links).first); std::size_t boxInd = base.idToIndexInContainer(boxId); EXPECT_EQ(0u, boxInd); - EXPECT_EQ(boxId, base.indexInContainerToId(linkId1, 0u)); + EXPECT_EQ(boxId, + base.indexInContainerToId(linkId1, 0u, base.collisions).first); std::size_t cylinderInd = base.idToIndexInContainer(cylinderId); EXPECT_EQ(0u, cylinderInd); - EXPECT_EQ(cylinderId, base.indexInContainerToId(linkId2, 0u)); + EXPECT_EQ(cylinderId, + base.indexInContainerToId(linkId2, 0u, base.collisions).first); } int main(int argc, char *argv[]) diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index 60ac6e77c..a69a6a5f2 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -110,11 +110,11 @@ std::size_t EntityManagementFeatures::GetModelCount( Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::size_t _modelIndex) const { - std::size_t modelId = this->indexInContainerToId(_worldID.id, _modelIndex); - auto it = this->models.find(modelId); - if (it != this->models.end() && it->second != nullptr) + const auto &[modelId, modelInfo] = + this->indexInContainerToId(_worldID.id, _modelIndex, this->models); + if (modelInfo != nullptr) { - return this->GenerateIdentity(modelId, it->second); + return this->GenerateIdentity(modelId, modelInfo); } return this->GenerateInvalidId(); } @@ -172,22 +172,59 @@ Identity EntityManagementFeatures::GetWorldOfModel( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface(_modelID)->model->GetModelCount(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + const auto &[nestedModelId, nestedModelInfo] = + this->indexInContainerToId(_modelID.id, _modelIndex, this->models); + if (nestedModelInfo != nullptr) + { + return this->GenerateIdentity(nestedModelId, nestedModelInfo); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + tpelib::Entity &modelEnt = modelInfo->model->GetChildByName(_modelName); + auto it = this->models.find(modelEnt.GetId()); + if (it != this->models.end() && it->second != nullptr) + { + return this->GenerateIdentity(it->first, it->second); + } + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const { - return this->ReferenceInterface(_modelID)->model->GetChildCount(); + return this->ReferenceInterface(_modelID)->model->GetLinkCount(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::size_t _linkIndex) const { - std::size_t linkId = this->indexInContainerToId(_modelID.id, _linkIndex); - auto it = this->links.find(linkId); - if (it != this->links.end() && it->second != nullptr) + const auto &[linkId, linkInfo] = + this->indexInContainerToId(_modelID.id, _linkIndex, this->links); + if (linkInfo != nullptr) { - return this->GenerateIdentity(it->first, it->second); + return this->GenerateIdentity(linkId, linkInfo); } return this->GenerateInvalidId(); } @@ -256,11 +293,11 @@ std::size_t EntityManagementFeatures::GetShapeCount( Identity EntityManagementFeatures::GetShape( const Identity &_linkID, const std::size_t _shapeIndex) const { - std::size_t shapeId = this->indexInContainerToId(_linkID.id, _shapeIndex); - auto it = this->collisions.find(shapeId); - if (it != this->collisions.end() && it->second != nullptr) + const auto &[shapeId, shapeInfo] = + this->indexInContainerToId(_linkID.id, _shapeIndex, this->collisions); + if (shapeInfo != nullptr) { - return this->GenerateIdentity(it->first, it->second); + return this->GenerateIdentity(shapeId, shapeInfo); } return this->GenerateInvalidId(); } @@ -328,8 +365,9 @@ bool EntityManagementFeatures::RemoveModelByIndex( auto worldInfo = this->ReferenceInterface(_worldID); if (worldInfo != nullptr) { - auto modelId = this->indexInContainerToId(_worldID.id, _modelIndex); - if (this->models.find(modelId) != this->models.end()) + const auto [modelId, modelInfo] = + this->indexInContainerToId(_worldID.id, _modelIndex, this->models); + if (modelInfo != nullptr) { this->models.erase(modelId); this->childIdToParentId.erase(modelId); @@ -406,6 +444,21 @@ Identity EntityManagementFeatures::ConstructEmptyModel( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity EntityManagementFeatures::ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + auto &modelEnt = modelInfo->model->AddModel(); + modelEnt.SetName(_name); + tpelib::Model *model = static_cast(&modelEnt); + return this->AddModel(_modelID.id, *model); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyLink( const Identity &_modelID, const std::string &_name) diff --git a/tpe/plugin/src/EntityManagementFeatures.hh b/tpe/plugin/src/EntityManagementFeatures.hh index 8f28d50a3..3e5075349 100644 --- a/tpe/plugin/src/EntityManagementFeatures.hh +++ b/tpe/plugin/src/EntityManagementFeatures.hh @@ -36,11 +36,13 @@ struct EntityManagementFeatureList : FeatureList< GetEngineInfo, GetWorldFromEngine, GetModelFromWorld, + GetNestedModelFromModel, GetLinkFromModel, GetShapeFromLink, RemoveEntities, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, + ConstructEmptyNestedModelFeature, ConstructEmptyLinkFeature, CollisionFilterMaskFeature > { }; @@ -87,6 +89,15 @@ class EntityManagementFeatures : public: Identity GetWorldOfModel(const Identity &_modelID) const override; + public: std::size_t GetNestedModelCount( + const Identity &_modelID) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, std::size_t _modelIndex) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const override; + public: std::size_t GetLinkCount(const Identity &_modelID) const override; public: Identity GetLink( @@ -135,6 +146,9 @@ class EntityManagementFeatures : public: Identity ConstructEmptyModel( const Identity &_worldID, const std::string &_name) override; + public: Identity ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) override; + public: Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) override; diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index bdc312eef..8dccad7d9 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -79,6 +79,19 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(model, link->GetModel()); EXPECT_EQ(link, model->GetLink(0)); EXPECT_EQ(link, model->GetLink("empty link")); + + EXPECT_EQ(0u, model->GetNestedModelCount()); + auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); + EXPECT_EQ(1u, model->GetNestedModelCount()); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("empty nested model", nestedModel->GetName()); + EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); + EXPECT_EQ(nestedModel, model->GetNestedModel(0)); + EXPECT_EQ(0u, nestedModel->GetLinkCount()); + ASSERT_NE(nullptr, nestedModel->ConstructEmptyLink("empty link")); + EXPECT_EQ(1u, nestedModel->GetLinkCount()); + + EXPECT_EQ(2u, model->GetLinkCount()); } TEST(EntityManagement_TEST, RemoveEntities)