space.h

Go to the documentation of this file.

 
#ifndef SPACE_H
#define SPACE_H
 
namespace argos {
   class CSpace;
   class CRay3;
   class CFloorEntity;
   class CSimulator;
}
 
#include <functional>
#include <string>
 
#include <argos3/core/utility/datatypes/any.h>
#include <argos3/core/simulator/medium/medium.h>
#include <argos3/core/simulator/entity/embodied_entity.h>
#include <argos3/core/simulator/entity/controllable_entity.h>
 
namespace argos {
 
   /****************************************/
   /****************************************/
 
   class CSpace : public CBaseConfigurableResource {
 
   public:
 
      typedef std::map <std::string, CAny, std::less <std::string> > TMapPerType;
 
      typedef std::map <std::string, TMapPerType, std::less <std::string> > TMapPerTypePerId;
 
      typedef std::function<void(CControllableEntity*)> TControllableEntityIterCBType;
 
      /****************************************/
      /****************************************/
 
   public:
 
      CSpace();
 
      virtual ~CSpace() {}
 
      virtual void Init(TConfigurationNode& t_tree);
 
      virtual void Reset();
 
      virtual void Destroy();
 
      inline UInt32 GetNumberEntities() const {
         return m_vecEntities.size();
      }
 
      inline CEntity::TVector& GetEntityVector() {
         return m_vecEntities;
      }
 
      inline CEntity::TVector& GetRootEntityVector() {
         return m_vecRootEntities;
      }
 
      inline CEntity& GetEntity(const std::string& str_id) {
         CEntity::TMap::const_iterator it = m_mapEntitiesPerId.find(str_id);
         if ( it != m_mapEntitiesPerId.end()) {
            return *(it->second);
         }
         THROW_ARGOSEXCEPTION("Unknown entity id \"" << str_id <<
                              "\" when requesting entity from space.");
      }
 
      void GetEntitiesMatching(CEntity::TVector& t_buffer,
                               const std::string& str_pattern);
 
      inline CEntity::TMap& GetEntityMapPerId() {
         return m_mapEntitiesPerId;
      }
 
      inline TMapPerTypePerId& GetEntityMapPerTypePerId() {
         return m_mapEntitiesPerTypePerId;
      }
 
      TMapPerType& GetEntitiesByType(const std::string& str_type) {
        return GetEntitiesByTypeImpl(str_type);
      }
 
      const TMapPerType& GetEntitiesByType(const std::string& str_type) const {
        return GetEntitiesByTypeImpl(str_type);
      }
 
      inline CFloorEntity& GetFloorEntity() {
         if(m_pcFloorEntity != NULL) return *m_pcFloorEntity;
         else THROW_ARGOSEXCEPTION("No floor entity has been added to the arena.");
      }
 
      inline void SetFloorEntity(CFloorEntity& c_floor_entity) {
         m_pcFloorEntity = &c_floor_entity;
      }
 
      virtual void Update();
 
      template <typename ENTITY>
      void AddEntity(ENTITY& c_entity) {
         std::string strEntityQualifiedName = c_entity.GetContext() + c_entity.GetId();
         /* Check that the id of the entity is not already present */
         if(m_mapEntitiesPerId.find(strEntityQualifiedName) != m_mapEntitiesPerId.end()) {
            THROW_ARGOSEXCEPTION("Error inserting a " <<
                                 c_entity.GetTypeDescription() <<
                                 " entity with id \"" <<
                                 strEntityQualifiedName <<
                                 "\". An entity with that id already exists.");
         }
         /* Add the entity to the indexes */
         if(!c_entity.HasParent()) {
            m_vecRootEntities.push_back(&c_entity);
         }
         /* Calculate index of in the global vector */
         size_t unIdx =
            !m_vecEntities.empty() ?
            m_vecEntities.back()->GetIndex() + 1
            :
            0;
         /* Add entity to global vector */
         m_vecEntities.push_back(&c_entity);
         c_entity.SetIndex(unIdx);
         m_mapEntitiesPerId[strEntityQualifiedName] = &c_entity;
         m_mapEntitiesPerTypePerId[c_entity.GetTypeDescription()][strEntityQualifiedName] = &c_entity;
      }
 
