LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
heap_indexed.h
Go to the documentation of this file.
1 
9 /* requires:
10 #include <vector>
11  */
12 
13 namespace pr_bgl
14 {
15 
33 template <typename KeyType>
35 {
36  struct element
37  {
38  KeyType key;
39  size_t idx;
40  element(KeyType key, size_t idx): key(key), idx(idx) {}
41  };
42  std::vector<element> backing;
43  std::vector<size_t> locs; // indexed by idx, 0=not in heap
44 
45 public:
46  heap_indexed(): backing(1,element(KeyType(),0)), locs(0) {}
47 
48  void reset()
49  {
50  backing.resize(1,element(KeyType(),0));
51  locs.clear();
52  }
53 
54  // simple queries
55  inline size_t size() const
56  {
57  return backing.size() - 1;
58  }
59  inline bool contains(size_t idx) const
60  {
61  if (idx < locs.size() && locs[idx])
62  return true;
63  else
64  return false;
65  }
66 
67  // assumes contains
68  inline KeyType key_of(size_t idx) const
69  {
70  size_t loc = locs[idx];
71  return backing[loc].key;
72  }
73 
74  // make an inconsistent heap consistent
75  // by up-heaping the element idx with key key
76  // which is CURRENTLY at loc loc
77  inline void up_heap(size_t idx, KeyType key, size_t loc)
78  {
79  for (;;)
80  {
81  // get parent location
82  size_t loc_parent = loc / 2;
83  // done if we have no parent or are bigger or equal to parent
84  if (!loc_parent || backing[loc_parent].key <= key)
85  return;
86  // upheap!
87  size_t idx_parent = backing[loc_parent].idx;
88  std::swap(backing[loc_parent], backing[loc]);
89  std::swap(locs[idx_parent], locs[idx]);
90  loc = loc_parent;
91  }
92  }
93 
94  // make an inconsistent heap consistent
95  // by down-heaping the element idx with key key
96  // which is CURRENTLY at loc loc
97  inline void down_heap(size_t idx, KeyType key, size_t loc)
98  {
99  for (;;)
100  {
101  // get child locations
102  size_t loc_left = 2*loc;
103  size_t loc_right = 2*loc+1;
104  // find largest among family
105  size_t loc_min = loc;
106  KeyType key_min = key;
107  if (loc_left < backing.size() && backing[loc_left].key < key_min)
108  {
109  loc_min = loc_left;
110  key_min = backing[loc_left].key;
111  }
112  if (loc_right < backing.size() && backing[loc_right].key < key_min)
113  {
114  loc_min = loc_right;
115  key_min = backing[loc_right].key;
116  }
117  // are we already the min in the family?
118  if (loc_min == loc)
119  break;
120  size_t idx_min = backing[loc_min].idx;
121  // ok, down-heap!
122  std::swap(backing[loc], backing[loc_min]);
123  std::swap(locs[idx], locs[idx_min]);
124  loc = loc_min;
125  }
126  }
127 
128  // only valid if contains(idx) is false
129  inline void insert(size_t idx, KeyType key)
130  {
131  // resize loc if necessary
132  if (locs.size() < idx+1)
133  locs.resize(idx+1, 0);
134  // insert at end of heap
135  size_t loc = backing.size();
136  backing.push_back(element(key,idx));
137  locs[idx] = loc;
138  // up-heap child (at loc)
139  up_heap(idx,key,loc);
140  }
141 
142  // only valid if contains(idx) is true
143  inline void update(size_t idx, KeyType key)
144  {
145  size_t loc = locs[idx];
146  if (key < backing[loc].key)
147  {
148  backing[loc].key = key;
149  up_heap(idx, key, loc);
150  return;
151  }
152  if (key > backing[loc].key)
153  {
154  backing[loc].key = key;
155  down_heap(idx, key, loc);
156  return;
157  }
158  }
159 
160  // only valid if its in the heap
161  inline void remove(size_t idx)
162  {
163  size_t loc = locs[idx];
164  KeyType key = backing[loc].key;
165  size_t loc_last = backing.size()-1;
166  // if already last, then we're basically done
167  if (loc == loc_last)
168  {
169  backing.pop_back();
170  locs[idx] = 0;
171  return;
172  }
173  // else swap with whatever's last
174  size_t idx_last = backing[loc_last].idx;
175  backing[loc] = backing[loc_last];
176  backing.pop_back();
177  locs[idx_last] = loc;
178  locs[idx] = 0;
179  // maintain proper heap order (old last item is now in loc)
180  if (backing[loc].key < key)
181  up_heap(idx_last, backing[loc].key, loc);
182  if (backing[loc].key > key)
183  down_heap(idx_last, backing[loc].key, loc);
184  }
185 
186  // assumes non-empty
187  inline KeyType top_key() const
188  {
189  return backing[1].key;
190  }
191  inline size_t top_idx() const
192  {
193  return backing[1].idx;
194  }
195  inline void remove_min()
196  {
197  // remove
198  locs[top_idx()] = 0;
199  if (size() > 1)
200  {
201  // move last heap element to root
202  backing[1] = backing[backing.size()-1];
203  locs[backing[1].idx] = 1;
204  }
205  // resize heap backing
206  backing.pop_back();
207  // down-heap the new root
208  if (size())
209  down_heap(backing[1].idx, backing[1].key, 1);
210  }
211 
212  void print() const
213  {
214  printf("backing:");
215  for (size_t ui=0; ui<backing.size(); ui++)
216  printf(" %u:(%.1f,%u)", ui, backing[ui].key, backing[ui].idx);
217  printf("\n");
218  printf("locs:");
219  for (size_t ui=0; ui<locs.size(); ui++)
220  printf(" %u:%u", ui, locs[ui]);
221  printf("\n");
222  }
223 };
224 
225 } // namespace pr_bgl
Binary min-heap with index lookups.
Definition: heap_indexed.h:34