ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #ifndef SPACE_HASH_NATIVE_H 00008 #define SPACE_HASH_NATIVE_H 00009 00010 #include <argos3/core/simulator/space/space_hash.h> 00011 00012 namespace argos { 00013 00018 template <class Element, class Updater> 00019 class CSpaceHashNative : public CSpaceHash<Element,Updater> { 00020 00021 private: 00022 00026 struct SBucket { 00027 00032 struct SBucketData { 00036 Element* Elem; 00040 SInt32 I,J,K; 00044 SBucketData* Next; 00045 00053 SBucketData(Element& c_element, 00054 SInt32 n_i, 00055 SInt32 n_j, 00056 SInt32 n_k, 00057 SBucketData* ps_next = NULL) : 00058 Elem(&c_element), 00059 I(n_i), 00060 J(n_j), 00061 K(n_k), 00062 Next(ps_next) {} 00063 00064 }; 00065 00069 UInt64 StoreTimestamp; 00070 00074 SBucketData* ElementList; 00075 00080 SBucket() : 00081 StoreTimestamp(0), 00082 ElementList(NULL) {} 00083 00089 ~SBucket() { 00090 Clear(); 00091 } 00092 00097 inline bool Empty() const { 00098 return (ElementList == NULL); 00099 } 00100 00105 inline void Clear() { 00106 if(!Empty()) { 00107 SBucketData* psCur = ElementList; 00108 SBucketData* psNext = psCur->Next; 00109 do { 00110 delete psCur; 00111 psCur = psNext; 00112 if(psCur) psNext = psCur->Next; 00113 } while(psCur); 00114 ElementList = NULL; 00115 } 00116 } 00117 00125 inline void Add(Element& c_element, 00126 SInt32 n_i, 00127 SInt32 n_j, 00128 SInt32 n_k) { 00129 if(Empty()) ElementList = new SBucketData(c_element, n_i, n_j, n_k); 00130 else ElementList = new SBucketData(c_element, n_i, n_j, n_k, ElementList); 00131 } 00132 00140 bool Exists(const Element& c_element, 00141 SInt32 n_i, 00142 SInt32 n_j, 00143 SInt32 n_k) { 00144 SBucketData* psCur = ElementList; 00145 while(psCur) { 00146 if(psCur->Elem == &c_element && 00147 psCur->I == n_i && 00148 psCur->J == n_j && 00149 psCur->K == n_k) return true; 00150 psCur = psCur->Next; 00151 } 00152 return false; 00153 } 00154 00155 }; 00156 00157 public: 00158 00166 CSpaceHashNative() : 00167 m_psBuckets(NULL), 00168 m_unCurrentStoreTimestamp(0) {} 00169 00173 ~CSpaceHashNative() { 00174 Clear(); 00175 delete[] m_psBuckets; 00176 } 00177 00181 inline void Clear() { 00182 for(size_t i = 0; i < CSpaceHash<Element,Updater>::GetSize(); ++i) { 00183 m_psBuckets[i].Clear(); 00184 } 00185 } 00186 00192 inline virtual void SetSize(size_t un_size) { 00193 CSpaceHash<Element,Updater>::SetSize(un_size); 00194 m_psBuckets = new SBucket[CSpaceHash<Element,Updater>::GetSize()]; 00196 } 00197 00203 inline virtual void Update() { 00204 /* Set the current store time stamp */ 00205 m_unCurrentStoreTimestamp++; 00206 /* Call base class method */ 00207 CSpaceHash<Element,Updater>::Update(); 00208 } 00209 00217 inline virtual void UpdateCell(SInt32 n_i, 00218 SInt32 n_j, 00219 SInt32 n_k, 00220 Element& c_element) { 00221 /* Calculate the hash of the current position */ 00222 SInt32 nHash = CSpaceHash<Element,Updater>::CoordinateHash(n_i, n_j, n_k); 00223 /* Get a reference to the bucket */ 00224 SBucket& sBucket = m_psBuckets[nHash]; 00225 /* Check if the bucket's content is obsolete */ 00226 if(sBucket.StoreTimestamp == m_unCurrentStoreTimestamp) { 00227 /* Add the current element to the bucket */ 00228 if(! sBucket.Exists(c_element, n_i, n_j, n_k)) { 00229 sBucket.Add(c_element, n_i, n_j, n_k); 00230 } 00231 } 00232 else { 00233 /* The bucket's content is obsolete, erase it */ 00234 sBucket.Clear(); 00235 /* Set the store timestamp to the current time */ 00236 sBucket.StoreTimestamp = m_unCurrentStoreTimestamp; 00237 /* Add the current element to the bucket */ 00238 sBucket.Add(c_element, n_i, n_j, n_k); 00239 } 00240 } 00241 00249 virtual bool CheckCell(SInt32 n_i, 00250 SInt32 n_j, 00251 SInt32 n_k, 00252 typename CSpaceHash<Element,Updater>::TElementList& t_elements) { 00253 /* In the beginning, no new elements have been found */ 00254 bool bNewElements = false; 00255 /* Calculate the hash of the current position */ 00256 SInt32 nHash = CSpaceHash<Element,Updater>::CoordinateHash(n_i, n_j, n_k); 00257 /* Get a reference to the bucket */ 00258 SBucket& sBucket = m_psBuckets[nHash]; 00259 /* Visit the bucket IF: 00260 1. its data is up-to-date AND 00261 2. it is not empty 00262 */ 00263 if((sBucket.StoreTimestamp == m_unCurrentStoreTimestamp) && /* 1. */ 00264 !sBucket.Empty()) /* 2. */ { 00265 /* Check the bucket's elements */ 00266 for(typename SBucket::SBucketData* psCur = sBucket.ElementList; 00267 psCur; 00268 psCur = psCur->Next) { 00269 /* Check that the element is in the wanted cell */ 00270 if(n_i == psCur->I && 00271 n_j == psCur->J && 00272 n_k == psCur->K) { 00273 /* We have a new element to add to the list */ 00274 bNewElements = true; 00275 t_elements.insert(psCur->Elem); 00276 } 00277 } 00278 } 00279 return bNewElements; 00280 } 00281 00282 virtual void Dump(CARGoSLog& c_os) { 00283 for(size_t i = 0; i < CSpaceHash<Element,Updater>::GetSize(); ++i) { 00284 if((m_psBuckets[i].StoreTimestamp == m_unCurrentStoreTimestamp) && 00285 !m_psBuckets[i].Empty()) { 00286 c_os << "BUCKET " << i << std::endl; 00287 for(typename SBucket::SBucketData* psCur = m_psBuckets[i].ElementList; 00288 psCur; 00289 psCur = psCur->Next) { 00290 c_os << " " 00291 << psCur->Elem->GetId() 00292 << " (" 00293 << psCur->I 00294 << "," 00295 << psCur->J 00296 << "," 00297 << psCur->K 00298 << ")" 00299 << std::endl; 00300 } 00301 } 00302 } 00303 } 00304 00305 private: 00306 00310 SBucket* m_psBuckets; 00311 00319 UInt64 m_unCurrentStoreTimestamp; 00320 00321 }; 00322 00323 } 00324 00325 #endif