      template <typename ENTITY>
      void RemoveEntity(ENTITY& c_entity) {
         std::string strEntityQualifiedName = c_entity.GetContext() + c_entity.GetId();
         /* Search for entity in the index per type */
         TMapPerTypePerId::iterator itMapPerType = m_mapEntitiesPerTypePerId.find(c_entity.GetTypeDescription());
         if(itMapPerType != m_mapEntitiesPerTypePerId.end()) {
            /* Search for entity in the index per type per id */
            TMapPerType::iterator itMapPerTypePerId = itMapPerType->second.find(strEntityQualifiedName);
            if(itMapPerTypePerId != itMapPerType->second.end()) {
               /* Remove the entity from the indexes */
               CEntity::TVector::iterator itVec = find(m_vecEntities.begin(),
                                                       m_vecEntities.end(),
                                                       &c_entity);
               m_vecEntities.erase(itVec);
               CEntity::TMap::iterator itMap = m_mapEntitiesPerId.find(strEntityQualifiedName);
               itMapPerType->second.erase(itMapPerTypePerId);
               m_mapEntitiesPerId.erase(itMap);
               if(!c_entity.HasParent()) {
                  CEntity::TVector::iterator itRootVec = find(m_vecRootEntities.begin(),
                                                              m_vecRootEntities.end(),
                                                              &c_entity);
                  m_vecRootEntities.erase(itRootVec);
               }
               /* Remove entity object */
               c_entity.Destroy();
               delete &c_entity;
               return;
            }
         }
         THROW_ARGOSEXCEPTION("CSpace::RemoveEntity() : Entity \"" <<
                              strEntityQualifiedName <<
                              "\" has not been found in the indexes.");
      }
 
      inline UInt32 GetSimulationClock() const {
         return m_unSimulationClock;
      }
 
      inline void SetSimulationClock(UInt32 un_simulation_clock) {
         m_unSimulationClock = un_simulation_clock;
      }
 
      inline void IncreaseSimulationClock(UInt32 un_increase = 1) {
         m_unSimulationClock += un_increase;
      }
 
      inline const CVector3& GetArenaSize() const {
         return m_cArenaSize;
      }
 
      inline void SetArenaSize(const CVector3& c_size) {
         m_cArenaSize = c_size;
         m_cArenaLimits.Set(m_cArenaCenter - m_cArenaSize,
                            m_cArenaCenter + m_cArenaSize);
      }
 
      inline const CVector3& GetArenaCenter() const {
         return m_cArenaCenter;
      }
 
      inline void SetArenaCenter(const CVector3& c_center) {
         m_cArenaCenter = c_center;
         m_cArenaLimits.Set(m_cArenaCenter - m_cArenaSize,
                            m_cArenaCenter + m_cArenaSize);
      }
 
      inline const CRange<CVector3>& GetArenaLimits() const {
         return m_cArenaLimits;
      }
 
      virtual void AddControllableEntity(CControllableEntity& c_entity);
      virtual void RemoveControllableEntity(CControllableEntity& c_entity);
      virtual void AddEntityToPhysicsEngine(CEmbodiedEntity& c_entity);
 
      virtual void IterateOverControllableEntities(
          const TControllableEntityIterCBType& c_cb) = 0;
 
   protected:
 
      virtual void UpdateControllableEntitiesAct() = 0;
      virtual void UpdatePhysics() = 0;
      virtual void UpdateMedia() = 0;
      virtual void UpdateControllableEntitiesSenseStep() = 0;
 
      virtual void ControllableEntityIterationWaitAbort() {}
 
