ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/simulator/entity/embodied_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "embodied_entity.h"
00008 #include "composable_entity.h"
00009 #include <argos3/core/simulator/space/space.h>
00010 #include <argos3/core/simulator/simulator.h>
00011 #include <argos3/core/utility/string_utilities.h>
00012 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
00013 
00014 namespace argos {
00015 
00016    /****************************************/
00017    /****************************************/
00018 
00019    CEmbodiedEntity::CEmbodiedEntity(CComposableEntity* pc_parent) :
00020       CPositionalEntity(pc_parent),
00021       m_bMovable(true),
00022       m_sBoundingBox(NULL) {}
00023 
00024    /****************************************/
00025    /****************************************/
00026 
00027    CEmbodiedEntity::CEmbodiedEntity(CComposableEntity* pc_parent,
00028                                     const std::string& str_id,
00029                                     const CVector3& c_position,
00030                                     const CQuaternion& c_orientation,
00031                                     bool b_movable) :
00032       CPositionalEntity(pc_parent,
00033                         str_id,
00034                         c_position,
00035                         c_orientation),
00036       m_bMovable(b_movable),
00037       m_sBoundingBox(NULL) {}
00038 
00039    /****************************************/
00040    /****************************************/
00041 
00042    CEmbodiedEntity::~CEmbodiedEntity() {
00043       if(!m_bMovable && m_sBoundingBox != NULL) {
00044          delete m_sBoundingBox;
00045       }
00046    }
00047 
00048    /****************************************/
00049    /****************************************/
00050 
00051    void CEmbodiedEntity::Init(TConfigurationNode& t_tree) {
00052       try {
00053          CPositionalEntity::Init(t_tree);
00054          m_bMovable = true;
00055       }
00056       catch(CARGoSException& ex) {
00057          THROW_ARGOSEXCEPTION_NESTED("Failed to initialize embodied entity \"" << GetId() << "\".", ex);
00058       }
00059    }
00060 
00061    /****************************************/
00062    /****************************************/
00063 
00064    const SBoundingBox& CEmbodiedEntity::GetBoundingBox() const {
00065       if(GetPhysicsModelsNum() == 0) {
00066          /* No engine associated to this entity */
00067          THROW_ARGOSEXCEPTION("CEmbodiedEntity::GetBoundingBox() : entity \"" << GetId() << "\" is not associated to any engine");
00068       }
00069       return *m_sBoundingBox;
00070    }
00071 
00072    /****************************************/
00073    /****************************************/
00074 
00075    UInt32 CEmbodiedEntity::GetPhysicsModelsNum() const {
00076       return m_tPhysicsModelVector.size();
00077    }
00078 
00079    /****************************************/
00080    /****************************************/
00081 
00082    void CEmbodiedEntity::AddPhysicsModel(const std::string& str_engine_id,
00083                                          CPhysicsModel& c_physics_model) {
00084       if(m_bMovable && GetPhysicsModelsNum() > 0) {
00085          THROW_ARGOSEXCEPTION(GetId() << " is movable embodied entity and can't have more than 1 physics engine entity associated");
00086       }
00087       m_tPhysicsModelMap[str_engine_id] = &c_physics_model;
00088       m_tPhysicsModelVector.push_back(&c_physics_model);
00089       CalculateBoundingBox();
00090    }
00091 
00092    /****************************************/
00093    /****************************************/
00094 
00095    void CEmbodiedEntity::RemovePhysicsModel(const std::string& str_engine_id) {
00096       CPhysicsModel::TMap::iterator itMap = m_tPhysicsModelMap.find(str_engine_id);
00097       if(itMap == m_tPhysicsModelMap.end()) {
00098          THROW_ARGOSEXCEPTION("Entity \"" << GetId() << "\" has no associated entity in physics engine " << str_engine_id);
00099       }
00100       CPhysicsModel::TVector::iterator itVec = std::find(m_tPhysicsModelVector.begin(),
00101                                                          m_tPhysicsModelVector.end(),
00102                                                          itMap->second);
00103       m_tPhysicsModelMap.erase(itMap);
00104       m_tPhysicsModelVector.erase(itVec);
00105       CalculateBoundingBox();
00106    }
00107 
00108    /****************************************/
00109    /****************************************/
00110 
00111    const CPhysicsModel& CEmbodiedEntity::GetPhysicsModel(size_t un_idx) const {
00112       if(un_idx > m_tPhysicsModelVector.size()) {
00113          THROW_ARGOSEXCEPTION("CEmbodiedEntity::GetPhysicsModel: entity \"" << GetId() << "\": the passed index " << un_idx << " is out of bounds, the max allowed is " << m_tPhysicsModelVector.size());
00114       }
00115       return *m_tPhysicsModelVector[un_idx];
00116    }
00117 
00118    /****************************************/
00119    /****************************************/
00120 
00121    CPhysicsModel& CEmbodiedEntity::GetPhysicsModel(size_t un_idx) {
00122       if(un_idx > m_tPhysicsModelVector.size()) {
00123          THROW_ARGOSEXCEPTION("CEmbodiedEntity::GetPhysicsModel: entity \"" << GetId() << "\": the passed index " << un_idx << " is out of bounds, the max allowed is " << m_tPhysicsModelVector.size());
00124       }
00125       return *m_tPhysicsModelVector[un_idx];
00126    }
00127 
00128    /****************************************/
00129    /****************************************/
00130 
00131    const CPhysicsModel& CEmbodiedEntity::GetPhysicsModel(const std::string& str_engine_id) const {
00132       CPhysicsModel::TMap::const_iterator it = m_tPhysicsModelMap.find(str_engine_id);
00133       if(it == m_tPhysicsModelMap.end()) {
00134          THROW_ARGOSEXCEPTION("Entity \"" << GetId() << "\" has no associated entity in physics engine \"" << str_engine_id << "\"");
00135       }
00136       return *(it->second);
00137    }
00138 
00139    /****************************************/
00140    /****************************************/
00141 
00142    CPhysicsModel& CEmbodiedEntity::GetPhysicsModel(const std::string& str_engine_id) {
00143       CPhysicsModel::TMap::iterator it = m_tPhysicsModelMap.find(str_engine_id);
00144       if(it == m_tPhysicsModelMap.end()) {
00145          THROW_ARGOSEXCEPTION("Entity \"" << GetId() << "\" has no associated entity in physics engine \"" << str_engine_id << "\"");
00146       }
00147       return *(it->second);
00148    }
00149 
00150    /****************************************/
00151    /****************************************/
00152 
00153    bool CEmbodiedEntity::MoveTo(const CVector3& c_position,
00154                                 const CQuaternion& c_orientation,
00155                                 bool b_check_only) {
00156       bool bNoCollision = true;
00157       for(CPhysicsModel::TVector::const_iterator it = m_tPhysicsModelVector.begin();
00158           it != m_tPhysicsModelVector.end() && bNoCollision; ++it) {
00159          if(! (*it)->MoveTo(c_position, c_orientation, b_check_only)) {
00160             bNoCollision = false;
00161          }
00162       }
00163       if(bNoCollision && !b_check_only) {
00164          /* Update space position */
00165          SetPosition(c_position);
00166          SetOrientation(c_orientation);
00167          if( HasParent() ) {
00168             CComposableEntity* pcEntity = dynamic_cast<CComposableEntity*>(&GetParent());
00169             if( pcEntity != NULL ) {
00170                pcEntity->Update();
00171             }
00172          }
00173          return true;
00174       }
00175       else {
00176          /* No Collision or check only, undo changes */
00177          for(CPhysicsModel::TVector::const_iterator it = m_tPhysicsModelVector.begin();
00178              it != m_tPhysicsModelVector.end(); ++it) {
00179             (*it)->MoveTo(GetPosition(), GetOrientation());
00180          }
00181          if(!bNoCollision) {
00182             /* Collision */
00183             return false;
00184          }
00185          else {
00186             /* No collision, it was a check-only */
00187             return true;
00188          }
00189       }
00190    }
00191 
00192    /****************************************/
00193    /****************************************/
00194 
00195 #define CHECK_CORNER(MINMAX, COORD, OP)                                 \
00196    if(m_sBoundingBox->MINMAX ## Corner.Get ## COORD() OP sBBox.MINMAX ## Corner.Get ## COORD()) { \
00197       m_sBoundingBox->MINMAX ## Corner.Set ## COORD(sBBox.MINMAX ## Corner.Get ## COORD()); \
00198    }
00199 
00200    void CEmbodiedEntity::CalculateBoundingBox() {
00201       if(GetPhysicsModelsNum() > 0) {
00202          /*
00203           * There is at least one physics engine entity associated
00204           */
00205          if(m_bMovable) {
00206             /* The bounding box points directly to the associated model bounding box */
00207             m_sBoundingBox = &m_tPhysicsModelVector[0]->GetBoundingBox();
00208          }
00209          else {
00210             /* The bounding box is obtained taking the extrema of all the bboxes of all the engines */
00211             if(m_sBoundingBox == NULL) {
00212                m_sBoundingBox = new SBoundingBox();
00213             }
00214             *m_sBoundingBox = m_tPhysicsModelVector[0]->GetBoundingBox();
00215             for(size_t i = 1; i < GetPhysicsModelsNum(); ++i) {
00216                const SBoundingBox& sBBox = m_tPhysicsModelVector[0]->GetBoundingBox();
00217                CHECK_CORNER(Min, X, >);
00218                CHECK_CORNER(Min, Y, >);
00219                CHECK_CORNER(Min, Z, >);
00220                CHECK_CORNER(Max, X, <);
00221                CHECK_CORNER(Max, Y, <);
00222                CHECK_CORNER(Max, Z, <);
00223             }
00224          }
00225       }
00226       else {
00227          /*
00228           * No physics engine entity associated
00229           */
00230          if(! m_bMovable && m_sBoundingBox != NULL) {
00231             /* A non-movable entity has its own bounding box, delete it */
00232             delete m_sBoundingBox;
00233          }
00234          m_sBoundingBox = NULL;
00235       }
00236    }
00237 
00238    /****************************************/
00239    /****************************************/
00240 
00241    bool CEmbodiedEntity::IsCollidingWithSomething() const {
00242       /* If no model is associated, you can't call this function */
00243       if(m_tPhysicsModelVector.empty()) {
00244          THROW_ARGOSEXCEPTION("CEmbodiedEntity::IsCollidingWithSomething() called on entity \"" << GetId() << "\", but this entity has not been added to any physics engine.");
00245       }
00246       /* Special case: if there is only one model, check that directly */
00247       if(m_tPhysicsModelVector.size() == 1) {
00248          return m_tPhysicsModelVector[0]->IsCollidingWithSomething();
00249       }
00250       /* Multiple associations, go through them */
00251       else {
00252          /* Return true at the first detected collision */
00253          for(size_t i = 0; i < m_tPhysicsModelVector.size(); ++i) {
00254             if(m_tPhysicsModelVector[i]->IsCollidingWithSomething()) {
00255                return true;
00256             }
00257          }
00258          /* If you get here it's because there are collisions */
00259          return false;
00260       }
00261    }
00262 
00263    /****************************************/
00264    /****************************************/
00265 
00266    void CEmbodiedEntitySpaceHashUpdater::operator()(CAbstractSpaceHash<CEmbodiedEntity>& c_space_hash,
00267                                                     CEmbodiedEntity& c_element) {
00268       /* Translate the min corner of the bounding box into the map's coordinate */
00269       c_space_hash.SpaceToHashTable(m_nMinX, m_nMinY, m_nMinZ, c_element.GetBoundingBox().MinCorner);
00270       /* Translate the max corner of the bounding box into the map's coordinate */
00271       c_space_hash.SpaceToHashTable(m_nMaxX, m_nMaxY, m_nMaxZ, c_element.GetBoundingBox().MaxCorner);
00272       /* Finally, go through the affected cells and update them */
00273       for(SInt32 nK = m_nMinZ; nK <= m_nMaxZ; ++nK) {
00274          for(SInt32 nJ = m_nMinY; nJ <= m_nMaxY; ++nJ) {
00275             for(SInt32 nI = m_nMinX; nI <= m_nMaxX; ++nI) {
00276                c_space_hash.UpdateCell(nI, nJ, nK, c_element);
00277             }
00278          }
00279       }
00280    }
00281 
00282    /****************************************/
00283    /****************************************/
00284 
00285    CEmbodiedEntityGridUpdater::CEmbodiedEntityGridUpdater(CGrid<CEmbodiedEntity>& c_grid) :
00286       m_cGrid(c_grid) {}
00287 
00288    bool CEmbodiedEntityGridUpdater::operator()(CEmbodiedEntity& c_entity) {
00289       try {
00290          /* Get cell of bb min corner, clamping it if is out of bounds */
00291          m_cGrid.PositionToCell(m_nMinI, m_nMinJ, m_nMinK, c_entity.GetBoundingBox().MinCorner);
00292          m_cGrid.ClampCoordinates(m_nMinI, m_nMinJ, m_nMinK);
00293          /* Get cell of bb max corner, clamping it if is out of bounds */
00294          m_cGrid.PositionToCell(m_nMaxI, m_nMaxJ, m_nMaxK, c_entity.GetBoundingBox().MaxCorner);
00295          m_cGrid.ClampCoordinates(m_nMaxI, m_nMaxJ, m_nMaxK);
00296          /* Go through cells */
00297          for(SInt32 m_nK = m_nMinK; m_nK <= m_nMaxK; ++m_nK) {
00298             for(SInt32 m_nJ = m_nMinJ; m_nJ <= m_nMaxJ; ++m_nJ) {
00299                for(SInt32 m_nI = m_nMinI; m_nI <= m_nMaxI; ++m_nI) {
00300                   m_cGrid.UpdateCell(m_nI, m_nJ, m_nK, c_entity);
00301                }
00302             }
00303          }
00304          /* Continue with the other entities */
00305          return true;
00306       }
00307       catch(CARGoSException& ex) {
00308          THROW_ARGOSEXCEPTION_NESTED("While updating the embodied entity grid for embodied entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex);
00309       }
00310    }
00311 
00312    /****************************************/
00313    /****************************************/
00314 
00318    class CSpaceOperationAddEmbodiedEntity : public CSpaceOperationAddEntity {
00319    public:
00320       void ApplyTo(CSpace& c_space, CEmbodiedEntity& c_entity) {
00321          /* Add entity to space */
00322          c_space.AddEntity(c_entity);
00323          /* Try to add entity to physics engine(s) */
00324          c_space.AddEntityToPhysicsEngine(c_entity);
00325       }
00326    };
00327    REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, CSpaceOperationAddEmbodiedEntity, CEmbodiedEntity);
00328 
00329    class CSpaceOperationRemoveEmbodiedEntity : public CSpaceOperationRemoveEntity {
00330    public:
00331       void ApplyTo(CSpace& c_space, CEmbodiedEntity& c_entity) {
00332          /* Get a reference to the root entity */
00333          CEntity* pcRoot = &c_entity;
00334          while(pcRoot->HasParent()) {
00335             pcRoot = &pcRoot->GetParent();
00336          }
00337          /* Remove entity from all physics engines */
00338          try {
00339             while(c_entity.GetPhysicsModelsNum() > 0) {
00340                c_entity.GetPhysicsModel(0).GetEngine().RemoveEntity(*pcRoot);
00341             }
00342          }
00343          catch(CARGoSException& ex) {
00344             /*
00345              * It is safe to ignore errors because they happen only when an entity
00346              * is completely removed from the space. In this case, the body is
00347              * first removed from the composable entity, and then the embodied entity
00348              * is asked to clear up the physics models. In turn, this last operation
00349              * searches for the body component, which is not there anymore.
00350              *
00351              * It is anyway useful to search for the body component because, when robots
00352              * are transferred from an engine to another, only the physics model is to be
00353              * removed.
00354              */
00355          }
00356          /* Remove entity from space */
00357          c_space.RemoveEntity(c_entity);
00358       }
00359    };
00360    REGISTER_SPACE_OPERATION(CSpaceOperationRemoveEntity, CSpaceOperationRemoveEmbodiedEntity, CEmbodiedEntity);
00365    /****************************************/
00366    /****************************************/
00367 
00368    bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem& s_item,
00369                                                  const CRay3& c_ray) {
00370       /* This variable is instantiated at the first call of this function, once and forever */
00371       static CSimulator& cSimulator = CSimulator::GetInstance();
00372       /* Initialize s_item */
00373       s_item.IntersectedEntity = NULL;
00374       s_item.TOnRay = 1.0f;
00375       /* Create a reference to the vector of physics engines */
00376       CPhysicsEngine::TVector& vecEngines = cSimulator.GetPhysicsEngines();
00377       /* Ask each engine to perform the ray query, keeping the closest intersection */
00378       Real fTOnRay;
00379       CEmbodiedEntity* pcEntity;
00380       for(size_t i = 0; i < vecEngines.size(); ++i) {
00381          pcEntity = vecEngines[i]->CheckIntersectionWithRay(fTOnRay, c_ray);
00382          if(pcEntity != NULL) {
00383             if(s_item.TOnRay > fTOnRay) {
00384                s_item.TOnRay = fTOnRay;
00385                s_item.IntersectedEntity = pcEntity;
00386             }
00387          }
00388       }
00389       /* Return true if an intersection was found */
00390       return (s_item.IntersectedEntity != NULL);
00391    }
00392 
00393    /****************************************/
00394    /****************************************/
00395 
00396    bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem& s_item,
00397                                                  const CRay3& c_ray,
00398                                                  CEmbodiedEntity& c_entity) {
00399       /* This variable is instantiated at the first call of this function, once and forever */
00400       static CSimulator& cSimulator = CSimulator::GetInstance();
00401       /* Initialize s_item */
00402       s_item.IntersectedEntity = NULL;
00403       s_item.TOnRay = 1.0f;
00404       /* Create a reference to the vector of physics engines */
00405       CPhysicsEngine::TVector& vecEngines = cSimulator.GetPhysicsEngines();
00406       /* Ask each engine to perform the ray query, keeping the closest intersection
00407          that does not involve the entity to discard */
00408       Real fTOnRay;
00409       CEmbodiedEntity* pcEntity;
00410       for(size_t i = 0; i < vecEngines.size(); ++i) {
00411          pcEntity = vecEngines[i]->CheckIntersectionWithRay(fTOnRay, c_ray);
00412          if((pcEntity != NULL) &&
00413             (pcEntity != &c_entity)){
00414             if(s_item.TOnRay > fTOnRay) {
00415                s_item.TOnRay = fTOnRay;
00416                s_item.IntersectedEntity = pcEntity;
00417             }
00418          }
00419       }
00420       /* Return true if an intersection was found */
00421       return (s_item.IntersectedEntity != NULL);
00422    }
00423 
00424    /****************************************/
00425    /****************************************/
00426 
00427 }