Debian Codegraph @

protolib/src/common/protoSpace.cpp

raw
1 /**
2 * @file protoSpace.cpp
3 *
4 * @brief This maintains a set of "Nodes" in n-dimensional Euclidean space.
5 */
6 // Uncomment this to have SDT display bounding box iteration
7 //#define USE_SDT
8
9 #include "protoDebug.h"
10
11 #include "protoSpace.h"
12 #include <math.h> // for "fabs()"
13 #include <stdio.h> // for "printf()"
14 ProtoSpace::Node::Node()
15 {
16 }
17
18 ProtoSpace::Node::~Node()
19 {
20 }
21
22 ProtoSpace::Ordinate::Ordinate()
23 {
24 memset(key, 0, sizeof(double)+sizeof(Node*));
25 }
26
27 ProtoSpace::Ordinate::~Ordinate()
28 {
29 }
30
31 ProtoSpace::ProtoSpace()
32 : num_dimensions(0), ord_tree(NULL), node_count(0)
33 {
34 }
35
36 ProtoSpace::~ProtoSpace()
37 {
38 if (NULL != ord_tree)
39 {
40 for (unsigned int i = 0; i < num_dimensions; i++)
41 ord_tree[i].EmptyToPool(ord_pool);
42 delete[] ord_tree;
43 ord_tree = NULL;
44 }
45 ord_pool.Destroy();
46 }
47
48 void ProtoSpace::Empty()
49 {
50 if (NULL != ord_tree)
51 {
52 for (unsigned int i = 0; i < num_dimensions; i++)
53 ord_tree[i].EmptyToPool(ord_pool);
54 }
55 } // end ProtoSpace::Empty()
56
57
58 bool ProtoSpace::InsertNode(Node& node)
59 {
60 if (0 == num_dimensions)
61 {
62 unsigned int numDimensions = node.GetDimensions();
63 if (NULL == (ord_tree = new ProtoSortedTree[numDimensions]))
64 {
65 PLOG(PL_ERROR, "ProtoSpace::InsertNode() error: unable to allocate Ordinate tree array\n");
66 return false;
67 }
68 num_dimensions = numDimensions;
69 }
70 else if (node.GetDimensions() != num_dimensions)
71 {
72 // (TBD) Allow space to be re-scoped if empty???
73 PLOG(PL_ERROR, "ProtoSpace::InsertNode() error: Node dimensions does not match space!\n");
74 return false;
75 }
76
77 // Get and Insert "Ordinate" entries for the node for each dimension
78 for (unsigned int i = 0; i < num_dimensions; i++)
79 {
80 Ordinate* ord = static_cast<Ordinate*>(ord_pool.Get());
81 if ((NULL == ord) && (NULL == (ord = new Ordinate)))
82 {
83 PLOG(PL_ERROR, "ProtoSpace::InsertNode() error: unable to allocate Ordinate\n");
84 RemoveNode(node);
85 return false;
86 }
87 ord->SetValue(node.GetOrdinate(i));
88 ord->SetNode(&node);
89 ord_tree[i].Insert(*ord);
90 }
91 node_count++;
92 return true;
93 } // end ProtoSpace::InsertNode();
94
95
96 bool ProtoSpace::RemoveNode(Node& node)
97 {
98 if (0 == num_dimensions) return false;
99 ASSERT(node.GetDimensions() == num_dimensions);
100 bool result = false;
101 for (unsigned int i = 0; i < num_dimensions; i++)
102 {
103 Ordinate tempOrd;
104 tempOrd.SetNode(&node);
105 tempOrd.SetValue(node.GetOrdinate(i));
106 Ordinate* ord = static_cast<Ordinate*>(ord_tree[i].Find(tempOrd.GetKey(), tempOrd.GetKeysize()));
107 if (NULL != ord)
108 {
109 ord_tree[i].Remove(*ord);
110 ReturnOrdinateToPool(*ord);
111 result = true;
112 }
113 else
114 {
115 //PLOG(PL_WARN, "ProtoSpace::RemoveNode() warning: no ordinate[%u] for node\n", i);
116 continue;
117 }
118 }
119 if (result) node_count--;
120 return result;
121 } // end ProtoSpace::RemoveNode()
122
123 bool ProtoSpace::ContainsNode(Node& node)
124 {
125 if (0 == num_dimensions) return false;
126 ASSERT(node.GetDimensions() == num_dimensions);
127 bool result = true;
128 for (unsigned int i = 0; i < num_dimensions; i++)
129 {
130 Ordinate tempOrd;
131 tempOrd.SetNode(&node);
132 tempOrd.SetValue(node.GetOrdinate(i));
133 Ordinate* ord = static_cast<Ordinate*>(ord_tree[i].Find(tempOrd.GetKey(), tempOrd.GetKeysize()));
134 if (NULL == ord)
135 {
136 //PLOG(PL_WARN, "ProtoSpace::RemoveNode() warning: no ordinate[%u] for node\n", i);
137 return false;
138 }
139 }
140 return result;
141 } // end ProtoSpace::ContainsNode()
142
143
144 ProtoSpace::Iterator::Iterator(ProtoSpace& theSpace)
145 : space(theSpace), orig(NULL), bbox_radius(0.0), x_factor(0.0),
146 pos_it(NULL), neg_it(NULL)
147 {
148 }
149
150 ProtoSpace::Iterator::~Iterator()
151 {
152 Destroy();
153 }
154
155 bool ProtoSpace::Iterator::Init(const double* originOrdinates)
156 {
157 Destroy();
158 // Allocate and set origin ordinates for iteration
159 unsigned int dim = space.GetDimensions();
160 if (NULL == (orig = new double[dim]))
161 {
162 PLOG(PL_ERROR, "ProtoSpace::Iterator::Init() error: unable to allocate 'orig' array: %s\n",
163 GetErrorString());
164 return false;
165 }
166 if (NULL != originOrdinates)
167 memcpy(orig, originOrdinates, dim*sizeof(double));
168 else
169 memset(orig, 0, dim*sizeof(double));
170
171 // Allocate and init positive ordinate iterators
172 if (NULL == (pos_it = new ProtoSortedTree::Iterator*[dim]))
173 {
174 PLOG(PL_ERROR, "ProtoSpace::Iterator::Init() error: unable to allocate 'pos_it' array: %s\n",
175 GetErrorString());
176 Destroy();
177 return false;
178 }
179 memset(pos_it, 0, dim*sizeof(ProtoSortedTree::Iterator*));
180 Ordinate tempOrd;
181 for (unsigned int i = 0; i < dim; i++)
182 {
183 tempOrd.SetValue(orig[i]);
184 if (NULL == (pos_it[i] = new ProtoSortedTree::Iterator(space.ord_tree[i],
185 false,
186 tempOrd.GetKey(),
187 Ordinate::KEYBITS)))
188 {
189 Destroy();
190 return false;
191 }
192 }
193
194 // Allocate and init negative ordinate iterators
195 if (NULL == (neg_it = new ProtoSortedTree::Iterator*[dim]))
196 {
197 PLOG(PL_ERROR, "ProtoSpace::Iterator::Init() error: unable to allocate 'pos_it' array: %s\n",
198 GetErrorString());
199 Destroy();
200 return false;
201 }
202 memset(neg_it, 0, dim*sizeof(ProtoSortedTree::Iterator*));
203 for (unsigned int i = 0; i < dim; i++)
204 {
205 tempOrd.SetValue(orig[i]);
206 if (NULL == (neg_it[i] = new ProtoSortedTree::Iterator(space.ord_tree[i],
207 false,
208 tempOrd.GetKey(),
209 Ordinate::KEYBITS)))
210 {
211 Destroy();
212 return false;
213 }
214 neg_it[i]->Reverse();
215 }
216
217 #ifdef USE_SDT
218 // Create bounding box for SDT visualization
219 printf("node ul position %f,%f label off\n", originOrdinates[0], originOrdinates[1]);
220 printf("node ur position %f,%f label off\n", originOrdinates[0], originOrdinates[1]);
221 printf("node ll position %f,%f label off\n", originOrdinates[0], originOrdinates[1]);
222 printf("node lr position %f,%f label off\n", originOrdinates[0], originOrdinates[1]);
223 printf("link ul,ur,blue,2\n");
224 printf("link ul,ll,blue,2\n");
225 printf("link ur,lr,blue,2\n");
226 printf("link ll,lr,blue,2\n");
227 #endif // USE_SDT
228
229 bbox_radius = 0.0;
230 x_factor = sqrt((double)dim);
231
232 return true;
233 } // end ProtoSpace::Iterator::Init()
234
235 void ProtoSpace::Iterator::Destroy()
236 {
237 // empty our queue
238 Ordinate* nextOrd;
239 while (NULL != (nextOrd = static_cast<Ordinate*>(ord_tree.RemoveHead())))
240 space.ReturnOrdinateToPool(*nextOrd);
241 if (NULL != orig)
242 {
243 delete[] orig;
244 orig = NULL;
245 }
246 unsigned int dim = space.GetDimensions();
247 if (NULL != pos_it)
248 {
249 for (unsigned int i = 0; i < dim; i++)
250 {
251 if (NULL != pos_it[i]) delete pos_it[i];
252 }
253 delete[] pos_it;
254 pos_it = NULL;
255 }
256 if (NULL != neg_it)
257 {
258 for (unsigned int i = 0; i < dim; i++)
259 {
260 if (NULL != neg_it[i]) delete neg_it[i];
261 }
262 delete[] neg_it;
263 neg_it = NULL;
264 }
265 if (NULL != orig)
266 {
267 delete[] orig;
268 orig = NULL;
269 }
270 } // end ProtoSpace::Iterator::Destroy()
271
272
273 void ProtoSpace::Iterator::Reset(const double* originOrdinates)
274 {
275 unsigned int dim = space.GetDimensions();
276 ASSERT(NULL != orig);
277 if (NULL != originOrdinates)
278 memcpy(orig, originOrdinates, dim*sizeof(double));
279 Ordinate tempOrd;
280 for (unsigned int i = 0; i < dim; i++)
281 {
282 tempOrd.SetValue(orig[i]);
283 pos_it[i]->Reset(false, tempOrd.GetKey(), Ordinate::KEYBITS);
284 neg_it[i]->Reset(false, tempOrd.GetKey(), Ordinate::KEYBITS);
285 neg_it[i]->Reverse();
286 }
287 // empty our queue
288 Ordinate* nextOrd;
289 while (NULL != (nextOrd = static_cast<Ordinate*>(ord_tree.RemoveHead())))
290 space.ReturnOrdinateToPool(*nextOrd);
291 } // end ProtoSpace::Iterator::Reset()
292
293 /**
294 * Return the Node of those discovered by our expanding
295 * bounding box that meets these criteria
296 * 1) Closest location to <orig_x, orig_y>, _and_
297 * 2) Falls within the current bounding box
298 */
299 ProtoSpace::Node* ProtoSpace::Iterator::GetNextNode(double* distance)
300 {
301 unsigned int dim = space.GetDimensions();
302
303 while(1)
304 {
305 Ordinate* nextOrd = static_cast<Ordinate*>(ord_tree.GetHead());
306 if (NULL != nextOrd)
307 {
308 Node* nextNode = nextOrd->GetNode();
309 ASSERT(NULL != nextNode);
310 bool inBox = true;
311 if (bbox_radius >= 0.0)
312 {
313
314 // Is the "nextNode" within the bbox*x_factor
315 // (Note "x_factor = radius * sqrt(num_dimensions))
316 double xRadius = bbox_radius / x_factor;
317 for (unsigned int i = 0; i < dim; i++)
318 {
319 if ((nextNode->GetOrdinate(i) <= (orig[i] - xRadius)) ||
320 (nextNode->GetOrdinate(i) >= (orig[i] + xRadius)))
321 {
322 inBox = false;
323 break;
324 }
325 }
326 }
327 if (inBox)
328 {
329 ord_tree.RemoveHead();
330 if (NULL != distance)
331 {
332 *distance = sqrt(nextOrd->GetValue());
333 //TRACE("ProtoSpace returning node01x distance %lf\n", *distance);
334 }
335 space.ReturnOrdinateToPool(*nextOrd);
336 return nextNode;
337 }
338
339 }
340 else if (bbox_radius < 0.0)
341 {
342 return NULL; // finished
343 }
344
345
346 // A) Find the ordinate with smallest delta from origin
347 // to set "radius" of current bounding space
348 double deltaMin = -1.0;
349 unsigned int index = 0; // which dimensional iterator was closest
350 bool neg = false; // Was it "pos_it" or "neg_it"
351 for (unsigned int direction = 0; direction <= 1; direction++)
352 {
353 for (unsigned int i = 0 ; i < dim; i++)
354 {
355 Ordinate* ord = (0 == direction) ?
356 static_cast<Ordinate*>(neg_it[i]->PeekPrevItem()) :
357 static_cast<Ordinate*>(pos_it[i]->PeekNextItem());
358 if (NULL != ord)
359 {
360 Node* node = ord->GetNode();
361 ASSERT(NULL != node);
362 double delta = fabs(ord->GetValue() - orig[i]);
363 if ((delta < deltaMin) || (deltaMin < 0.0))
364 {
365 deltaMin = delta;
366 index = i;
367 neg = (0 == direction);
368 }
369 }
370 }
371 }
372 bbox_radius = deltaMin;
373 if (deltaMin < 0.0) continue;
374
375 // Output new bounding box for SDT visualization
376 #ifdef USE_SDT
377 printf("node ul position %f,%f\n", orig[0] - deltaMin, orig[1] - deltaMin);
378 printf("node ur position %f,%f\n", orig[0] + deltaMin, orig[1] - deltaMin);
379 printf("node ll position %f,%f\n", orig[0] - deltaMin, orig[1] + deltaMin);
380 printf("node lr position %f,%f\n", orig[0] + deltaMin, orig[1] + deltaMin);
381 #endif // USE_SDT
382
383 Node* node = neg ? static_cast<Ordinate*>(neg_it[index]->PeekPrevItem())->GetNode() :
384 static_cast<Ordinate*>(pos_it[index]->PeekNextItem())->GetNode();
385
386 // B) Is the closest node on this axis within our current bounding box?
387 bool inBox = true;
388 for (unsigned int i = 0; i < dim; i++)
389 {
390 double ordDelta = fabs(orig[i] - node->GetOrdinate(i));
391 if (ordDelta > deltaMin)
392 {
393 inBox = false;
394 break;
395 }
396 }
397
398 // C) Enqueue any nodes that lie _within_ the current bounding space
399 if (inBox)
400 {
401 // a) calculate distance
402 double distPartial = 0.0;
403 for (unsigned j = 0; j < dim; j++)
404 {
405 double delta = node->GetOrdinate(j) - orig[j];
406 distPartial += delta*delta;
407 }
408
409
410
411 // b) get and init Ordinate
412 Ordinate* ord = space.GetOrdinateFromPool();
413 if ((NULL == ord) && (NULL == (ord = new Ordinate)))
414 {
415 PLOG(PL_ERROR, "ProtoSpace::Iterator::GetNextNode() error: unable to allocate Ordinate: %s\n",
416 GetErrorString());
417 return NULL;
418 }
419 ord->SetNode(node);
420 ord->SetValue(distPartial);
421
422 // c) if not already enqueued, enqueue it
423 if (NULL == ord_tree.Find(ord->GetKey(), Ordinate::KEYBITS))
424 ord_tree.Insert(*ord);
425 else
426 space.ReturnOrdinateToPool(*ord);
427 }
428
429 // D) Consume the applicable ordinate, expanding our bounding box
430 if (neg)
431 neg_it[index]->GetPrevItem();
432 else
433 pos_it[index]->GetNextItem();
434
435 #ifdef USE_SDT
436 printf("wait 1\n");
437 #endif // USE_SDT
438
439 } // end while(1)
440
441 } // end ProtoSpace::Iterator::GetNextNode()
442
443