diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 1d2e55f408..d5cadc408d 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -17,6 +17,8 @@ #include #include +#include +#include #include #include @@ -41,7 +43,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \param[in] _entity Entity to be inserted. /// \param[in, out] _set Set to be filled. public: void InsertEntityRecursive(Entity _entity, - std::set &_set); + std::unordered_set &_set); /// \brief Register a new component type. /// \param[in] _typeId Type if of the new component. @@ -50,7 +52,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Map of component storage classes. The key is a component /// type id, and the value is a pointer to the component storage. - public: std::map> components; /// \brief A graph holding all entities, arranged according to their @@ -64,16 +66,17 @@ class ignition::gazebo::EntityComponentManagerPrivate public: std::set oneTimeChangedComponents; /// \brief Entities that have just been created - public: std::set newlyCreatedEntities; + public: std::unordered_set newlyCreatedEntities; /// \brief Entities that need to be removed. - public: std::set toRemoveEntities; + public: std::unordered_set toRemoveEntities; /// \brief Flag that indicates if all entities should be removed. public: bool removeAllEntities{false}; /// \brief The set of components that each entity has - public: std::map> entityComponents; + public: std::unordered_map> entityComponents; /// \brief A mutex to protect newly created entityes. public: std::mutex entityCreatedMutex; @@ -90,7 +93,8 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Cache of previously queried descendants. The key is the parent /// entity for which descendants were queried, and the value are all its /// descendants. - public: mutable std::map> descendantCache; + public: mutable std::unordered_map> + descendantCache; /// \brief Keep track of entities already used to ensure uniqueness. public: uint64_t entityCount{0}; @@ -158,7 +162,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities() ///////////////////////////////////////////////// void EntityComponentManagerPrivate::InsertEntityRecursive(Entity _entity, - std::set &_set) + std::unordered_set &_set) { for (const auto &vertex : this->entities.AdjacentsFrom(_entity)) { @@ -173,7 +177,7 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, { // Store the to-be-removed entities in a temporary set so we can call // UpdateViews on each of them - std::set tmpToRemoveEntities; + std::unordered_set tmpToRemoveEntities; if (!_recursive) { tmpToRemoveEntities.insert(_entity); @@ -241,14 +245,14 @@ void EntityComponentManager::ProcessRemoveEntityRequests() // Remove from graph this->dataPtr->entities.RemoveVertex(entity); + auto entityIter = this->dataPtr->entityComponents.find(entity); // Remove the components, if any. - if (this->dataPtr->entityComponents.find(entity) != - this->dataPtr->entityComponents.end()) + if (entityIter != this->dataPtr->entityComponents.end()) { - for (const ComponentKey &key : - this->dataPtr->entityComponents.at(entity)) + for (const auto &key : entityIter->second) { - this->dataPtr->components.at(key.first)->Remove(key.second); + this->dataPtr->components.at(key.second.first)->Remove( + key.second.second); } // Remove the entry in the entityComponent map @@ -287,12 +291,8 @@ bool EntityComponentManager::RemoveComponent( if (!this->EntityHasComponent(_entity, _key)) return false; - auto entityComponentIter = std::find( - this->dataPtr->entityComponents[_entity].begin(), - this->dataPtr->entityComponents[_entity].end(), _key); - this->dataPtr->components.at(_key.first)->Remove(_key.second); - this->dataPtr->entityComponents[_entity].erase(entityComponentIter); + this->dataPtr->entityComponents[_entity].erase(_key.first); this->dataPtr->oneTimeChangedComponents.erase(_key); this->dataPtr->periodicChangedComponents.erase(_key); @@ -304,10 +304,10 @@ bool EntityComponentManager::RemoveComponent( bool EntityComponentManager::EntityHasComponent(const Entity _entity, const ComponentKey &_key) const { - return this->HasEntity(_entity) && - std::find(this->dataPtr->entityComponents[_entity].begin(), - this->dataPtr->entityComponents[_entity].end(), _key) != - this->dataPtr->entityComponents[_entity].end(); + if (!this->HasEntity(_entity)) + return false; + auto &compMap = this->dataPtr->entityComponents[_entity]; + return compMap.find(_key.first) != compMap.end(); } ///////////////////////////////////////////////// @@ -322,11 +322,8 @@ bool EntityComponentManager::EntityHasComponentType(const Entity _entity, if (iter == this->dataPtr->entityComponents.end()) return false; - return std::find_if(iter->second.begin(), iter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _typeId; - }) != iter->second.end(); + auto typeIter = iter->second.find(_typeId); + return (typeIter != iter->second.end()); } ///////////////////////////////////////////////// @@ -360,21 +357,16 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity, if (ecIter == this->dataPtr->entityComponents.end()) return result; - auto iter = std::find_if(ecIter->second.begin(), ecIter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _typeId; - }); - - if (iter == ecIter->second.end()) + auto typeKey = ecIter->second.find(_typeId); + if (typeKey == ecIter->second.end()) return result; - if (this->dataPtr->oneTimeChangedComponents.find(*iter) != + if (this->dataPtr->oneTimeChangedComponents.find(typeKey->second) != this->dataPtr->oneTimeChangedComponents.end()) { result = ComponentState::OneTimeChange; } - else if (this->dataPtr->periodicChangedComponents.find(*iter) != + else if (this->dataPtr->periodicChangedComponents.find(typeKey->second) != this->dataPtr->periodicChangedComponents.end()) { result = ComponentState::PeriodicChange; @@ -468,7 +460,8 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( ComponentKey componentKey{_componentTypeId, componentIdPair.first}; - this->dataPtr->entityComponents[_entity].push_back(componentKey); + this->dataPtr->entityComponents[_entity].insert( + {_componentTypeId, componentKey}); this->dataPtr->oneTimeChangedComponents.insert(componentKey); if (componentIdPair.second) @@ -487,23 +480,14 @@ bool EntityComponentManager::EntityMatches(Entity _entity, if (iter == this->dataPtr->entityComponents.end()) return false; - // \todo(nkoenig) The performance of this could be improved. Ideally we - // wouldn't need two loops to confirm that an entity matches a set of - // types. It might be possible to create bitmask for component sets. + // \todo(nkoenig) The performance of this could be improved. + // It might be possible to create bitmask for component sets. // Fixing this might not be high priority, unless we expect frequent // creation of entities and/or queries. for (const ComponentTypeId &type : _types) { - bool found = false; - for (const ComponentKey &comp : iter->second) - { - if (comp.first == type) - { - found = true; - break; - } - } - if (!found) + auto typeIter = iter->second.find(type); + if (typeIter == iter->second.end()) return false; } @@ -519,15 +503,9 @@ ComponentId EntityComponentManager::EntityComponentIdFromType( if (ecIter == this->dataPtr->entityComponents.end()) return -1; - auto iter = - std::find_if(ecIter->second.begin(), ecIter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _type; - }); - - if (iter != ecIter->second.end()) - return iter->second; + auto typeIter = ecIter->second.find(_type); + if (typeIter != ecIter->second.end()) + return typeIter->second.second; return -1; } @@ -543,14 +521,10 @@ const components::BaseComponent if (ecIter == this->dataPtr->entityComponents.end()) return nullptr; - auto iter = std::find_if(ecIter->second.begin(), ecIter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _type; - }); - - if (iter != ecIter->second.end()) - return this->dataPtr->components.at(iter->first)->Component(iter->second); + auto typeIter = ecIter->second.find(_type); + if (typeIter != ecIter->second.end()) + return this->dataPtr->components.at(typeIter->second.first)->Component( + typeIter->second.second); return nullptr; } @@ -564,15 +538,10 @@ components::BaseComponent *EntityComponentManager::ComponentImplementation( if (ecIter == this->dataPtr->entityComponents.end()) return nullptr; - auto iter = - std::find_if(ecIter->second.begin(), ecIter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _type; - }); - - if (iter != ecIter->second.end()) - return this->dataPtr->components.at(iter->first)->Component(iter->second); + auto typeIter = ecIter->second.find(_type); + if (typeIter != ecIter->second.end()) + return this->dataPtr->components.at(typeIter->second.first)->Component( + typeIter->second.second); return nullptr; } @@ -739,6 +708,9 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, { auto entityMsg = _msg.add_entities(); entityMsg->set_id(_entity); + auto iter = this->dataPtr->entityComponents.find(_entity); + if (iter == this->dataPtr->entityComponents.end()) + return; if (this->dataPtr->toRemoveEntities.find(_entity) != this->dataPtr->toRemoveEntities.end()) @@ -746,19 +718,29 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, entityMsg->set_remove(true); } - // Empty means all types - bool allTypes = _types.empty(); + // Insert all of the entity's components if the passed in types + // set is empty + auto types = _types; + if (types.empty()) + { + for (auto &type : this->dataPtr->entityComponents[_entity]) + { + types.insert(type.first); + } + } - auto components = this->dataPtr->entityComponents[_entity]; - for (const auto &comp : components) + for (const ComponentTypeId type : types) { - if (!allTypes && _types.find(comp.first) == _types.end()) + // If the entity does not have the component, continue + std::unordered_map::iterator typeIter = + iter->second.find(type); + if (typeIter == iter->second.end()) { continue; } + ComponentKey comp = (typeIter->second); auto compMsg = entityMsg->add_components(); - auto compBase = this->ComponentImplementation(_entity, comp.first); compMsg->set_type(compBase->TypeId()); @@ -776,10 +758,13 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, Entity _entity, const std::unordered_set &_types, bool _full) const { + auto iter = this->dataPtr->entityComponents.find(_entity); + if (iter == this->dataPtr->entityComponents.end()) + return; + // Set the default entity iterator to the end. This will allow us to know // if the entity has been added to the message. auto entIter = _msg.mutable_entities()->end(); - // Add an entity to the message and set it to be removed if the entity // exists in the toRemoveEntities list. if (this->dataPtr->toRemoveEntities.find(_entity) != @@ -798,16 +783,28 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, entIter->second.set_remove(true); } - // Empty means all types - bool allTypes = _types.empty(); + // Insert all of the entity's components if the passed in types + // set is empty + auto types = _types; + if (types.empty()) + { + for (auto &type : this->dataPtr->entityComponents[_entity]) + { + types.insert(type.first); + } + } - for (const ComponentKey &comp : this->dataPtr->entityComponents[_entity]) + // Empty means all types + for (const ComponentTypeId type : types) { - if (!allTypes && _types.find(comp.first) == _types.end()) + std::unordered_map::iterator typeIter = + iter->second.find(type); + if (typeIter == iter->second.end()) { continue; } + ComponentKey comp = typeIter->second; const components::BaseComponent *compBase = this->ComponentImplementation(_entity, comp.first); @@ -835,9 +832,9 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, } } + auto compIter = entIter->second.mutable_components()->find(comp.first); // Find the component in the message, and add the component to the // message if it's not present. - auto compIter = entIter->second.mutable_components()->find(comp.first); if (compIter == entIter->second.mutable_components()->end()) { msgs::SerializedComponent cmp; @@ -1181,29 +1178,24 @@ void EntityComponentManager::SetChanged( if (ecIter == this->dataPtr->entityComponents.end()) return; - auto iter = std::find_if(ecIter->second.begin(), ecIter->second.end(), - [&] (const ComponentKey &_key) - { - return _key.first == _type; - }); - - if (iter == ecIter->second.end()) + auto typeIter = ecIter->second.find(_type); + if (typeIter == ecIter->second.end()) return; if (_c == ComponentState::PeriodicChange) { - this->dataPtr->periodicChangedComponents.insert(*iter); - this->dataPtr->oneTimeChangedComponents.erase(*iter); + this->dataPtr->periodicChangedComponents.insert(typeIter->second); + this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); } else if (_c == ComponentState::OneTimeChange) { - this->dataPtr->periodicChangedComponents.erase(*iter); - this->dataPtr->oneTimeChangedComponents.insert(*iter); + this->dataPtr->periodicChangedComponents.erase(typeIter->second); + this->dataPtr->oneTimeChangedComponents.insert(typeIter->second); } else { - this->dataPtr->periodicChangedComponents.erase(*iter); - this->dataPtr->oneTimeChangedComponents.erase(*iter); + this->dataPtr->periodicChangedComponents.erase(typeIter->second); + this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); } }