HOG2
BDIndexOpenClosed.h
Go to the documentation of this file.
1 /*
2  * BDIndexOpenClosed.h
3  */
4 
5 #ifndef BDIndexOpenClosed_H
6 #define BDIndexOpenClosed_H
7 
8 #include <cassert>
9 #include <vector>
10 #include <unordered_map>
11 #include <stdint.h>
12 #include <unordered_map>
13 #include "AStarOpenClosed.h"
14 #include "BDIndexOpenClosed.h"
15 //#define ADMISSIBLE
16 //struct AHash64 {
17 // size_t operator()(const uint64_t &x) const
18 // { return (size_t)(x); }
19 //};
20 
21 template<typename state>
23 public:
25  BDIndexOpenClosedData(const state &theData, double gCost, double hCost, uint64_t parent, uint64_t openLoc, stateLocation location)
26  :data(theData), g(gCost), h(hCost), parentID(parent), openLocation(openLoc), where(location) { reopened = false; round = 0; }
27  state data;
28  double g;
29  double h;
30  uint64_t parentID;
31  uint64_t openLocation;
32  bool reopened;
33  uint32_t round;
35 };
36 
37 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure = BDIndexOpenClosedData<state> >
39 public:
41  void Reset(int max);
42  uint64_t AddOpenNode(const state &val, uint64_t hash, double g, double h, uint64_t parent=kTBDNoNode, stateLocation whichQueue = kOpenWaiting);
43  uint64_t AddClosedNode(state &val, uint64_t hash, double g, double h, uint64_t parent=kTBDNoNode);
44  void KeyChanged(uint64_t objKey);
45  void Remove(uint64_t objKey);
46  //void IncreaseKey(uint64_t objKey);
47  stateLocation Lookup(uint64_t hashKey, uint64_t &objKey) const;
48  inline dataStructure &Lookup(uint64_t objKey) { return elements[objKey]; }
49  inline const dataStructure &Lookat(uint64_t objKey) const { return elements[objKey]; }
50  uint64_t Peek(stateLocation whichQueue) const;
51  inline const dataStructure &PeekAt(stateLocation whichQueue) const;
52 
53  //if exist min pair, return true, else(one queue is empty) return false;
54  //if it returns true, left and right should be set to the pair that is supposed to be returned.
55  //bool ExtractMinPair(uint64_t& left, uint64_t& right) ;
56  uint64_t Close();
57  uint64_t PutToReady();
58  //void Reopen(uint64_t objKey);
59 #ifdef ADMISSIBLE
60  void Reopen(uint64_t objKey);
61 #endif // ADMISSIBLE
62 
63 
64  uint64_t GetOpenItem(unsigned int which, stateLocation where){ return priorityQueues[where][which];}
65  size_t OpenReadySize() const { return priorityQueues[kOpenReady].size(); }
66  size_t OpenWaitingSize() const { return priorityQueues[kOpenWaiting].size(); }
67  size_t OpenSize() const { return priorityQueues[kOpenReady].size()+priorityQueues[kOpenWaiting].size(); }
68 
69  size_t ClosedSize() const { return size()-OpenReadySize()-OpenWaitingSize(); }
70  size_t size() const { return elements.size(); }
71  void verifyData();
72  bool ValidateOpenReady(int index = 0)
73  {
74  stateLocation whichQ = kOpenReady;
75  CmpKey0 compare;
76  if (index >= priorityQueues[whichQ].size())
77  return true;
78  int child1 = index * 2 + 1;
79  int child2 = index * 2 + 2;
80  if (priorityQueues[whichQ].size() > child1 &&
81  !compare(elements[priorityQueues[whichQ][child1]], elements[priorityQueues[whichQ][index]]))
82  return false;
83  if (priorityQueues[whichQ].size() > child2 &&
84  !compare(elements[priorityQueues[whichQ][child2]], elements[priorityQueues[whichQ][index]]))
85  return false;
86  return ValidateOpenReady(child1) && ValidateOpenReady(child2);
87  }
88 
89  bool ValidateOpenWaiting(int index = 0)
90  {
91  stateLocation whichQ = kOpenWaiting;
92  CmpKey1 compare;
93  if (index >= priorityQueues[whichQ].size())
94  return true;
95  int child1 = index * 2 + 1;
96  int child2 = index * 2 + 2;
97  if (priorityQueues[whichQ].size() > child1 &&
98  !compare(elements[priorityQueues[whichQ][child1]], elements[priorityQueues[whichQ][index]]))
99  return false;
100  if (priorityQueues[whichQ].size() > child2 &&
101  !compare(elements[priorityQueues[whichQ][child2]], elements[priorityQueues[whichQ][index]]))
102  return false;
103  return ValidateOpenWaiting(child1) && ValidateOpenWaiting(child2);
104  }
105 private:
106  bool HeapifyUp(unsigned int index, stateLocation whichQueue);
107  void HeapifyDown(unsigned int index, stateLocation whichQueue);
108 
109  //2 queues:
110  //priorityQueues[0] is openReady, priorityQueues[1] is openWaiting
111  std::vector<std::vector<uint64_t>> priorityQueues;
112  std::vector<dataStructure> elements;
113  uint32_t currRound;
114 };
115 
116 #ifdef ADMISSIBLE
117 
118 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
120 {
121  assert(elements[objKey].where == kClosed);
122  elements[objKey].reopened = true;
123  elements[objKey].where = kOpenWaiting;
124  elements[objKey].openLocation = priorityQueues[kOpenWaiting].size();
125  priorityQueues[kOpenWaiting].push_back(objKey);
126  HeapifyUp(priorityQueues[kOpenWaiting].size() - 1, kOpenWaiting);
127 }
128 
129 #endif // ADMISSIBLE
130 
131 
132 
133 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
135 {
136  currRound = 1;
137  std::vector<uint64_t> queue;
138  queue.resize(0);
139  priorityQueues.push_back(queue);
140  priorityQueues.push_back(queue);
141  //readyQueue.resize(0);
142  //waitingQueue.resize(0);
143 }
144 
148 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
150 {
151  elements.resize(maxHash);
152  priorityQueues[0].resize(0);
153  priorityQueues[1].resize(0);
154  currRound++;
155  //readyQueue.resize(0);
156  //waitingQueue.resize(0);
157 }
158 
162 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
163 uint64_t BDIndexOpenClosed<state, CmpKey0, CmpKey1, dataStructure>::AddOpenNode(const state &val, uint64_t hash, double g, double h, uint64_t parent, stateLocation whichQueue)
164 {
165  if (elements[hash].round == currRound) // already in open
166  {
167  //return -1; // TODO: find correct id and return
168  assert(false);
169  }
170  if (whichQueue == kOpenReady)
171  {
172  elements[hash] = dataStructure(val, g, h, parent, priorityQueues[0].size() , kOpenReady);
173  }
174  else if (whichQueue == kOpenWaiting)
175  {
176  elements[hash] = dataStructure(val, g, h, parent, priorityQueues[1].size(), kOpenWaiting);
177  }
178  elements[hash].round = currRound;
179 
180  if (parent == kTBDNoNode)
181  elements[hash].parentID = hash;
182  //table[hash] = elements.size()-1; // hashing to element list location
183 
184  priorityQueues[whichQueue].push_back(hash);
185  HeapifyUp(priorityQueues[whichQueue].size() - 1,whichQueue);
186 
187  return hash;
188 }
189 
193 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
194 uint64_t BDIndexOpenClosed<state, CmpKey0, CmpKey1, dataStructure>::AddClosedNode(state &val, uint64_t hash, double g, double h, uint64_t parent)
195 {
196  assert(elements[hash].currRound < currRound); // already in open/closed
197  //assert(table.find(hash) == table.end());
198  elements[hash] = dataStructure(val, g, h, parent, 0, kClosed);
199  elements[hash].round = currRound;
200  if (parent == kTBDNoNode)
201  elements[hash].parentID = hash;//elements.back().parentID = elements.size()-1;
202 // table[hash] = elements.size()-1; // hashing to element list location
203  return hash;
204 }
205 
209 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
211 {
212 // EqKey eq;
213 // assert(eq(waitingQueue[table[val]], val));
214 // //HeapifyUp(val->key);
215 // waitingQueue[table[val]] = val;
216  if (elements[val].where == kOpenReady)
217  {
218  if (!HeapifyUp(elements[val].openLocation, kOpenReady))
219  HeapifyDown(elements[val].openLocation, kOpenReady);
220  }
221  else if (elements[val].where == kOpenWaiting)
222  {
223  if (!HeapifyUp(elements[val].openLocation, kOpenWaiting))
224  HeapifyDown(elements[val].openLocation, kOpenWaiting);
225  }
226 }
227 
228 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
230 {
231 
232  int index = elements[val].openLocation;
233  stateLocation whichQueue = elements[val].where;
234  elements[val].where = kClosed;
235  priorityQueues[whichQueue][index] = priorityQueues[whichQueue][priorityQueues[whichQueue].size() - 1];
236  elements[priorityQueues[whichQueue][index]].openLocation = index;
237  priorityQueues[whichQueue].pop_back();
238 
239  if (!HeapifyUp(index, whichQueue))
240  HeapifyDown(index, whichQueue);
241 
242 
243 }
244 
246 // * Indicate that the key for a particular object has increased.
247 // */
248 //template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
249 //void BDIndexOpenClosed<state, CmpKey0, CmpKey1, dataStructure>::IncreaseKey(uint64_t val)
250 //{
254 // HeapifyDown(val);
255 //}
256 
261 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
263 {
264  if (elements[hashKey].round != currRound)
265  return kUnseen;
266  objKey = hashKey;
267  return elements[objKey].where;
268 }
269 
270 
274 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
276 {
277  if (whichQueue == kOpenReady)
278  {
279  assert(OpenReadySize() != 0);
280  }
281  else if (whichQueue == kOpenWaiting)
282  {
283  assert(OpenWaitingSize() != 0);
284  }
285  return priorityQueues[whichQueue][0];
286 }
287 
291 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
293 {
294  if (whichQueue == kOpenReady)
295  {
296  assert(OpenReadySize() != 0);
297  }
298  else if (whichQueue == kOpenWaiting)
299  {
300  assert(OpenWaitingSize() != 0);
301  }
302  return elements[priorityQueues[whichQueue][0]];
303 }
304 
305 
306 
310 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
312 {
313  assert(OpenReadySize() != 0);
314 
315  uint64_t ans = priorityQueues[0][0];
316  elements[ans].where = kClosed;
317  priorityQueues[0][0] = priorityQueues[0][OpenReadySize()-1];
318  elements[priorityQueues[0][0]].openLocation = 0;
319  priorityQueues[0].pop_back();
320 
321  HeapifyDown(0,kOpenReady);
322 
323  return ans;
324 }
325 
326 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
328 {
329  assert(OpenWaitingSize() != 0);
330 
331 
332  //remove it from openWaiting
333  uint64_t ans = priorityQueues[kOpenWaiting][0];
334  uint64_t back = priorityQueues[kOpenWaiting].back();
335  priorityQueues[kOpenWaiting][0] = back;
336  priorityQueues[kOpenWaiting].pop_back();
337  elements[back].openLocation = 0;
338 
339  HeapifyDown(0, kOpenWaiting);
340 // assert(ValidateOpenReady());
341 // assert(ValidateOpenWaiting());
342 
343  //put it to openReady
344  priorityQueues[kOpenReady].push_back(ans);
345  elements[ans].where = kOpenReady;
346  elements[ans].openLocation = priorityQueues[kOpenReady].size()-1;
347 
348  HeapifyUp(priorityQueues[kOpenReady].size() - 1,kOpenReady);
349 
350 // assert(ValidateOpenReady());
351 // assert(ValidateOpenWaiting());
352 
353  return ans;
354 }
355 
359 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
361 {
362  if (index == 0) return false;
363  int parent = (index-1)/2;
364 
365  if (whichQueue == kOpenReady)
366  {
367  CmpKey0 compare;
368  if (compare(elements[priorityQueues[whichQueue][parent]], elements[priorityQueues[whichQueue][index]]))
369  {
370  unsigned int tmp = priorityQueues[whichQueue][parent];
371  priorityQueues[whichQueue][parent] = priorityQueues[whichQueue][index];
372  priorityQueues[whichQueue][index] = tmp;
373  elements[priorityQueues[whichQueue][parent]].openLocation = parent;
374  elements[priorityQueues[whichQueue][index]].openLocation = index;
375  HeapifyUp(parent, whichQueue);
376  return true;
377  }
378  }
379  else if (whichQueue == kOpenWaiting)
380  {
381  CmpKey1 compare;
382  if (compare(elements[priorityQueues[whichQueue][parent]], elements[priorityQueues[whichQueue][index]]))
383  {
384  unsigned int tmp = priorityQueues[whichQueue][parent];
385  priorityQueues[whichQueue][parent] = priorityQueues[whichQueue][index];
386  priorityQueues[whichQueue][index] = tmp;
387  elements[priorityQueues[whichQueue][parent]].openLocation = parent;
388  elements[priorityQueues[whichQueue][index]].openLocation = index;
389  HeapifyUp(parent, whichQueue);
390  return true;
391  }
392  }
393 
394 
395  return false;
396 }
397 
398 template<typename state, typename CmpKey0, typename CmpKey1, class dataStructure>
400 {
401 
402  unsigned int child1 = index*2+1;
403  unsigned int child2 = index*2+2;
404 
405 
406 
407  if (whichQueue == kOpenReady)
408  {
409  CmpKey0 compare;
410  int which;
411  unsigned int count = priorityQueues[whichQueue].size();
412  // find smallest child
413  if (child1 >= count)
414  return;
415  else if (child2 >= count)
416  which = child1;
417  else if (!(compare(elements[priorityQueues[whichQueue][child1]], elements[priorityQueues[whichQueue][child2]])))
418  which = child1;
419  else
420  which = child2;
421 
422  //if (fless(waitingQueue[which]->GetKey(), waitingQueue[index]->GetKey()))
423  if (!(compare(elements[priorityQueues[whichQueue][which]], elements[priorityQueues[whichQueue][index]])))
424  {
425  unsigned int tmp = priorityQueues[whichQueue][which];
426  priorityQueues[whichQueue][which] = priorityQueues[whichQueue][index];
427  // table[waitingQueue[which]] = which;
428  priorityQueues[whichQueue][index] = tmp;
429  elements[priorityQueues[whichQueue][which]].openLocation = which;
430  elements[priorityQueues[whichQueue][index]].openLocation = index;
431  // table[waitingQueue[index]] = index;
432  // waitingQueue[which]->key = which;
433  // waitingQueue[index]->key = index;
434  HeapifyDown(which,whichQueue);
435  }
436  }
437  else if (whichQueue == kOpenWaiting)
438  {
439  CmpKey1 compare;
440  int which;
441  unsigned int count = priorityQueues[whichQueue].size();
442  // find smallest child
443  if (child1 >= count) // no children; done
444  return;
445  else if (child2 >= count) // one child - compare there
446  which = child1;
447  // find larger child to move up
448  else if (!(compare(elements[priorityQueues[whichQueue][child1]], elements[priorityQueues[whichQueue][child2]])))
449  which = child1;
450  else
451  which = child2;
452 
453  //if (fless(waitingQueue[which]->GetKey(), waitingQueue[index]->GetKey()))
454  if (!(compare(elements[priorityQueues[whichQueue][which]], elements[priorityQueues[whichQueue][index]])))
455  {
456  unsigned int tmp = priorityQueues[whichQueue][which];
457  priorityQueues[whichQueue][which] = priorityQueues[whichQueue][index];
458  priorityQueues[whichQueue][index] = tmp;
459 
460 // assert(elements[priorityQueues[whichQueue][which]].where == kOpenWaiting);
461 // assert(elements[priorityQueues[whichQueue][index]].where == kOpenWaiting);
462  elements[priorityQueues[whichQueue][which]].openLocation = which;
463  elements[priorityQueues[whichQueue][index]].openLocation = index;
464  HeapifyDown(which, whichQueue);
465 // assert((compare(elements[priorityQueues[whichQueue][which]], elements[priorityQueues[whichQueue][index]])));
466 // if (child1 < count)
467 // assert((compare(elements[priorityQueues[whichQueue][child1]], elements[priorityQueues[whichQueue][index]])));
468 // if (child2 < count)
469 // {
470 // printf("w:%d - c1:%d c2:%d\n", which, child1, child2);
471 // assert((compare(elements[priorityQueues[whichQueue][child2]], elements[priorityQueues[whichQueue][index]])));
472 // }
473  }
474  }
475 
476 }
477 
478 #endif
BDIndexOpenClosed::ValidateOpenReady
bool ValidateOpenReady(int index=0)
Definition: BDIndexOpenClosed.h:72
kOpenWaiting
@ kOpenWaiting
Definition: BDOpenClosed.h:24
BDIndexOpenClosed.h
BDIndexOpenClosedData::BDIndexOpenClosedData
BDIndexOpenClosedData(const state &theData, double gCost, double hCost, uint64_t parent, uint64_t openLoc, stateLocation location)
Definition: BDIndexOpenClosed.h:25
BDIndexOpenClosedData::parentID
uint64_t parentID
Definition: BDIndexOpenClosed.h:30
BDIndexOpenClosed::AddOpenNode
uint64_t AddOpenNode(const state &val, uint64_t hash, double g, double h, uint64_t parent=kTBDNoNode, stateLocation whichQueue=kOpenWaiting)
Add object into open list.
Definition: BDIndexOpenClosed.h:163
kOpenReady
@ kOpenReady
Definition: BDOpenClosed.h:23
AStarOpenClosed.h
BDIndexOpenClosedData::round
uint32_t round
Definition: BDIndexOpenClosed.h:33
BDIndexOpenClosedData::openLocation
uint64_t openLocation
Definition: BDIndexOpenClosed.h:31
BDIndexOpenClosed::Remove
void Remove(uint64_t objKey)
Definition: BDIndexOpenClosed.h:229
BDIndexOpenClosedData::g
double g
Definition: BDIndexOpenClosed.h:28
kClosed
@ kClosed
Definition: BDOpenClosed.h:25
BDIndexOpenClosed::HeapifyUp
bool HeapifyUp(unsigned int index, stateLocation whichQueue)
Moves a node up the heap.
Definition: BDIndexOpenClosed.h:360
BDIndexOpenClosed::KeyChanged
void KeyChanged(uint64_t objKey)
Indicate that the key for a particular object has changed.
Definition: BDIndexOpenClosed.h:210
BDIndexOpenClosed::ValidateOpenWaiting
bool ValidateOpenWaiting(int index=0)
Definition: BDIndexOpenClosed.h:89
BDIndexOpenClosed::size
size_t size() const
Definition: BDIndexOpenClosed.h:70
BDIndexOpenClosed::GetOpenItem
uint64_t GetOpenItem(unsigned int which, stateLocation where)
Definition: BDIndexOpenClosed.h:64
BDIndexOpenClosedData
Definition: BDIndexOpenClosed.h:22
BDIndexOpenClosed::Lookup
stateLocation Lookup(uint64_t hashKey, uint64_t &objKey) const
‍**
Definition: BDIndexOpenClosed.h:262
kUnseen
@ kUnseen
Definition: BDOpenClosed.h:26
BDIndexOpenClosed::AddClosedNode
uint64_t AddClosedNode(state &val, uint64_t hash, double g, double h, uint64_t parent=kTBDNoNode)
Add object into closed list.
Definition: BDIndexOpenClosed.h:194
BDIndexOpenClosedData::BDIndexOpenClosedData
BDIndexOpenClosedData()
Definition: BDIndexOpenClosed.h:24
BDIndexOpenClosed::Reset
void Reset(int max)
Remove all objects from queue.
Definition: BDIndexOpenClosed.h:149
BDIndexOpenClosed::priorityQueues
std::vector< std::vector< uint64_t > > priorityQueues
Definition: BDIndexOpenClosed.h:111
kTBDNoNode
const uint64_t kTBDNoNode
Definition: BDOpenClosed.h:30
BDIndexOpenClosed::HeapifyDown
void HeapifyDown(unsigned int index, stateLocation whichQueue)
Definition: BDIndexOpenClosed.h:399
BDIndexOpenClosed::PeekAt
const dataStructure & PeekAt(stateLocation whichQueue) const
Peek at the next item to be expanded.
Definition: BDIndexOpenClosed.h:292
stateLocation
stateLocation
Definition: BDOpenClosed.h:22
BDIndexOpenClosed::ClosedSize
size_t ClosedSize() const
Definition: BDIndexOpenClosed.h:69
BDIndexOpenClosed::OpenWaitingSize
size_t OpenWaitingSize() const
Definition: BDIndexOpenClosed.h:66
BDIndexOpenClosed
Definition: BDIndexOpenClosed.h:38
BDIndexOpenClosed::BDIndexOpenClosed
BDIndexOpenClosed()
Definition: BDIndexOpenClosed.h:134
BDIndexOpenClosedData::data
state data
Definition: BDIndexOpenClosed.h:27
BDIndexOpenClosedData::reopened
bool reopened
Definition: BDIndexOpenClosed.h:32
BDIndexOpenClosed::verifyData
void verifyData()
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
BDIndexOpenClosedData::h
double h
Definition: BDIndexOpenClosed.h:29
BDIndexOpenClosed::Peek
uint64_t Peek(stateLocation whichQueue) const
Peek at the next item to be expanded.
Definition: BDIndexOpenClosed.h:275
BDIndexOpenClosed::OpenReadySize
size_t OpenReadySize() const
Definition: BDIndexOpenClosed.h:65
BDIndexOpenClosed::Lookat
const dataStructure & Lookat(uint64_t objKey) const
Definition: BDIndexOpenClosed.h:49
BDIndexOpenClosed::currRound
uint32_t currRound
Definition: BDIndexOpenClosed.h:113
BDIndexOpenClosed::elements
std::vector< dataStructure > elements
Definition: BDIndexOpenClosed.h:112
BDIndexOpenClosed::Lookup
dataStructure & Lookup(uint64_t objKey)
Definition: BDIndexOpenClosed.h:48
BDIndexOpenClosed::Close
uint64_t Close()
Move the best item to the closed list and return key.
Definition: BDIndexOpenClosed.h:311
BDIndexOpenClosed::PutToReady
uint64_t PutToReady()
Definition: BDIndexOpenClosed.h:327
BDIndexOpenClosedData::where
stateLocation where
Definition: BDIndexOpenClosed.h:34
BDIndexOpenClosed::OpenSize
size_t OpenSize() const
Definition: BDIndexOpenClosed.h:67