      void Distribute(TConfigurationNode& t_tree);
 
      void AddBoxStrip(TConfigurationNode& t_tree);
 
      bool ControllableEntityIterationEnabled() const {
        return nullptr != m_cbControllableEntityIter;
      }
 
   protected:
 
      friend class CSpaceOperationAddControllableEntity;
      friend class CSpaceOperationRemoveControllableEntity;
      friend class CSpaceOperationAddEmbodiedEntity;
 
   protected:
 
      /* The active simulator instance */
      CSimulator& m_cSimulator;
 
      UInt32 m_unSimulationClock;
 
      CVector3 m_cArenaCenter;
 
      CVector3 m_cArenaSize;
 
      CRange<CVector3> m_cArenaLimits;
 
      CEntity::TVector m_vecEntities;
 
      CEntity::TVector m_vecRootEntities;
 
      CEntity::TMap m_mapEntitiesPerId;
 
      TMapPerTypePerId m_mapEntitiesPerTypePerId;
 
      CControllableEntity::TVector m_vecControllableEntities;
 
      CFloorEntity* m_pcFloorEntity;
 
      CPhysicsEngine::TVector* m_ptPhysicsEngines;
 
      CMedium::TVector* m_ptMedia;
 
      TControllableEntityIterCBType m_cbControllableEntityIter{nullptr};
 
  private:
      TMapPerType& GetEntitiesByTypeImpl(const std::string& str_type) const;
 
   };
 
   /****************************************/
   /****************************************/
 
   template <typename ACTION>
   class CSpaceOperation : public CEntityOperation<ACTION, CSpace, void> {
   public:
      virtual ~CSpaceOperation() {}
   };
 
   class CSpaceOperationAddEntity : public CSpaceOperation<CSpaceOperationAddEntity> {
   public:
      virtual ~CSpaceOperationAddEntity() {}
   };
   class CSpaceOperationRemoveEntity : public CSpaceOperation<CSpaceOperationRemoveEntity> {
   public:
      virtual ~CSpaceOperationRemoveEntity() {}
   };
 
}
 
   /****************************************/
   /****************************************/
 
#define SPACE_OPERATION_ADD_ENTITY(ENTITY)                                 \
   class CSpaceOperationAdd ## ENTITY : public CSpaceOperationAddEntity {  \
   public:                                                                 \
      void ApplyTo(CSpace& c_space, ENTITY& c_entity) {                    \
         c_space.AddEntity(c_entity);                                      \
      }                                                                    \
   };
 
#define SPACE_OPERATION_REMOVE_ENTITY(ENTITY)                                   \
   class CSpaceOperationRemove ## ENTITY : public CSpaceOperationRemoveEntity { \
   public:                                                                      \
      void ApplyTo(CSpace& c_space, ENTITY& c_entity) {                         \
         c_space.RemoveEntity(c_entity);                                        \
      }                                                                         \
   };
 
#define REGISTER_SPACE_OPERATION(ACTION, OPERATION, ENTITY)             \
   REGISTER_ENTITY_OPERATION(ACTION, CSpace, OPERATION, void, ENTITY);
 
#define REGISTER_STANDARD_SPACE_OPERATION_ADD_ENTITY(ENTITY)            \
   SPACE_OPERATION_ADD_ENTITY(ENTITY)                                   \
   REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity,                   \
                            CSpaceOperationAdd ## ENTITY,               \
                            ENTITY);
 
#define REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(ENTITY)         \
   SPACE_OPERATION_REMOVE_ENTITY(ENTITY)                                \
   REGISTER_SPACE_OPERATION(CSpaceOperationRemoveEntity,                \
                            CSpaceOperationRemove ## ENTITY,            \
                            ENTITY);
 
#define REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(ENTITY) \
   REGISTER_STANDARD_SPACE_OPERATION_ADD_ENTITY(ENTITY)      \
   REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(ENTITY)
 
#endif