Branch data Line data Source code
1 : : //-----------------------------------------------------------------
2 : : //- Class: KDDTree
3 : : //- Author: Kevin Albrecht
4 : : //- Created: 13 May 2003
5 : : //- Updated: 8 Feb 2004
6 : : //-
7 : : //- Description:
8 : : //- Dynamic version of the k-d tree, where k=3.
9 : : //-
10 : : //- References:
11 : : //-
12 : : //- Hanan Samet. Design and Analysis of Spatial Data Structures.
13 : : //- Addison-Wesley, Reading, MA, 1990.
14 : : //-
15 : : //- Jon Louis Bentley. Multidimensional binary search trees used
16 : : //- for associative searching. In Communications of the ACM,
17 : : //- 18(9), pages 509-517, September 1975.
18 : : //-----------------------------------------------------------------
19 : :
20 : : //---------------------------------
21 : : // Include Files
22 : : //---------------------------------
23 : :
24 : : #include <cstdlib>
25 : : #include <cstdio>
26 : : #include <time.h>
27 : :
28 : : #include "KDDTree.hpp"
29 : : #include "KDDTreeNode.hpp"
30 : : #include "CubitBox.hpp"
31 : : #include "CubitVector.hpp"
32 : : #include "DLIList.hpp"
33 : : #include "PriorityQueue.hpp"
34 : :
35 : : //---------------------------------
36 : : // Define Methods
37 : : //---------------------------------
38 : :
39 : : //- Constructor
40 : : //- * The following only applies if self-balancing is turned on: if
41 : : //- selfBalancingDeletionTolerance is set to 0, then there is no limit
42 : : //- to the number of nodes that can be marked for deletion at a time;
43 : : //- otherwise, the tree will rebalance itself whenever the percentage
44 : : //- of nodes on the tree marked for deletion is greater than the
45 : : //- tolerance.
46 : 0 : template <class Z> KDDTree<Z>::KDDTree (double tol, CubitBoolean selfBalancingOn,
47 : : double selfBalancingDeletionTolerance,
48 [ # # ][ # # ]: 0 : CubitBoolean randomOn)
49 : : {
50 : 0 : root = NULL;
51 : :
52 : 0 : myTolerance = tol;
53 : 0 : mySelfBalancingOn = selfBalancingOn;
54 : 0 : myDeletionTolerance = selfBalancingDeletionTolerance;
55 : 0 : myMarkedNodes = 0;
56 : 0 : myRandomOn = randomOn;
57 : :
58 [ # # ]: 0 : if (myRandomOn)
59 : : {
60 : : //// seed the random number generator
61 : 0 : srand( (unsigned)time( NULL ) );
62 : : }
63 : 0 : }
64 : :
65 : : //- Destructor
66 : 0 : template <class Z> KDDTree<Z>::~KDDTree()
67 : : {
68 : : int i;
69 [ # # ][ # # ]: 0 : for (i = myAddList.size(); i > 0; i--)
70 : : {
71 [ # # ][ # # ]: 0 : delete myAddList.pop();
[ # # ]
72 : : }
73 [ # # ][ # # ]: 0 : for (i = myNodeList.size(); i > 0; i--)
74 : : {
75 [ # # ]: 0 : KDDTreeNode<Z> *node = myNodeList.pop();
76 [ # # ][ # # ]: 0 : delete node;
77 : : }
78 [ # # ][ # # ]: 0 : }
[ # # ]
79 : :
80 : : //- Immediately put all nodes on list onto the tree
81 : 0 : template <class Z> CubitStatus KDDTree<Z>::dump_list ()
82 : : {
83 [ # # ]: 0 : while (myAddList.size() > 0)
84 : : {
85 : 0 : KDDTreeNode<Z> *node = myAddList.pop();
86 : 0 : insert_node (node);
87 : : }
88 : :
89 : 0 : return CUBIT_SUCCESS;
90 : : }
91 : :
92 : : //- "insert_node"
93 : : //- Dynamically insert the data into the k-d tree
94 : : //-
95 : : //- Algorithm INSERT (From Bentley):
96 : : //- This algoritm is passed an object "data" of class "Z",
97 : : //- which has a bounding_box() method. If there is already
98 : : //- a node in the tree with equal bounding box center point,
99 : : //- it is put in the right subtree.
100 : : //- I0. [Create new node] Create a node P with the bounding box
101 : : //- specified, and set P.LEFT <- null, P.RIGHT <- null, and
102 : : //- P.DISC <- null.
103 : : //- I1. [Check for null tree] If ROOT = null, then set ROOT <- P
104 : : //- and return CUBIT_SUCCESS; otherwise, set Q <- ROOT (Q
105 : : //- will move down the tree).
106 : : //- I2. [Compare] Compare the nodes and set the child in the
107 : : //- correct direction to T.
108 : : //- I3. [Move down] Set Q <- child of Q and go to I2.
109 : : //- I4. [Insert new node in tree] Set the child of Q to P, then
110 : : //- set the children of P to null. Set the discriminator of
111 : : //- P to be the discriminator after that in Q.
112 : : //-
113 : 0 : template <class Z> CubitStatus KDDTree<Z>::insert_node (KDDTreeNode<Z>* P)
114 : : {
115 : 0 : KDDTreeNode<Z> *F = NULL; // father node
116 : : KDDTreeNode<Z> *T; // temp node
117 : :
118 [ # # ]: 0 : if (root == NULL)
119 : : {
120 : 0 : root = P;
121 : 0 : P->set_disc (DIMX);
122 : : }
123 : : else
124 : : {
125 : 0 : T = root;
126 : 0 : DIRECTION direction = DIR_NULL;
127 : :
128 [ # # ]: 0 : while (T != NULL)
129 : : {
130 : 0 : F = T; // remember the father
131 [ # # ]: 0 : direction = P->compare (T);
132 : :
133 [ # # ]: 0 : CubitVector tmin = T->safetyBox.minimum();
134 [ # # ]: 0 : CubitVector tmax = T->safetyBox.maximum();
135 [ # # ]: 0 : CubitVector pmin = P->safetyBox.minimum();
136 [ # # ]: 0 : CubitVector pmax = P->safetyBox.maximum();
137 : :
138 [ # # ][ # # ]: 0 : if (pmin.x() < tmin.x()) tmin.x (pmin.x());
[ # # ][ # # ]
[ # # ]
139 [ # # ][ # # ]: 0 : if (pmin.y() < tmin.y()) tmin.y (pmin.y());
[ # # ][ # # ]
[ # # ]
140 [ # # ][ # # ]: 0 : if (pmin.z() < tmin.z()) tmin.z (pmin.z());
[ # # ][ # # ]
[ # # ]
141 [ # # ][ # # ]: 0 : if (pmax.x() > tmax.x()) tmax.x (pmax.x());
[ # # ][ # # ]
[ # # ]
142 [ # # ][ # # ]: 0 : if (pmax.y() > tmax.y()) tmax.y (pmax.y());
[ # # ][ # # ]
[ # # ]
143 [ # # ][ # # ]: 0 : if (pmax.z() > tmax.z()) tmax.z (pmax.z());
[ # # ][ # # ]
[ # # ]
144 : :
145 [ # # ]: 0 : T->safetyBox.reset (tmin, tmax);
146 : :
147 [ # # ]: 0 : T = T->get_child (direction);
148 : : }
149 : :
150 : 0 : F->set_child (P, direction);
151 : 0 : P->set_disc (F->next_disc());
152 : 0 : P->parent = F;
153 : : }
154 : 0 : myNodeList.push (P);
155 : :
156 : 0 : return CUBIT_SUCCESS;
157 : : }
158 : :
159 : : //- "modifind"
160 : : //- Rearrange the array around the median point.
161 : : //-
162 : : //- Description:
163 : : //- This is the MODIFIND algorithm of V. Zabrodsky, but modified to choose a random
164 : : //- pivot point. This is in turn a modified version of the Hoare FIND algorithm for
165 : : //- finding the median point. Running time is O(n^2) in the worst case and O(n) in
166 : : //- the average case.
167 : : //-
168 : : //- Zabrodsky's algorithm:
169 : : //- http://www.geocities.com/zabrodskyvlada/aat/a_modi.html
170 : : //- http://www.geocities.com/zabrodskyvlada/3alg.html
171 : : //-
172 : : //- Results:
173 : : //- Reordering of input array such that A[K] has the value it would have if A were
174 : : //- sorted, L<=I<=K will imply A[I]<=A[K], and K<=I<=R will imply A[I]>=A[K]
175 : 0 : template <class Z> int KDDTree<Z>::modifind (DIMENSION dim, int left, int right,
176 : : KDDTreeNode<Z>* array[])
177 : : {
178 : 0 : int K = ((right - left + 1) / 2) + left + 1;
179 : 0 : int L = left + 1;
180 : 0 : int R = right + 1;
181 : : int I, J;
182 : : KDDTreeNode<Z>* node; // "X" in MODIFIND
183 : : KDDTreeNode<Z>* temp; // temp for swapping; "W" in MODIFIND
184 : :
185 : : //// Choose pivot between left and right
186 [ # # ]: 0 : if (myRandomOn == CUBIT_TRUE)
187 : : {
188 : 0 : int pivot = ( rand() % (right - left) ) + left; // create random pivot between left and right
189 : :
190 : : //// Swap array[pivot] and array[K]
191 : 0 : temp = array[pivot];
192 : 0 : array[pivot] = array[K - 1];
193 : 0 : array[K - 1] = temp;
194 : : }
195 : :
196 [ # # ]: 0 : while (L < R)
197 : : {
198 : 0 : node = array[K - 1];
199 : 0 : I = L;
200 : 0 : J = R;
201 [ # # ][ # # ]: 0 : while (! ((J < K) || (K < I)) )
202 : : {
203 [ # # ]: 0 : if (dim == DIMX)
204 : : {
205 [ # # ]: 0 : while (array[I - 1]->x < node->x) {
206 : 0 : I++;
207 : : }
208 [ # # ]: 0 : while (node->x < array[J - 1]->x) {
209 : 0 : J--;
210 : : }
211 : : }
212 [ # # ]: 0 : else if (dim == DIMY)
213 : : {
214 [ # # ]: 0 : while (array[I - 1]->y < node->y) {
215 : 0 : I++;
216 : : }
217 [ # # ]: 0 : while (node->y < array[J - 1]->y) {
218 : 0 : J--;
219 : : }
220 : : }
221 : : else
222 : : {
223 [ # # ]: 0 : while (array[I - 1]->z < node->z) {
224 : 0 : I++;
225 : : }
226 [ # # ]: 0 : while (node->z < array[J - 1]->z) {
227 : 0 : J--;
228 : : }
229 : : }
230 : :
231 : : //// Swap array[I] and array[J]
232 : 0 : temp = array[I - 1];
233 : 0 : array[I - 1] = array[J - 1];
234 : 0 : array[J - 1] = temp;
235 : :
236 : 0 : I++;
237 : 0 : J--;
238 : : }
239 : :
240 [ # # ]: 0 : if (J < K)
241 : : {
242 : 0 : L = I;
243 : : }
244 [ # # ]: 0 : if (K < I)
245 : : {
246 : 0 : R = J;
247 : : }
248 : : }
249 : :
250 : 0 : return K - 1;
251 : : }
252 : :
253 : : //- "balance" and "recursive_balance"
254 : : //- Create a balanced tree out of the nodes on the tree and in the Add List.
255 : : //- This is used to balance the tree manually; it is also called by the
256 : : //- find method when self-balancing is on.
257 : : //-
258 : : //- Description:
259 : : //- This is the OPTIMIZE algorithm of Bentley. Total running time is
260 : : //- O(n lg n). It uses the MODIFIND algorithm to find the median.
261 : : //-
262 : : //- Results:
263 : : //- The tree produced will be balanced so that all leaf nodes occur on
264 : : //- at most two adjacent levels.
265 : : //-
266 : 0 : template <class Z> CubitStatus KDDTree<Z>::balance ()
267 : : {
268 : 0 : int arraypos = 0;
269 [ # # ]: 0 : KDDTreeNode<Z> ** array = new KDDTreeNode<Z>*[myAddList.size () + myNodeList.size ()];
270 : :
271 : 0 : root = NULL;
272 : :
273 [ # # ]: 0 : while (myAddList.size () > 0)
274 : : {
275 : 0 : array[arraypos] = myAddList.pop();
276 : 0 : arraypos++;
277 : : }
278 [ # # ]: 0 : while (myNodeList.size () > 0)
279 : : {
280 : 0 : array[arraypos] = myNodeList.pop();
281 : :
282 [ # # ]: 0 : if (array[arraypos]->valid == CUBIT_FALSE)
283 : : {
284 : 0 : array[arraypos]->left = NULL;
285 : 0 : array[arraypos]->right = NULL;
286 [ # # ]: 0 : delete array[arraypos];
287 : : }
288 : : else
289 : : {
290 : 0 : arraypos++;
291 : : }
292 : : }
293 : :
294 : 0 : int left = 0;
295 : 0 : int right = (arraypos - 1);
296 : 0 : root = recursive_balance (DIMX, left, right, array, NULL);
297 : :
298 : 0 : myMarkedNodes = 0;
299 : :
300 [ # # ]: 0 : delete [] array;
301 : 0 : return CUBIT_SUCCESS;
302 : : }
303 : :
304 : 0 : template <class Z> KDDTreeNode<Z> *KDDTree<Z>::recursive_balance
305 : : (DIMENSION dim, int left, int right, KDDTreeNode<Z>** array, KDDTreeNode<Z>* parent)
306 : : {
307 [ # # ]: 0 : if (left > right)
308 : : {
309 : 0 : return NULL;
310 : : }
311 : : else
312 : : {
313 : : KDDTreeNode<Z>* P;
314 : : int K;
315 : :
316 [ # # ]: 0 : if (left != right)
317 : : {
318 : 0 : K = modifind (dim, left, right, array);
319 : 0 : P = array[K];
320 : : }
321 : : else
322 : : {
323 : 0 : K = left;
324 : 0 : P = array[left];
325 : : }
326 : :
327 : 0 : myNodeList.push (P);
328 : :
329 : 0 : P->safetyBox.reset (P->boundingBox);
330 [ # # ]: 0 : for (int i = left; i <= right; i++)
331 : : {
332 [ # # ]: 0 : CubitVector imin = array[i]->safetyBox.minimum();
333 [ # # ]: 0 : CubitVector imax = array[i]->safetyBox.maximum();
334 [ # # ]: 0 : CubitVector pmin = P->safetyBox.minimum();
335 [ # # ]: 0 : CubitVector pmax = P->safetyBox.maximum();
336 : :
337 [ # # ][ # # ]: 0 : if (imin.x() < pmin.x()) pmin.x (imin.x());
[ # # ][ # # ]
[ # # ]
338 [ # # ][ # # ]: 0 : if (imin.y() < pmin.y()) pmin.y (imin.y());
[ # # ][ # # ]
[ # # ]
339 [ # # ][ # # ]: 0 : if (imin.z() < pmin.z()) pmin.z (imin.z());
[ # # ][ # # ]
[ # # ]
340 [ # # ][ # # ]: 0 : if (imax.x() > pmax.x()) pmax.x (imax.x());
[ # # ][ # # ]
[ # # ]
341 [ # # ][ # # ]: 0 : if (imax.y() > pmax.y()) pmax.y (imax.y());
[ # # ][ # # ]
[ # # ]
342 [ # # ][ # # ]: 0 : if (imax.z() > pmax.z()) pmax.z (imax.z());
[ # # ][ # # ]
[ # # ]
343 : :
344 [ # # ]: 0 : P->safetyBox.reset (pmin, pmax);
345 : : }
346 : :
347 : : DIMENSION nextDim;
348 [ # # # ]: 0 : switch (dim)
349 : : {
350 : 0 : case DIMX: nextDim = DIMY; break;
351 : 0 : case DIMY: nextDim = DIMZ; break;
352 : 0 : default: nextDim = DIMX;
353 : : }
354 : :
355 : 0 : P->set_disc (dim);
356 : 0 : P->parent = parent;
357 : :
358 : 0 : P->left = recursive_balance (nextDim, left, K - 1, array, P);
359 : 0 : P->right = recursive_balance (nextDim, K + 1, right, array, P);
360 : :
361 : 0 : return P;
362 : : }
363 : : }
364 : :
365 : : //- Find the depth of the tree
366 : : template <class Z> int KDDTree<Z>::find_max_height ()
367 : : {
368 : : int depth = 0, maxdepth = 0;
369 : : recursive_find_max_height (root, depth, &maxdepth);
370 : :
371 : : return maxdepth;
372 : : }
373 : :
374 : : //- Find the depth of the tree
375 : : template <class Z> void KDDTree<Z>::recursive_find_max_height
376 : : (KDDTreeNode<Z> *root, int depth, int *maxdepth)
377 : : {
378 : : if (root)
379 : : {
380 : : depth++;
381 : : if (depth > *maxdepth)
382 : : {
383 : : *maxdepth = depth;
384 : : }
385 : : recursive_find_max_height (root->left, depth, maxdepth);
386 : : recursive_find_max_height (root->right, depth, maxdepth);
387 : : }
388 : : }
389 : :
390 : : //- Add a node with the data to the list
391 : 0 : template <class Z> CubitStatus KDDTree<Z>::add (Z data)
392 : : {
393 [ # # ]: 0 : KDDTreeNode<Z> *P = new KDDTreeNode<Z> (data);
394 [ # # ]: 0 : P->safetyBox.reset (data->bounding_box());
395 : :
396 : 0 : myAddList.push (P);
397 : :
398 : 0 : return CUBIT_SUCCESS;
399 : : }
400 : :
401 : : //- Return a pointer to the node containing the specified data
402 : 0 : template <class Z> KDDTreeNode<Z> *KDDTree<Z>::find_node_containing_data (KDDTreeNode<Z> *subtreeRoot, Z data)
403 : : {
404 [ # # ]: 0 : KDDTreeNode<Z> *T = new KDDTreeNode<Z>(data); // temp node to use in searching
405 : 0 : KDDTreeNode<Z> *P = subtreeRoot; // node to delete
406 : : DIRECTION D;
407 : :
408 : : //// Find the node
409 [ # # ]: 0 : while (P != NULL)
410 : : {
411 [ # # ][ # # ]: 0 : if ((P->boundingBox.minimum() == T->boundingBox.minimum()) &&
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # #
# # # # ]
412 [ # # ][ # # ]: 0 : (P->boundingBox.maximum() == T->boundingBox.maximum()))
[ # # ][ # # ]
[ # # ]
[ # # # # ]
413 : : {
414 [ # # ]: 0 : if (P->valid == CUBIT_TRUE)
415 : : {
416 : 0 : break; // the bounding boxes match and this node has not been deleted
417 : : }
418 : : }
419 : :
420 : 0 : D = T->compare_with_equality (P);
421 : :
422 [ # # ]: 0 : if (D == DIR_EITHER)
423 : : {
424 : 0 : KDDTreeNode<Z> *leftResult = find_node_containing_data (P->get_child (DIR_LEFT), data);
425 : :
426 [ # # ]: 0 : if (leftResult != NULL)
427 : : {
428 : 0 : P = leftResult;
429 : 0 : break;
430 : : }
431 : :
432 : 0 : KDDTreeNode<Z> *rightResult = find_node_containing_data (P->get_child (DIR_RIGHT), data);
433 : :
434 [ # # ]: 0 : if (rightResult != NULL)
435 : : {
436 : 0 : P = rightResult;
437 : 0 : break;
438 : : }
439 : :
440 : 0 : P = NULL;
441 : : }
442 : : else
443 : : {
444 : 0 : P = P->get_child (D);
445 : : }
446 : : }
447 : :
448 [ # # ]: 0 : delete T;
449 : :
450 : 0 : return P;
451 : : }
452 : :
453 : : //- Remove the data member's entry in the tree. Returns CUBIT_TRUE
454 : : //- if item removed, CUBIT_FALSE if item not in tree.
455 : 0 : template <class Z> CubitBoolean KDDTree<Z>::remove (Z data)
456 : : {
457 : : //// If the Add List is not empty, action must be taken
458 [ # # ]: 0 : if (myAddList.size() > 0)
459 : : {
460 [ # # ]: 0 : if (mySelfBalancingOn == CUBIT_TRUE) // self-balancing is on, so rebalance the tree
461 : : {
462 : 0 : balance ();
463 : : }
464 : : else // self-balancing is off, so put everything in the Add List onto the tree
465 : : {
466 : 0 : dump_list ();
467 : : }
468 : : }
469 : :
470 : : //// Tree is empty
471 [ # # ]: 0 : if (root == NULL)
472 : : {
473 : 0 : return CUBIT_FALSE;
474 : : }
475 : : //// Tree is not empty
476 : : else
477 : : {
478 : 0 : KDDTreeNode<Z> *P = find_node_containing_data (root, data);
479 : :
480 [ # # ]: 0 : if (P == NULL) // no matching node was found
481 : : {
482 : 0 : return CUBIT_FALSE;
483 : : }
484 : : else // mark the matching node for deletion
485 : : {
486 [ # # ]: 0 : if (P->valid == CUBIT_FALSE)
487 : : {
488 : 0 : return CUBIT_FALSE; // this node was already deleted
489 : : }
490 : :
491 : 0 : P->valid = CUBIT_FALSE; // set the node to be deleted
492 : :
493 : 0 : myMarkedNodes++;
494 [ # # ]: 0 : if (myDeletionTolerance != 0)
495 : : {
496 [ # # ][ # # ]: 0 : if ( (((double)myMarkedNodes / myNodeList.size()) > myDeletionTolerance) &&
[ # # ]
497 : : (myMarkedNodes > 1)
498 : : )
499 : : {
500 : 0 : balance ();
501 : : }
502 : : }
503 : :
504 : 0 : return CUBIT_TRUE;
505 : : }
506 : : }
507 : : }
508 : :
509 : : //- Find members intersecting this range box
510 : 0 : template <class Z> CubitStatus KDDTree<Z>::find (const CubitBox &range_box, DLIList <Z> &range_members)
511 : : {
512 : : //// If the Add List is not empty, action must be taken
513 [ # # ]: 0 : if (myAddList.size() > 0)
514 : : {
515 [ # # ]: 0 : if (mySelfBalancingOn == CUBIT_TRUE) // self-balancing is on, so rebalance the tree
516 : : {
517 : 0 : balance ();
518 : : }
519 : : else // self-balancing is off, so put everything in the Add List onto the tree
520 : : {
521 : 0 : dump_list ();
522 : : }
523 : : }
524 : :
525 : : //// Find all of the members of the tree that intersect this range_box
526 [ # # ]: 0 : if (root != NULL)
527 : : {
528 : 0 : recursive_find (root, range_box, range_members);
529 : : }
530 : :
531 : 0 : return CUBIT_SUCCESS;
532 : : }
533 : :
534 : : //- Recursively find members intersecting this range box (called by "find")
535 : 0 : template <class Z> void KDDTree<Z>::recursive_find
536 : : ( KDDTreeNode<Z> *rect_tree,
537 : : const CubitBox &range_box,
538 : : DLIList <Z> &range_members
539 : : )
540 : : {
541 : : //// Check for overlap with the safety box
542 [ # # ]: 0 : if ( ! range_box.overlap (myTolerance, rect_tree->safetyBox) )
543 : : {
544 : 0 : return; // no overlap, return
545 : : }
546 : :
547 : : //// Check for overlap with the bounding box
548 [ # # ]: 0 : if ( range_box.overlap (myTolerance, rect_tree->boundingBox) )
549 : : {
550 [ # # ]: 0 : if (rect_tree->valid == CUBIT_TRUE)
551 : : {
552 : 0 : range_members.append (rect_tree->data); // append the data to the list.
553 : : }
554 : : }
555 : :
556 [ # # ]: 0 : if (rect_tree->left != NULL)
557 : : {
558 : 0 : recursive_find (rect_tree->left, range_box, range_members);
559 : : }
560 [ # # ]: 0 : if (rect_tree->right != NULL)
561 : : {
562 : 0 : recursive_find (rect_tree->right, range_box, range_members);
563 : : }
564 : :
565 : 0 : return;
566 : : }
567 : :
568 : : //- Finds the minimum distance squared between the given point and the box. If
569 : : //- the point is on or in the box, the min distance is zero.
570 : 0 : template <class Z> double KDDTree<Z>::min_dist_sq (CubitVector &q, CubitBox &b_box)
571 : : {
572 [ # # ]: 0 : CubitVector b_min = b_box.minimum();
573 [ # # ]: 0 : CubitVector b_max = b_box.maximum();
574 [ # # ]: 0 : CubitVector r;
575 : :
576 : : //// set "r" in the x-dim
577 [ # # ][ # # ]: 0 : if (q.x () < b_min.x ())
[ # # ]
578 : : {
579 [ # # ][ # # ]: 0 : r.x (b_min.x ());
580 : : }
581 [ # # ][ # # ]: 0 : else if (q.x () > b_max.x ())
[ # # ]
582 : : {
583 [ # # ][ # # ]: 0 : r.x (b_max.x ());
584 : : }
585 : : else
586 : : {
587 [ # # ][ # # ]: 0 : r.x (q.x ());
588 : : }
589 : :
590 : : //// set "r" in the y-dim
591 [ # # ][ # # ]: 0 : if (q.y () < b_min.y ())
[ # # ]
592 : : {
593 [ # # ][ # # ]: 0 : r.y (b_min.y ());
594 : : }
595 [ # # ][ # # ]: 0 : else if (q.y () > b_max.y ())
[ # # ]
596 : : {
597 [ # # ][ # # ]: 0 : r.y (b_max.y ());
598 : : }
599 : : else
600 : : {
601 [ # # ][ # # ]: 0 : r.y (q.y ());
602 : : }
603 : :
604 : : //// set "r" in the z-dim
605 [ # # ][ # # ]: 0 : if (q.z () < b_min.z ())
[ # # ]
606 : : {
607 [ # # ][ # # ]: 0 : r.z (b_min.z ());
608 : : }
609 [ # # ][ # # ]: 0 : else if (q.z () > b_max.z ())
[ # # ]
610 : : {
611 [ # # ][ # # ]: 0 : r.z (b_max.z ());
612 : : }
613 : : else
614 : : {
615 [ # # ][ # # ]: 0 : r.z (q.z ());
616 : : }
617 : :
618 [ # # ][ # # ]: 0 : double dist = (q-r).length_squared();
619 : :
620 : 0 : return dist;
621 : : }
622 : :
623 : 0 : template <class Z> bool KDDTree<Z>::less_than_func (KDDTreeNode<Z> *&node_a,
624 : : KDDTreeNode<Z> *&node_b)
625 : : {
626 [ # # ]: 0 : if (node_a->get_dist() < node_b->get_dist ())
627 : : {
628 : 0 : return true;
629 : : }
630 : : else
631 : : {
632 : 0 : return false;
633 : : }
634 : : }
635 : :
636 : : //- "k_nearest_neighbor"
637 : : //- Find the K nearest neighbors to a point.
638 : : //-
639 : : //- Description:
640 : : //- This algorithm is based on the best-first search. The goal of this
641 : : //- algorithm is to minimize the number of nodes visited by using the
642 : : //- distance to each subtree's bounding box to avoid visiting subtrees
643 : : //- which could not possibly contain one of the k nearest objects.
644 : : //-
645 : 0 : template <class Z> CubitStatus KDDTree<Z>::k_nearest_neighbor
646 : : (CubitVector &q, int k, double &closest_dist, DLIList<Z> &nearest_neighbors,
647 : : typename KDDTree<Z>::DistSqFunc dist_sq_point_data
648 : : )
649 : : {
650 : : //// Create the priority queues
651 [ # # ]: 0 : PriorityQueue<KDDTreeNode<Z>*> *queue = new PriorityQueue<KDDTreeNode<Z>*> (KDDTree<Z>::less_than_func);
652 [ # # ]: 0 : PriorityQueue<KDDTreeNode<Z>*> *queueTemp = new PriorityQueue<KDDTreeNode<Z>*> (KDDTree<Z>::less_than_func);
653 : :
654 : :
655 : 0 : KDDTreeNode<Z> *element = root;
656 : :
657 : : // push this node on the queue
658 : 0 : element->set_dist (min_dist_sq (q, element->safetyBox));
659 : 0 : element->set_dist_data (DD_SAFETY);
660 : 0 : queue->push (element);
661 : :
662 : :
663 : : // if the k closest nodes on the tree are not leaf-nodes, expand the closest
664 : : // non-leaf node
665 [ # # ]: 0 : while ( !queue->empty() )
666 : : {
667 : 0 : element = queue->top();
668 : 0 : queue->pop();
669 : :
670 [ # # ]: 0 : if (element->get_dist_data() == DD_LEAF)
671 : : {
672 : : // this node is a leaf, so it can be pushed onto the temporary queue
673 : 0 : queueTemp->push (element);
674 : : }
675 : : else
676 : : {
677 : : // one of the top k nodes is a non-leaf node, so expand it
678 [ # # ]: 0 : if (element->left)
679 : : {
680 : 0 : element->left->set_dist (min_dist_sq (q, element->left->safetyBox));
681 : 0 : element->left->set_dist_data (DD_SAFETY);
682 : 0 : queue->push (element->left);
683 : : }
684 [ # # ]: 0 : if (element->right)
685 : : {
686 : 0 : element->right->set_dist (min_dist_sq (q, element->right->safetyBox));
687 : 0 : element->right->set_dist_data (DD_SAFETY);
688 : 0 : queue->push (element->right);
689 : : }
690 : 0 : element->set_dist (dist_sq_point_data (q, element->data));
691 : 0 : element->set_dist_data (DD_LEAF);
692 : 0 : queue->push (element);
693 : :
694 : : // take all the elements in the temporary queue and reinsert them into
695 : : // the actual queue
696 [ # # ]: 0 : while ( !queueTemp->empty() )
697 : : {
698 : 0 : queue->push ( queueTemp->top() );
699 : 0 : queueTemp->pop ();
700 : : }
701 : : }
702 : :
703 [ # # ]: 0 : if (queueTemp->size() == k)
704 : : {
705 : : // success-- place the k nodes into the nearest_neighbors list
706 : 0 : element = queueTemp->top();
707 : 0 : queueTemp->pop();
708 : 0 : closest_dist = element->get_dist();
709 : 0 : nearest_neighbors.append (element->data);
710 : :
711 [ # # ]: 0 : while ( !queueTemp->empty() )
712 : : {
713 : 0 : nearest_neighbors.append ( queueTemp->top()->data );
714 : 0 : queueTemp->pop();
715 : : }
716 : :
717 : 0 : return CUBIT_SUCCESS;
718 : : }
719 : : }
720 : 0 : return CUBIT_FAILURE;
721 : : }
722 : :
|
