ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 #ifndef GRID_H 00002 #define GRID_H 00003 00004 #include <argos3/core/utility/datatypes/set.h> 00005 #include <argos3/core/utility/math/range.h> 00006 #include <argos3/core/utility/math/ray3.h> 00007 #include <argos3/core/simulator/space/positional_indices/positional_index.h> 00008 00009 namespace argos { 00010 00011 template<class ENTITY> 00012 class CGrid : public CPositionalIndex<ENTITY> { 00013 00014 public: 00015 00016 typedef typename CPositionalIndex<ENTITY>::COperation CEntityOperation; 00017 00018 struct SCell { 00019 CSet<ENTITY*> Entities; 00020 size_t Timestamp; 00021 00022 SCell() : Timestamp(0) {} 00023 inline void Reset() { 00024 Entities.clear(); 00025 Timestamp = 0; 00026 } 00027 }; 00028 00029 class CCellOperation { 00030 public: 00031 virtual ~CCellOperation() {} 00032 virtual bool operator()(SInt32 n_i, 00033 SInt32 n_j, 00034 SInt32 n_k, 00035 SCell& s_cell) = 0; 00036 }; 00037 00038 public: 00039 00040 CGrid(const CVector3& c_area_min_corner, 00041 const CVector3& c_area_max_corner, 00042 SInt32 n_size_i, 00043 SInt32 n_size_j, 00044 SInt32 n_size_k); 00045 00046 virtual ~CGrid(); 00047 00048 virtual void Init(TConfigurationNode& t_tree); 00049 virtual void Reset(); 00050 virtual void Destroy(); 00051 00052 virtual void AddEntity(ENTITY& c_entity); 00053 00054 virtual void RemoveEntity(ENTITY& c_entity); 00055 00056 virtual void Update(); 00057 00058 virtual void GetEntitiesAt(CSet<ENTITY*>& c_entities, 00059 const CVector3& c_position) const; 00060 00061 virtual void ForAllEntities(CEntityOperation& c_operation); 00062 00063 virtual void ForEntitiesInSphereRange(const CVector3& c_center, 00064 Real f_radius, 00065 CEntityOperation& c_operation); 00066 00067 virtual void ForEntitiesInBoxRange(const CVector3& c_center, 00068 const CVector3& c_half_size, 00069 CEntityOperation& c_operation); 00070 00071 virtual void ForEntitiesInCircleRange(const CVector3& c_center, 00072 Real f_radius, 00073 CEntityOperation& c_operation); 00074 00075 virtual void ForEntitiesInRectangleRange(const CVector3& c_center, 00076 const CVector2& c_half_size, 00077 CEntityOperation& c_operation); 00078 00079 virtual void ForEntitiesAlongRay(const CRay3& c_ray, 00080 CEntityOperation& c_operation, 00081 bool b_stop_at_closest_match = false); 00082 00083 virtual void ForAllCells(CCellOperation& c_operation); 00084 00085 virtual void ForCellsInSphereRange(const CVector3& c_center, 00086 Real f_radius, 00087 CCellOperation& c_operation); 00088 00089 virtual void ForCellsInBoxRange(const CVector3& c_center, 00090 const CVector3& c_half_size, 00091 CCellOperation& c_operation); 00092 00093 virtual void ForCellsInCircleRange(const CVector3& c_center, 00094 Real f_radius, 00095 CCellOperation& c_operation); 00096 00097 virtual void ForCellsInRectangleRange(const CVector3& c_center, 00098 const CVector2& c_half_size, 00099 CCellOperation& c_operation); 00100 00101 virtual void ForCellsAlongRay(const CRay3& c_ray, 00102 CCellOperation& c_operation); 00103 00104 inline SInt32 GetSizeI() const { 00105 return m_nSizeI; 00106 } 00107 00108 inline SInt32 GetSizeJ() const { 00109 return m_nSizeJ; 00110 } 00111 00112 inline SInt32 GetSizeK() const { 00113 return m_nSizeK; 00114 } 00115 00116 inline void SetUpdateEntityOperation(CEntityOperation* pc_operation); 00117 00118 void UpdateCell(SInt32 n_i, 00119 SInt32 n_j, 00120 SInt32 n_k, 00121 ENTITY& c_entity); 00122 00123 inline void PositionToCell(SInt32& n_i, 00124 SInt32& n_j, 00125 SInt32& n_k, 00126 const CVector3& c_position) const; 00127 00128 inline void PositionToCellUnsafe(SInt32& n_i, 00129 SInt32& n_j, 00130 SInt32& n_k, 00131 const CVector3& c_position) const; 00132 00133 inline void ClampCoordinates(SInt32& n_i, 00134 SInt32& n_j, 00135 SInt32& n_k) const; 00136 00137 inline void ClampCoordinates(CVector3& c_pos) const; 00138 00139 inline SCell& GetCellAt(SInt32 n_i, 00140 SInt32 n_j, 00141 SInt32 n_k); 00142 00143 inline const SCell& GetCellAt(SInt32 n_i, 00144 SInt32 n_j, 00145 SInt32 n_k) const; 00146 00147 protected: 00148 00149 CVector3 m_cAreaMinCorner; 00150 CVector3 m_cAreaMaxCorner; 00151 SInt32 m_nSizeI; 00152 SInt32 m_nSizeJ; 00153 SInt32 m_nSizeK; 00154 CRange<Real> m_cRangeX; 00155 CRange<Real> m_cRangeY; 00156 CRange<Real> m_cRangeZ; 00157 CVector3 m_cCellSize; 00158 CVector3 m_cInvCellSize; 00159 SCell* m_psCells; 00160 size_t m_unCurTimestamp; 00161 CSet<ENTITY*> m_cEntities; 00162 CEntityOperation* m_pcUpdateEntityOperation; 00163 00164 }; 00165 00166 } 00167 00168 #include <argos3/core/simulator/space/positional_indices/grid_impl.h> 00169 00170 #endif