ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef SPACE_HASH_H 00008 #define SPACE_HASH_H 00009 00010 namespace argos { 00011 class CSpace; 00012 class CVector3; 00013 class CRay3; 00014 } 00015 00016 #include <argos3/core/utility/math/ray3.h> 00017 #include <argos3/core/utility/datatypes/set.h> 00018 #include <argos3/core/simulator/space/positional_indices/positional_index.h> 00019 00020 namespace argos { 00021 00022 /****************************************/ 00023 /****************************************/ 00024 00033 template <class ENTITY> 00034 class CAbstractSpaceHash : public CPositionalIndex<ENTITY> { 00035 00036 public: 00037 00041 typedef CSet<ENTITY*> TEntityList; 00042 00043 public: 00044 00052 CAbstractSpaceHash() : m_unSize(0) {} 00053 00057 virtual ~CAbstractSpaceHash() {} 00058 00063 virtual void AddEntity(ENTITY& c_entity) { 00064 m_tEntities.insert(&c_entity); 00065 } 00066 00071 inline TEntityList& GetEntities() { 00072 return m_tEntities; 00073 } 00074 00079 virtual void RemoveEntity(ENTITY& c_entity) { 00080 typename TEntityList::iterator it = m_tEntities.find(&c_entity); 00081 if(it != m_tEntities.end()) { 00082 m_tEntities.erase(it); 00083 } 00084 else { 00085 THROW_ARGOSEXCEPTION("Entity not found when removing it from space hash."); 00086 } 00087 } 00088 00094 inline size_t GetSize() { 00095 return m_unSize; 00096 } 00097 00103 virtual void SetSize(size_t un_size) { 00104 m_unSize = un_size; 00105 } 00106 00112 inline CVector3& GetCellSize() { 00113 return m_cCellSize; 00114 } 00115 00122 inline CVector3& GetInvCellSize() { 00123 return m_cInvCellSize; 00124 } 00125 00131 virtual void SetCellSize(const CVector3& c_cell_size) { 00132 m_cCellSize = c_cell_size; 00133 m_cInvCellSize.Set(1.0f / m_cCellSize.GetX(), 00134 1.0f / m_cCellSize.GetY(), 00135 1.0f / m_cCellSize.GetZ()); 00136 } 00137 00143 virtual void Update() = 0; 00144 00152 virtual void UpdateCell(SInt32 n_x, 00153 SInt32 n_y, 00154 SInt32 n_z, 00155 ENTITY& c_entity) = 0; 00156 00163 virtual SInt32 SpaceToHashTable(Real f_coord, 00164 UInt32 un_axis) { 00165 return RoundClosestToZero(f_coord * GetInvCellSize()[un_axis]); 00166 } 00167 00174 virtual Real HashTableToSpace(SInt32 n_coord, 00175 UInt32 un_axis) { 00176 return n_coord * m_cCellSize[un_axis]; 00177 } 00178 00187 virtual void SpaceToHashTable(SInt32& n_i, 00188 SInt32& n_j, 00189 SInt32& n_k, 00190 const CVector3& c_pos) { 00191 n_i = RoundClosestToZero(c_pos.GetX() * CAbstractSpaceHash::GetInvCellSize().GetX()); 00192 n_j = RoundClosestToZero(c_pos.GetY() * CAbstractSpaceHash::GetInvCellSize().GetY()); 00193 n_k = RoundClosestToZero(c_pos.GetZ() * CAbstractSpaceHash::GetInvCellSize().GetZ()); 00194 } 00195 00204 virtual bool CheckCell(SInt32 n_i, 00205 SInt32 n_j, 00206 SInt32 n_k, 00207 TEntityList& t_entities) = 0; 00208 00209 virtual void Dump(CARGoSLog& c_os) = 0; 00210 00211 protected: 00212 00220 inline UInt32 CoordinateHash(SInt32 n_i, 00221 SInt32 n_j, 00222 SInt32 n_k) { 00223 return 00224 ((73856093u * n_i) ^ 00225 (19349663u * n_j) ^ 00226 (83492791u * n_k)) % 00227 m_unSize; 00228 } 00229 00230 private: 00231 00235 TEntityList m_tEntities; 00236 00240 size_t m_unSize; 00241 00246 CVector3 m_cCellSize; 00247 00252 CVector3 m_cInvCellSize; 00253 00254 }; 00255 00256 /****************************************/ 00257 /****************************************/ 00258 00268 template <class ENTITY> 00269 class CSpaceHashUpdater { 00270 00271 public: 00272 00276 virtual ~CSpaceHashUpdater() {} 00277 00283 virtual void operator()(CAbstractSpaceHash<ENTITY>& c_space_hash, 00284 ENTITY& c_entity) = 0; 00285 00286 }; 00287 00288 /****************************************/ 00289 /****************************************/ 00290 00299 template <class ENTITY, class UPDATER> 00300 class CSpaceHash : public CAbstractSpaceHash<ENTITY> { 00301 00302 public: 00303 00309 virtual void Update() { 00310 /* Go through all the entities */ 00311 for(typename CAbstractSpaceHash<ENTITY>::TEntityList::const_iterator el = CAbstractSpaceHash<ENTITY>::GetEntities().begin(); 00312 el != CAbstractSpaceHash<ENTITY>::GetEntities().end(); ++el) { 00313 m_cUpdater(*this, **el); 00314 } 00315 } 00316 00317 private: 00318 00319 UPDATER m_cUpdater; 00320 00321 }; 00322 00323 /****************************************/ 00324 /****************************************/ 00325 00326 } 00327 00328 #endif