ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/simulator/space/space.h
Go to the documentation of this file.
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