Branch data Line data Source code
1 : : //-------------------------------------------------------------------------
2 : : // Filename : OctTree.cpp
3 : : //
4 : : // Purpose : Class for O(ln n) search for nodes within a specified
5 : : // tolerance of a specified position.
6 : : //
7 : : // Special Notes :
8 : : //
9 : : // Creator : Jason Kraftcheck
10 : : //
11 : : // Creation Date : 01/30/02
12 : : //-------------------------------------------------------------------------
13 : :
14 : : #include "OctTree.hpp"
15 : : #include "OctTreeCell.hpp"
16 : : #include "DLIList.hpp"
17 : : #include "CubitDefines.h"
18 : : #include "GeometryDefines.h"
19 : :
20 : : const int DEFAULT_MIN_NODES_PER_BOX = 6;
21 : : // The default value to use for the minimum number of
22 : : // nodes in a box, if none is specified in the constructr.
23 : :
24 : : const int OCT_TREE_BOX_PAGE_SIZE = 8192;
25 : : // The size of memory blocks to allocate for holding
26 : : // oct-tree nodes.
27 : :
28 : : const int OCT_TREE_CHUNK_SIZE = 8 * sizeof(OctTreeCell<int,int>);
29 : : // Do not change. Size of a child array for a oct-tree node.
30 : :
31 : : const int OCT_TREE_CHUNK_PER_PAGE = OCT_TREE_BOX_PAGE_SIZE /
32 : : OCT_TREE_CHUNK_SIZE;
33 : : // Do not change. Number of arrays of 8 oct-tree nodes
34 : : // (child arrays) fit in a page.
35 : :
36 : : const int OCT_TREE_ALLOC_COUNT = OCT_TREE_CHUNK_PER_PAGE * 8;
37 : : // When allocating the page as one big array of oct-tree nodes,
38 : : // the size of the array to request. Will be sligthtly smaller
39 : : // than the requested page because page size probably is not a
40 : : // multiple of OCT_TREE_CHUNK_SIZE. That's a good thing though,
41 : : // because then the std-c and c++ memory allocators have a little
42 : : // extra space for internal data.
43 : :
44 : : //-------------------------------------------------------------------------
45 : : // Purpose : Constructor
46 : : //
47 : : // Special Notes :
48 : : //
49 : : // Creator : Jason Kraftcheck
50 : : //
51 : : // Creation Date : 01/30/02
52 : : //-------------------------------------------------------------------------
53 : : template<class X, class E>
54 : 0 : OctTree<X,E>::OctTree( DLIList<X*>& nodes,
55 : : double tolerance,
56 : : int min_nodes_per_box,
57 [ # # ][ # # ]: 0 : double min_box_dimension )
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
58 : : {
59 : : // Check and store constructor arguments
60 [ # # ]: 0 : tolerance_ = tolerance < GEOMETRY_RESABS ? GEOMETRY_RESABS : tolerance;
61 [ # # ]: 0 : min_nodes_ = min_nodes_per_box < 0 ? DEFAULT_MIN_NODES_PER_BOX : min_nodes_per_box;
62 : 0 : double tol3 = 3 * tolerance_;
63 [ # # ]: 0 : min_box_size_ = min_box_dimension < tol3 ? tol3 : min_box_dimension;
64 : : // Note: min_box_size must be greater than twice the tolerance
65 : : // or the internal stack used for searching the tree will
66 : : // overflow (more than 8 boxes may be within the tolerance
67 : : // of a passed position).
68 : :
69 : : // set up data for memory pool of oct-tree nodes
70 [ # # ][ # # ]: 0 : mem_pages_.append( new OctTreeCell<X,E>[OCT_TREE_ALLOC_COUNT] );
[ # # ][ # # ]
71 [ # # ]: 0 : curr_page_end_ = mem_pages_.get() + OCT_TREE_ALLOC_COUNT;
72 : :
73 : : // construct root node
74 [ # # ][ # # ]: 0 : node_memory_ = new OctTreeEntry<X,E>[nodes.size()];
[ # # ]
75 [ # # ]: 0 : nodes.reset();
76 : 0 : OctTreeEntry<X,E>* ptr = node_memory_;
77 [ # # ][ # # ]: 0 : for( int i = nodes.size(); i--; )
78 : : {
79 [ # # ]: 0 : ptr->node = nodes.get_and_step();
80 : 0 : ptr->next = 0;
81 : 0 : ptr++;
82 : : }
83 [ # # ][ # # ]: 0 : root_ = new OctTreeCell<X,E>( node_memory_, nodes.size() );
[ # # ]
84 : :
85 : : // get bounding box of all nodes
86 [ # # ]: 0 : CubitVector junk;
87 [ # # ]: 0 : root_->node_locations( min_, max_, junk );
88 : :
89 : : // create the oct-tree
90 [ # # ]: 0 : split_node(root_, min_, max_ );
91 : 0 : }
92 : :
93 : : //-------------------------------------------------------------------------
94 : : // Purpose : Destructor
95 : : //
96 : : // Special Notes :
97 : : //
98 : : // Creator : Jason Kraftcheck
99 : : //
100 : : // Creation Date : 01/30/02
101 : : //-------------------------------------------------------------------------
102 : : template<class X, class E>
103 : 0 : OctTree<X,E>::~OctTree()
104 : : {
105 : : // Release all memory
106 : : // Note that the oct-tree is not deleted except for the root node.
107 : : // Only the root node was allocated directly from the heap. All
108 : : // other nodes were allocated from our internal memory bool by
109 : : // the allocate_8() method. We just release the whole pool.
110 : 0 : delete root_;
111 [ # # ]: 0 : delete [] node_memory_;
112 [ # # ][ # # ]: 0 : while( mem_pages_.size() )
113 [ # # ][ # # ]: 0 : delete [] mem_pages_.pop();
114 : :
115 : : // Reinitialize to catch stale pointer to this object.
116 : :
117 : 0 : node_memory_ = 0;
118 : 0 : root_ = 0;
119 [ # # ]: 0 : }
120 : :
121 : : //-------------------------------------------------------------------------
122 : : // Purpose : Find all the nodes within tolerance of the passed position.
123 : : //
124 : : // Special Notes :
125 : : //
126 : : // Creator : Jason Kraftcheck
127 : : //
128 : : // Creation Date : 01/30/02
129 : : //-------------------------------------------------------------------------
130 : : template<class X, class E>
131 : 0 : void OctTree<X,E>::nodes_near( const CubitVector& position,
132 : : DLIList<X*>& result_list )
133 : : {
134 [ # # # # : 0 : if( position.x() - min_.x() < -tolerance_ ||
# # # # #
# # # ]
[ # # ]
135 : 0 : position.y() - min_.y() < -tolerance_ ||
136 : 0 : position.z() - min_.z() < -tolerance_ ||
137 : 0 : position.x() - max_.x() > tolerance_ ||
138 : 0 : position.y() - max_.y() > tolerance_ ||
139 : 0 : position.z() - max_.z() > tolerance_ )
140 : 0 : return;
141 : :
142 : : // Initialize search stack
143 [ # # ]: 0 : if( root_->leaf() )
144 : : {
145 : : // Done searching already -- that was easy
146 : 0 : root_->append_nodes( search_results );
147 : 0 : box_count = 0;
148 : : }
149 : : else
150 : : {
151 : 0 : search_results.clean_out();
152 : :
153 : : // Start with root node on stack of nodes to search
154 : 0 : box_count = 1;
155 : 0 : box_list[0] = root_;
156 : :
157 : 0 : box_min[0] = min_;
158 : 0 : box_max[0] = max_;
159 : : }
160 : :
161 : : // While there are still boxes on the search stack
162 [ # # ]: 0 : while( box_count )
163 : : {
164 : : // Pop the 'current' box from the stack
165 [ # # ]: 0 : pop_to_current();
166 : :
167 : : // Push appropriate child box(es) to stack
168 [ # # ]: 0 : CubitVector diff = position - center;
169 [ # # ][ # # ]: 0 : if( diff.x() <= tolerance_ )
170 : : {
171 [ # # ][ # # ]: 0 : if( diff.y() <= tolerance_ )
172 : : {
173 [ # # ][ # # ]: 0 : if( diff.z() <= tolerance_ )
174 : : {
175 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MIN |
176 : : OctTreeCell<X,E>::Y_MIN |
177 : : OctTreeCell<X,E>::Z_MIN );
178 : : }
179 [ # # ][ # # ]: 0 : if( diff.z() >= -tolerance_ )
180 : : {
181 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MIN |
182 : : OctTreeCell<X,E>::Y_MIN |
183 : : OctTreeCell<X,E>::Z_MAX );
184 : : }
185 : : }
186 [ # # ][ # # ]: 0 : if( diff.y() >= -tolerance_ )
187 : : {
188 [ # # ][ # # ]: 0 : if( diff.z() <= tolerance_ )
189 : : {
190 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MIN |
191 : : OctTreeCell<X,E>::Y_MAX |
192 : : OctTreeCell<X,E>::Z_MIN );
193 : : }
194 [ # # ][ # # ]: 0 : if( diff.z() >= -tolerance_ )
195 : : {
196 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MIN |
197 : : OctTreeCell<X,E>::Y_MAX |
198 : : OctTreeCell<X,E>::Z_MAX );
199 : : }
200 : : }
201 : : }
202 [ # # ][ # # ]: 0 : if( diff.x() >= -tolerance_ )
203 : : {
204 [ # # ][ # # ]: 0 : if( diff.y() <= tolerance_ )
205 : : {
206 [ # # ][ # # ]: 0 : if( diff.z() <= tolerance_ )
207 : : {
208 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MAX |
209 : : OctTreeCell<X,E>::Y_MIN |
210 : : OctTreeCell<X,E>::Z_MIN );
211 : : }
212 [ # # ][ # # ]: 0 : if( diff.z() >= -tolerance_ )
213 : : {
214 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MAX |
215 : : OctTreeCell<X,E>::Y_MIN |
216 : : OctTreeCell<X,E>::Z_MAX );
217 : : }
218 : : }
219 [ # # ][ # # ]: 0 : if( diff.y() >= -tolerance_ )
220 : : {
221 [ # # ][ # # ]: 0 : if( diff.z() <= tolerance_ )
222 : : {
223 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MAX |
224 : : OctTreeCell<X,E>::Y_MAX |
225 : : OctTreeCell<X,E>::Z_MIN );
226 : : }
227 [ # # ][ # # ]: 0 : if( diff.z() >= -tolerance_ )
228 : : {
229 [ # # ]: 0 : push_search_box( OctTreeCell<X,E>::X_MAX |
230 : : OctTreeCell<X,E>::Y_MAX |
231 : : OctTreeCell<X,E>::Z_MAX );
232 : : }
233 : : }
234 : : }
235 : : }
236 : :
237 : : // append to result list all nodes within tolerance
238 : : // of passed position
239 : 0 : search_results.reset();
240 : 0 : const double tol_sqr = tolerance_ * tolerance_;
241 [ # # ]: 0 : for( int i = search_results.size(); i--; )
242 : : {
243 [ # # ]: 0 : X* node = search_results.get_and_step();
244 [ # # ]: 0 : CubitVector coords = E::coordinates(node);
245 [ # # ][ # # ]: 0 : double x = position.x() - coords.x();
246 [ # # ][ # # ]: 0 : double y = position.y() - coords.y();
247 [ # # ][ # # ]: 0 : double z = position.z() - coords.z();
248 : :
249 : 0 : double dist_sqr = x * x + y * y + z * z;
250 [ # # ]: 0 : if( dist_sqr <= tol_sqr )
251 [ # # ]: 0 : result_list.append( node );
252 : : }
253 : : }
254 : :
255 : :
256 : : //-------------------------------------------------------------------------
257 : : // Purpose : Push child boxes onto search stack (or leaf node list)
258 : : //
259 : : // Special Notes :
260 : : //
261 : : // Creator : Jason Kraftcheck
262 : : //
263 : : // Creation Date : 01/30/02
264 : : //-------------------------------------------------------------------------
265 : : template<class X, class E>
266 : 0 : void OctTree<X,E>::push_search_box( int quadrant_flags )
267 : : {
268 : 0 : OctTreeCell<X,E>* box = current_box->child( quadrant_flags );
269 [ # # ]: 0 : if( box->leaf() )
270 : : {
271 : : // If a leaf node, get its nodes
272 : 0 : box->append_nodes( search_results );
273 : : }
274 : : else
275 : : {
276 [ # # ]: 0 : assert( box_count < 64 );
277 : :
278 : : // Get appropriate min and max corners of child box
279 : :
280 : 0 : CubitVector& oldmin = current_min;
281 : 0 : CubitVector& oldmax = current_max;
282 : 0 : CubitVector& newmin = box_min[box_count];
283 : 0 : CubitVector& newmax = box_max[box_count];
284 : :
285 [ # # ]: 0 : if( quadrant_flags & OctTreeCell<X,E>::X_MAX )
286 : : {
287 : 0 : newmin.x( center.x() );
288 : 0 : newmax.x( oldmax.x() );
289 : : }
290 : : else
291 : : {
292 : 0 : newmin.x( oldmin.x() );
293 : 0 : newmax.x( center.x() );
294 : : }
295 : :
296 [ # # ]: 0 : if( quadrant_flags & OctTreeCell<X,E>::Y_MAX )
297 : : {
298 : 0 : newmin.y( center.y() );
299 : 0 : newmax.y( oldmax.y() );
300 : : }
301 : : else
302 : : {
303 : 0 : newmin.y( oldmin.y() );
304 : 0 : newmax.y( center.y() );
305 : : }
306 : :
307 [ # # ]: 0 : if( quadrant_flags & OctTreeCell<X,E>::Z_MAX )
308 : : {
309 : 0 : newmin.z( center.z() );
310 : 0 : newmax.z( oldmax.z() );
311 : : }
312 : : else
313 : : {
314 : 0 : newmin.z( oldmin.z() );
315 : 0 : newmax.z( center.z() );
316 : : }
317 : :
318 : : // Put the box on the stack
319 : 0 : box_list[box_count++] = box;
320 : : }
321 : 0 : }
322 : :
323 : : //-------------------------------------------------------------------------
324 : : // Purpose : Pop box/tree-node from stack and store in current_box
325 : : //
326 : : // Special Notes :
327 : : //
328 : : // Creator : Jason Kraftcheck
329 : : //
330 : : // Creation Date : 01/30/02
331 : : //-------------------------------------------------------------------------
332 : : template<class X, class E>
333 : 0 : void OctTree<X,E>::pop_to_current()
334 : : {
335 [ # # ]: 0 : assert( box_count );
336 : 0 : box_count--;
337 : 0 : current_min = box_min[box_count];
338 : 0 : current_max = box_max[box_count];
339 : 0 : current_box = box_list[box_count];
340 [ # # ][ # # ]: 0 : center = (current_min + current_max) * 0.5;
341 : 0 : }
342 : :
343 : : //-------------------------------------------------------------------------
344 : : // Purpose : Allocate block of 8 oct-tree nodes for use as children
345 : : // of some (currently-leaf) node.
346 : : //
347 : : // Special Notes :
348 : : //
349 : : // Creator : Jason Kraftcheck
350 : : //
351 : : // Creation Date : 01/30/02
352 : : //-------------------------------------------------------------------------
353 : : template<class X, class E>
354 : 0 : OctTreeCell<X,E>* OctTree<X,E>::allocate_8()
355 : : {
356 : : // Want to pop from end of page
357 : 0 : curr_page_end_ -= 8;
358 : :
359 : : // Any blocks available in current page?
360 : 0 : mem_pages_.last();
361 [ # # ]: 0 : if( curr_page_end_ < mem_pages_.get() )
362 : : {
363 : : // Allocate new page
364 [ # # ][ # # ]: 0 : mem_pages_.append( new OctTreeCell<X,E>[OCT_TREE_ALLOC_COUNT] );
[ # # ]
365 : :
366 : : // Pop last block from new page
367 : 0 : mem_pages_.last();
368 : 0 : curr_page_end_ = mem_pages_.get() + OCT_TREE_ALLOC_COUNT - 8;
369 : : }
370 : :
371 : 0 : return curr_page_end_;
372 : : }
373 : :
374 : :
375 : : //-------------------------------------------------------------------------
376 : : // Purpose : Recursively subdivide oct-tree nodes until
377 : : // one of three stop conditions are met:
378 : : // - min_nodes_ or less nodes in the box
379 : : // - size of child boxes will be smaller than
380 : : // min_box_size_ in all three dimensions
381 : : // - the nodes within this box are all within
382 : : // 2*tolerance_ of each other.
383 : : //
384 : : // Special Notes :
385 : : //
386 : : // Creator : Jason Kraftcheck
387 : : //
388 : : // Creation Date : 01/30/02
389 : : //-------------------------------------------------------------------------
390 : : template<class X, class E>
391 : 0 : void OctTree<X,E>::split_node( OctTreeCell<X,E>* box,
392 : : const CubitVector& min,
393 : : const CubitVector& max )
394 : : {
395 [ # # ][ # # ]: 0 : assert( box->leaf() );
396 [ # # ]: 0 : CubitVector diag = max - min;
397 [ # # ]: 0 : diag *= 0.5; // diagonal size of child boxes
398 [ # # ][ # # ]: 0 : if( (box->node_count() <= min_nodes_) || // must have more than min_nodes_
[ # # ][ # # ]
[ # # ][ # # ]
399 [ # # ]: 0 : (diag.x() < min_box_size_ && // child boxes will be smaller
400 [ # # ]: 0 : diag.y() < min_box_size_ && // than min_box_size_ in all
401 [ # # ]: 0 : diag.z() < min_box_size_ ) ) // three dimensions.
402 : 0 : return;
403 : :
404 : : // Check if all nodes are currently within 2*tolerance_
405 : : // of each other.
406 : 0 : double tol2 = 2 * tolerance_;
407 [ # # ][ # # ]: 0 : CubitVector node_min, node_max, junk;
[ # # ]
408 [ # # ]: 0 : box->node_locations( node_min, node_max, junk );
409 [ # # ][ # # ]: 0 : diag = node_max - node_min;
410 [ # # ][ # # ]: 0 : if( diag.x() < tol2 &&
[ # # ][ # # ]
[ # # ]
411 [ # # ]: 0 : diag.y() < tol2 &&
412 [ # # ]: 0 : diag.z() < tol2 )
413 : 0 : return;
414 : :
415 : : // Split the box
416 [ # # ][ # # ]: 0 : CubitVector mid = (min + max) * 0.5;
417 [ # # ][ # # ]: 0 : if( !box->split( mid, allocate_8() ) )
[ # # ]
418 : 0 : return;
419 : :
420 : : // Recursively call split_node on all 8 new child boxes
421 [ # # ]: 0 : split_node(
422 : : box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MIN),
423 : : CubitVector( min.x(), min.y(), min.z() ),
424 [ # # ][ # # ]: 0 : CubitVector( mid.x(), mid.y(), mid.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
425 [ # # ]: 0 : split_node(
426 : : box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MAX),
427 [ # # ]: 0 : CubitVector( min.x(), min.y(), mid.z() ),
428 [ # # ][ # # ]: 0 : CubitVector( mid.x(), mid.y(), max.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
429 [ # # ]: 0 : split_node(
430 : : box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MIN),
431 [ # # ]: 0 : CubitVector( min.x(), mid.y(), min.z() ),
432 [ # # ][ # # ]: 0 : CubitVector( mid.x(), max.y(), mid.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
433 [ # # ]: 0 : split_node(
434 : : box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MAX),
435 [ # # ][ # # ]: 0 : CubitVector( min.x(), mid.y(), mid.z() ),
436 [ # # ][ # # ]: 0 : CubitVector( mid.x(), max.y(), max.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
437 [ # # ]: 0 : split_node(
438 : : box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MIN),
439 [ # # ]: 0 : CubitVector( mid.x(), min.y(), min.z() ),
440 [ # # ][ # # ]: 0 : CubitVector( max.x(), mid.y(), mid.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ][ # # ]
441 [ # # ]: 0 : split_node(
442 : : box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MAX),
443 [ # # ][ # # ]: 0 : CubitVector( mid.x(), min.y(), mid.z() ),
444 [ # # ][ # # ]: 0 : CubitVector( max.x(), mid.y(), max.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
445 [ # # ]: 0 : split_node(
446 : : box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MIN),
447 [ # # ][ # # ]: 0 : CubitVector( mid.x(), mid.y(), min.z() ),
448 [ # # ][ # # ]: 0 : CubitVector( max.x(), max.y(), mid.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
[ # # ]
449 [ # # ]: 0 : split_node(
450 : : box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MAX),
451 [ # # ][ # # ]: 0 : CubitVector( mid.x(), mid.y(), mid.z() ),
[ # # ]
452 [ # # ][ # # ]: 0 : CubitVector( max.x(), max.y(), max.z() ) );
[ # # ][ # # ]
[ # # ][ # # ]
453 : : }
454 : :
455 : :
456 : : template<class X, class E>
457 : : void OctTree<X,E>::tree_size( DLIList<int>& boxes, DLIList<int>& leaves )
458 : : {
459 : : boxes.clean_out();
460 : : boxes.reset();
461 : :
462 : : leaves.clean_out();
463 : : leaves.reset();
464 : :
465 : : tree_size( root_, boxes, leaves );
466 : : }
467 : :
468 : : template<class X, class E>
469 : : void OctTree<X,E>::tree_size( OctTreeCell<X,E>* box, DLIList<int>& boxes, DLIList<int>& leaves )
470 : : {
471 : : if( boxes.is_at_end() )
472 : : {
473 : : boxes.append(1);
474 : : boxes.step();
475 : : }
476 : : else
477 : : {
478 : : boxes.step();
479 : : boxes.change_to( boxes.get() + 1 );
480 : : }
481 : :
482 : : if( box->leaf() )
483 : : {
484 : : if( leaves.is_at_end() )
485 : : {
486 : : leaves.append(1);
487 : : leaves.step();
488 : : }
489 : : else
490 : : {
491 : : leaves.step();
492 : : leaves.change_to( leaves.get() + 1 );
493 : : }
494 : : }
495 : : else
496 : : {
497 : : if( leaves.is_at_end() )
498 : : leaves.append(0);
499 : : leaves.step();
500 : :
501 : : for( int i = 0; i < 8; i++ )
502 : : tree_size( box->child(i), boxes, leaves );
503 : : }
504 : :
505 : : boxes.back();
506 : : leaves.back();
507 : : }
508 : :
|
