ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef EMBODIED_ENTITY_H 00008 #define EMBODIED_ENTITY_H 00009 00010 namespace argos { 00011 class CEmbodiedEntity; 00012 class CRotationMatrix3; 00013 } 00014 00015 #include <argos3/core/simulator/entity/positional_entity.h> 00016 #include <argos3/core/simulator/space/positional_indices/grid.h> 00017 #include <argos3/core/simulator/space/positional_indices/space_hash.h> 00018 #include <argos3/core/utility/datatypes/set.h> 00019 #include <argos3/core/utility/math/ray3.h> 00020 #include <argos3/core/utility/math/quaternion.h> 00021 #include <argos3/core/simulator/physics_engine/physics_engine.h> 00022 #include <argos3/core/simulator/physics_engine/physics_model.h> 00023 #include <algorithm> 00024 00025 namespace argos { 00026 00049 class CEmbodiedEntity : public CPositionalEntity { 00050 00051 public: 00052 00053 ENABLE_VTABLE(); 00054 00055 public: 00056 00062 CEmbodiedEntity(CComposableEntity* pc_parent); 00063 00075 CEmbodiedEntity(CComposableEntity* pc_parent, 00076 const std::string& str_id, 00077 const CVector3& c_position = CVector3(), 00078 const CQuaternion& c_orientation = CQuaternion(), 00079 bool b_movable = true); 00080 00084 virtual ~CEmbodiedEntity(); 00085 00094 virtual void Init(TConfigurationNode& t_tree); 00095 00100 inline bool IsMovable() const { 00101 return m_bMovable; 00102 } 00103 00110 inline void SetMovable(bool b_movable) { 00111 m_bMovable = b_movable; 00112 } 00113 00123 const SBoundingBox& GetBoundingBox() const; 00124 00129 UInt32 GetPhysicsModelsNum() const; 00130 00138 virtual void AddPhysicsModel(const std::string& str_engine_id, 00139 CPhysicsModel& c_physics_model); 00140 00147 void RemovePhysicsModel(const std::string& str_engine_id); 00148 00155 const CPhysicsModel& GetPhysicsModel(size_t un_idx) const; 00156 00163 CPhysicsModel& GetPhysicsModel(size_t un_idx); 00164 00171 const CPhysicsModel& GetPhysicsModel(const std::string& str_engine_id) const; 00172 00179 CPhysicsModel& GetPhysicsModel(const std::string& str_engine_id); 00180 00192 virtual bool MoveTo(const CVector3& c_position, 00193 const CQuaternion& c_orientation, 00194 bool b_check_only = false); 00195 00200 virtual bool IsCollidingWithSomething() const; 00201 00202 virtual std::string GetTypeDescription() const { 00203 return "body"; 00204 } 00205 00206 protected: 00207 00214 void CalculateBoundingBox(); 00215 00216 protected: 00217 00218 bool m_bMovable; 00219 CPhysicsModel::TMap m_tPhysicsModelMap; 00220 CPhysicsModel::TVector m_tPhysicsModelVector; 00221 SBoundingBox* m_sBoundingBox; 00222 00223 }; 00224 00225 /****************************************/ 00226 /****************************************/ 00227 00228 typedef std::vector<CEmbodiedEntity*> TEmbodiedEntityVector; 00229 typedef std::map<std::string, CEmbodiedEntity*> TEmbodiedEntityMap; 00230 typedef CSet<CEmbodiedEntity*> TEmbodiedEntitySet; 00231 00232 /****************************************/ 00233 /****************************************/ 00234 00238 class CEmbodiedEntitySpaceHashUpdater : public CSpaceHashUpdater<CEmbodiedEntity> { 00239 00240 public: 00241 00242 virtual void operator()(CAbstractSpaceHash<CEmbodiedEntity>& c_space_hash, 00243 CEmbodiedEntity& c_element); 00244 00245 private: 00246 00247 SInt32 m_nMinX, m_nMinY, m_nMinZ; 00248 SInt32 m_nMaxX, m_nMaxY, m_nMaxZ; 00249 00250 }; 00255 /****************************************/ 00256 /****************************************/ 00257 00258 class CEmbodiedEntityGridUpdater : public CGrid<CEmbodiedEntity>::COperation { 00259 00260 public: 00261 00262 CEmbodiedEntityGridUpdater(CGrid<CEmbodiedEntity>& c_grid); 00263 virtual bool operator()(CEmbodiedEntity& c_entity); 00264 00265 private: 00266 00267 CGrid<CEmbodiedEntity>& m_cGrid; 00268 SInt32 m_nMinI, m_nMinJ, m_nMinK; 00269 SInt32 m_nMaxI, m_nMaxJ, m_nMaxK; 00270 }; 00271 00272 /****************************************/ 00273 /****************************************/ 00274 00275 struct SEmbodiedEntityIntersectionItem { 00276 CEmbodiedEntity* IntersectedEntity; 00277 Real TOnRay; 00278 00279 SEmbodiedEntityIntersectionItem() : 00280 IntersectedEntity(NULL), 00281 TOnRay(1.0f) {} 00282 00283 SEmbodiedEntityIntersectionItem(CEmbodiedEntity* pc_entity, 00284 Real f_t_on_ray) : 00285 IntersectedEntity(pc_entity), 00286 TOnRay(f_t_on_ray) {} 00287 00288 inline bool operator<(const SEmbodiedEntityIntersectionItem& s_item) { 00289 return TOnRay < s_item.TOnRay; 00290 } 00291 }; 00292 00293 struct SEmbodiedEntityIntersectionData { 00294 bool Intersection; 00295 std::vector<SEmbodiedEntityIntersectionItem*> IntersectedEntities; 00296 00297 SEmbodiedEntityIntersectionData() : 00298 Intersection(false) {} 00299 00300 SEmbodiedEntityIntersectionData(std::vector<SEmbodiedEntityIntersectionItem*>& c_entities) : 00301 Intersection(c_entities.size() > 0), 00302 IntersectedEntities(c_entities) {} 00303 }; 00304 00305 extern bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem& s_item, 00306 const CRay3& c_ray); 00307 00308 extern bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem& s_item, 00309 const CRay3& c_ray, 00310 CEmbodiedEntity& c_entity); 00311 00312 /****************************************/ 00313 /****************************************/ 00314 00315 } 00316 00317 #endif