ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00009 #ifndef SPACE_H 00010 #define SPACE_H 00011 00012 namespace argos { 00013 class CSpace; 00014 class CRay3; 00015 class CFloorEntity; 00016 } 00017 00018 #include <argos3/core/utility/datatypes/any.h> 00019 #include <argos3/core/simulator/medium/medium.h> 00020 #include <argos3/core/simulator/space/positional_indices/positional_index.h> 00021 #include <argos3/core/simulator/entity/embodied_entity.h> 00022 #include <argos3/core/simulator/entity/controllable_entity.h> 00023 00024 namespace argos { 00025 00026 /****************************************/ 00027 /****************************************/ 00028 00029 class CSpace : public CBaseConfigurableResource { 00030 00031 public: 00032 00053 typedef std::map <std::string, CAny, std::less <std::string> > TMapPerType; 00054 00076 typedef std::map <std::string, TMapPerType, std::less <std::string> > TMapPerTypePerId; 00077 00078 /****************************************/ 00079 /****************************************/ 00080 00081 public: 00082 00086 CSpace(); 00087 00091 virtual ~CSpace() {} 00092 00097 virtual void Init(TConfigurationNode& t_tree); 00098 00102 virtual void Reset(); 00103 00107 virtual void Destroy(); 00108 00112 inline UInt32 GetNumberEntities() const { 00113 return m_vecEntities.size(); 00114 } 00115 00122 inline CEntity::TVector& GetEntityVector() { 00123 return m_vecEntities; 00124 } 00125 00136 inline CEntity::TVector& GetRootEntityVector() { 00137 return m_vecRootEntities; 00138 } 00139 00146 inline CEntity& GetEntity(const std::string& str_id) { 00147 CEntity::TMap::const_iterator it = m_mapEntitiesPerId.find(str_id); 00148 if ( it != m_mapEntitiesPerId.end()) { 00149 return *(it->second); 00150 } 00151 THROW_ARGOSEXCEPTION("Unknown entity id \"" << str_id << 00152 "\" when requesting entity from space."); 00153 } 00154 00163 void GetEntitiesMatching(CEntity::TVector& t_buffer, 00164 const std::string& str_pattern); 00165 00170 inline CEntity::TMap& GetEntityMapPerId() { 00171 return m_mapEntitiesPerId; 00172 } 00173 00191 inline TMapPerTypePerId& GetEntityMapPerTypePerId() { 00192 return m_mapEntitiesPerTypePerId; 00193 } 00194 00211 TMapPerType& GetEntitiesByType(const std::string& str_type); 00212 00218 inline CFloorEntity& GetFloorEntity() { 00219 if(m_pcFloorEntity != NULL) return *m_pcFloorEntity; 00220 else THROW_ARGOSEXCEPTION("No floor entity has been added to the arena."); 00221 } 00222 00227 inline void SetFloorEntity(CFloorEntity& c_floor_entity) { 00228 m_pcFloorEntity = &c_floor_entity; 00229 } 00230 00243 virtual void Update(); 00244 00250 template <typename ENTITY> 00251 void AddEntity(ENTITY& c_entity) { 00252 std::string strEntityQualifiedName = c_entity.GetContext() + c_entity.GetId(); 00253 /* Check that the id of the entity is not already present */ 00254 if(m_mapEntitiesPerId.find(strEntityQualifiedName) != m_mapEntitiesPerId.end()) { 00255 THROW_ARGOSEXCEPTION("Error inserting a " << 00256 c_entity.GetTypeDescription() << 00257 " entity with id \"" << 00258 strEntityQualifiedName << 00259 "\". An entity with that id already exists."); 00260 } 00261 /* Add the entity to the indexes */ 00262 if(!c_entity.HasParent()) { 00263 m_vecRootEntities.push_back(&c_entity); 00264 } 00265 m_vecEntities.push_back(&c_entity); 00266 m_mapEntitiesPerId[strEntityQualifiedName] = &c_entity; 00267 m_mapEntitiesPerTypePerId[c_entity.GetTypeDescription()][strEntityQualifiedName] = &c_entity; 00268 } 00269 00275 template <typename ENTITY> 00276 void RemoveEntity(ENTITY& c_entity) { 00277 std::string strEntityQualifiedName = c_entity.GetContext() + c_entity.GetId(); 00278 /* Search for entity in the index per type */ 00279 TMapPerTypePerId::iterator itMapPerType = m_mapEntitiesPerTypePerId.find(c_entity.GetTypeDescription()); 00280 if(itMapPerType != m_mapEntitiesPerTypePerId.end()) { 00281 /* Search for entity in the index per type per id */ 00282 TMapPerType::iterator itMapPerTypePerId = itMapPerType->second.find(strEntityQualifiedName); 00283 if(itMapPerTypePerId != itMapPerType->second.end()) { 00284 /* Remove the entity from the indexes */ 00285 CEntity::TVector::iterator itVec = find(m_vecEntities.begin(), 00286 m_vecEntities.end(), 00287 &c_entity); 00288 m_vecEntities.erase(itVec); 00289 CEntity::TMap::iterator itMap = m_mapEntitiesPerId.find(strEntityQualifiedName); 00290 itMapPerType->second.erase(itMapPerTypePerId); 00291 m_mapEntitiesPerId.erase(itMap); 00292 if(!c_entity.HasParent()) { 00293 CEntity::TVector::iterator itRootVec = find(m_vecRootEntities.begin(), 00294 m_vecRootEntities.end(), 00295 &c_entity); 00296 m_vecRootEntities.erase(itRootVec); 00297 } 00298 /* Remove entity object */ 00299 c_entity.Destroy(); 00300 delete &c_entity; 00301 return; 00302 } 00303 } 00304 THROW_ARGOSEXCEPTION("CSpace::RemoveEntity() : Entity \"" << 00305 strEntityQualifiedName << 00306 "\" has not been found in the indexes."); 00307 } 00308 00314 inline UInt32 GetSimulationClock() const { 00315 return m_unSimulationClock; 00316 } 00317 00323 inline void SetSimulationClock(UInt32 un_simulation_clock) { 00324 m_unSimulationClock = un_simulation_clock; 00325 } 00326 00332 inline void IncreaseSimulationClock(UInt32 un_increase = 1) { 00333 m_unSimulationClock += un_increase; 00334 } 00335 00340 inline const CVector3& GetArenaSize() const { 00341 return m_cArenaSize; 00342 } 00343 00348 inline void SetArenaSize(const CVector3& c_size) { 00349 m_cArenaSize = c_size; 00350 } 00351 00356 inline const CVector3& GetArenaCenter() const { 00357 return m_cArenaCenter; 00358 } 00359 00364 inline void SetArenaCenter(const CVector3& c_center) { 00365 m_cArenaCenter = c_center; 00366 } 00367 00368 virtual void AddControllableEntity(CControllableEntity& c_entity); 00369 virtual void RemoveControllableEntity(CControllableEntity& c_entity); 00370 virtual void AddEntityToPhysicsEngine(CEmbodiedEntity& c_entity); 00371 00372 protected: 00373 00374 virtual void UpdateControllableEntities() = 0; 00375 virtual void UpdatePhysics() = 0; 00376 virtual void UpdateMedia() = 0; 00377 00378 void Distribute(TConfigurationNode& t_tree); 00379 00380 void AddBoxStrip(TConfigurationNode& t_tree); 00381 00382 protected: 00383 00384 friend class CSpaceOperationAddControllableEntity; 00385 friend class CSpaceOperationRemoveControllableEntity; 00386 friend class CSpaceOperationAddEmbodiedEntity; 00387 00388 protected: 00389 00391 UInt32 m_unSimulationClock; 00392 00394 CVector3 m_cArenaCenter; 00395 00397 CVector3 m_cArenaSize; 00398 00400 CEntity::TVector m_vecEntities; 00401 00403 CEntity::TVector m_vecRootEntities; 00404 00406 CEntity::TMap m_mapEntitiesPerId; 00407 00411 TMapPerTypePerId m_mapEntitiesPerTypePerId; 00412 00414 CControllableEntity::TVector m_vecControllableEntities; 00415 00417 CFloorEntity* m_pcFloorEntity; 00418 00420 CPhysicsEngine::TVector* m_ptPhysicsEngines; 00421 00423 CMedium::TVector* m_ptMedia; 00424 }; 00425 00426 /****************************************/ 00427 /****************************************/ 00428 00429 template <typename ACTION> 00430 class CSpaceOperation : public CEntityOperation<ACTION, CSpace, void> { 00431 public: 00432 virtual ~CSpaceOperation() {} 00433 }; 00434 00435 class CSpaceOperationAddEntity : public CSpaceOperation<CSpaceOperationAddEntity> { 00436 public: 00437 virtual ~CSpaceOperationAddEntity() {} 00438 }; 00439 class CSpaceOperationRemoveEntity : public CSpaceOperation<CSpaceOperationRemoveEntity> { 00440 public: 00441 virtual ~CSpaceOperationRemoveEntity() {} 00442 }; 00443 00444 } 00445 00446 /****************************************/ 00447 /****************************************/ 00448 00449 #define SPACE_OPERATION_ADD_ENTITY(ENTITY) \ 00450 class CSpaceOperationAdd ## ENTITY : public CSpaceOperationAddEntity { \ 00451 public: \ 00452 void ApplyTo(CSpace& c_space, ENTITY& c_entity) { \ 00453 c_space.AddEntity(c_entity); \ 00454 } \ 00455 }; 00456 00457 #define SPACE_OPERATION_REMOVE_ENTITY(ENTITY) \ 00458 class CSpaceOperationRemove ## ENTITY : public CSpaceOperationRemoveEntity { \ 00459 public: \ 00460 void ApplyTo(CSpace& c_space, ENTITY& c_entity) { \ 00461 c_space.RemoveEntity(c_entity); \ 00462 } \ 00463 }; 00464 00465 #define REGISTER_SPACE_OPERATION(ACTION, OPERATION, ENTITY) \ 00466 REGISTER_ENTITY_OPERATION(ACTION, CSpace, OPERATION, void, ENTITY); 00467 00468 #define REGISTER_STANDARD_SPACE_OPERATION_ADD_ENTITY(ENTITY) \ 00469 SPACE_OPERATION_ADD_ENTITY(ENTITY) \ 00470 REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, \ 00471 CSpaceOperationAdd ## ENTITY, \ 00472 ENTITY); 00473 00474 #define REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(ENTITY) \ 00475 SPACE_OPERATION_REMOVE_ENTITY(ENTITY) \ 00476 REGISTER_SPACE_OPERATION(CSpaceOperationRemoveEntity, \ 00477 CSpaceOperationRemove ## ENTITY, \ 00478 ENTITY); 00479 00480 #define REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(ENTITY) \ 00481 REGISTER_STANDARD_SPACE_OPERATION_ADD_ENTITY(ENTITY) \ 00482 REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(ENTITY) 00483 00484 #endif