ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }