1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
//-------------------------------------------------------------------------<--- Skipping configuration 'ACIS' since the value of 'ACIS' is unknown. Use -D if you want to check it. You can use -U to skip it explicitly.<--- Skipping configuration 'DBL_MAX' since the value of 'DBL_MAX' is unknown. Use -D if you want to check it. You can use -U to skip it explicitly.<--- Skipping configuration 'DBL_MIN' since the value of 'DBL_MIN' is unknown. Use -D if you want to check it. You can use -U to skip it explicitly.<--- Skipping configuration 'M_PI' since the value of 'M_PI' is unknown. Use -D if you want to check it. You can use -U to skip it explicitly.
// Filename      : OctTree.cpp
//
// Purpose       : Class for O(ln n) search for nodes within a specified
//                 tolerance of a specified position.
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------

#include "OctTree.hpp"
#include "OctTreeCell.hpp"
#include "DLIList.hpp"
#include "CubitDefines.h"
#include "GeometryDefines.h"

const int DEFAULT_MIN_NODES_PER_BOX = 6;
  // The default value to use for the minimum number of
  // nodes in a box, if none is specified in the constructr.

const int OCT_TREE_BOX_PAGE_SIZE = 8192;
  // The size of memory blocks to allocate for holding
  // oct-tree nodes.

const int OCT_TREE_CHUNK_SIZE = 8 * sizeof(OctTreeCell<int,int>);
  // Do not change.  Size of a child array for a oct-tree node.

const int OCT_TREE_CHUNK_PER_PAGE = OCT_TREE_BOX_PAGE_SIZE / 
                                       OCT_TREE_CHUNK_SIZE;
  // Do not change.  Number of arrays of 8 oct-tree nodes 
  // (child arrays) fit in a page.
                                       
const int OCT_TREE_ALLOC_COUNT = OCT_TREE_CHUNK_PER_PAGE * 8;
  // When allocating the page as one big array of oct-tree nodes,
  // the size of the array to request.  Will be sligthtly smaller
  // than the requested page because page size probably is not a
  // multiple of OCT_TREE_CHUNK_SIZE.  That's a good thing though,
  // because then the std-c and c++ memory allocators have a little
  // extra space for internal data.

//-------------------------------------------------------------------------
// Purpose       : Constructor
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
OctTree<X,E>::OctTree( DLIList<X*>& nodes, 
                        double tolerance,
                        int min_nodes_per_box,
                        double min_box_dimension )
{
    // Check and store constructor arguments
  tolerance_ = tolerance < GEOMETRY_RESABS ? GEOMETRY_RESABS : tolerance;
  min_nodes_ = min_nodes_per_box < 0 ? DEFAULT_MIN_NODES_PER_BOX : min_nodes_per_box;
  double tol3 = 3 * tolerance_;
  min_box_size_ = min_box_dimension < tol3 ? tol3 : min_box_dimension;
    // Note: min_box_size must be greater than twice the tolerance
    //       or the internal stack used for searching the tree will
    //       overflow (more than 8 boxes may be within the tolerance
    //       of a passed position).
  
    // set up data for memory pool of oct-tree nodes
  mem_pages_.append( new OctTreeCell<X,E>[OCT_TREE_ALLOC_COUNT] );
  curr_page_end_ = mem_pages_.get() + OCT_TREE_ALLOC_COUNT;

    // construct root node
  node_memory_ = new OctTreeEntry<X,E>[nodes.size()];
  nodes.reset();
  OctTreeEntry<X,E>* ptr = node_memory_;
  for( int i = nodes.size(); i--; )
  {
    ptr->node = nodes.get_and_step();
    ptr->next = 0;
    ptr++;
  }
  root_ = new OctTreeCell<X,E>( node_memory_, nodes.size() );
  
    // get bounding box of all nodes
  CubitVector junk;
  root_->node_locations( min_, max_, junk );
  
    // create the oct-tree
  split_node(root_, min_, max_ );
}

//-------------------------------------------------------------------------
// Purpose       : Destructor
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
OctTree<X,E>::~OctTree()
{
    // Release all memory
    // Note that the oct-tree is not deleted except for the root node.
    // Only the root node was allocated directly from the heap.  All
    // other nodes were allocated from our internal memory bool by
    // the allocate_8() method.  We just release the whole pool.
  delete root_;
  delete [] node_memory_;
  while( mem_pages_.size() )
    delete [] mem_pages_.pop();
    
    // Reinitialize to catch stale pointer to this object.
  
  node_memory_ = 0;
  root_ = 0;
}

//-------------------------------------------------------------------------
// Purpose       : Find all the nodes within tolerance of the passed position.
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
void OctTree<X,E>::nodes_near( const CubitVector& position, 
                             DLIList<X*>& result_list )
{
  if( position.x() - min_.x() < -tolerance_ ||
      position.y() - min_.y() < -tolerance_ ||
      position.z() - min_.z() < -tolerance_ ||
      position.x() - max_.x() >  tolerance_ ||
      position.y() - max_.y() >  tolerance_ ||
      position.z() - max_.z() >  tolerance_  )
    return;
  
    // Initialize search stack
  if( root_->leaf() )
  {
      // Done searching already -- that was easy
    root_->append_nodes( search_results );
    box_count = 0;
  }
  else
  {
    search_results.clean_out();

      // Start with root node on stack of nodes to search
    box_count = 1;
    box_list[0] = root_;

    box_min[0] = min_;
    box_max[0] = max_;
  }
  
    // While there are still boxes on the search stack
  while( box_count )
  {
      // Pop the 'current' box from the stack
    pop_to_current();
    
      // Push appropriate child box(es) to stack
    CubitVector diff = position - center;
    if( diff.x() <= tolerance_ )
    {
      if( diff.y() <= tolerance_ )
      {
        if( diff.z() <= tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MIN |
                           OctTreeCell<X,E>::Y_MIN |
                           OctTreeCell<X,E>::Z_MIN );
        }
        if( diff.z() >= -tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MIN |
                           OctTreeCell<X,E>::Y_MIN |
                           OctTreeCell<X,E>::Z_MAX );
        }
      }
      if( diff.y() >= -tolerance_ )
      {
        if( diff.z() <= tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MIN |
                           OctTreeCell<X,E>::Y_MAX |
                           OctTreeCell<X,E>::Z_MIN );
        }
        if( diff.z() >= -tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MIN |
                           OctTreeCell<X,E>::Y_MAX |
                           OctTreeCell<X,E>::Z_MAX );
        }
      }
    }      
    if( diff.x() >= -tolerance_ )
    {
      if( diff.y() <= tolerance_ )
      {
        if( diff.z() <= tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MAX |
                           OctTreeCell<X,E>::Y_MIN |
                           OctTreeCell<X,E>::Z_MIN );
        }
        if( diff.z() >= -tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MAX |
                           OctTreeCell<X,E>::Y_MIN |
                           OctTreeCell<X,E>::Z_MAX );
        }
      }
      if( diff.y() >= -tolerance_ )
      {
        if( diff.z() <= tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MAX |
                           OctTreeCell<X,E>::Y_MAX |
                           OctTreeCell<X,E>::Z_MIN );
        }
        if( diff.z() >= -tolerance_ )
        {
          push_search_box( OctTreeCell<X,E>::X_MAX |
                           OctTreeCell<X,E>::Y_MAX |
                           OctTreeCell<X,E>::Z_MAX );
        }
      }
    }      
  }
  
    // append to result list all nodes within tolerance
    // of passed position
  search_results.reset();
  const double tol_sqr = tolerance_ * tolerance_;
  for( int i = search_results.size(); i--; )
  {
    X* node = search_results.get_and_step();
    CubitVector coords = E::coordinates(node);
    double x = position.x() - coords.x();
    double y = position.y() - coords.y();
    double z = position.z() - coords.z();
    
    double dist_sqr = x * x + y * y + z * z;
    if( dist_sqr <= tol_sqr )
      result_list.append( node );
  }
}


//-------------------------------------------------------------------------
// Purpose       : Push child boxes onto search stack (or leaf node list)
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
void OctTree<X,E>::push_search_box( int quadrant_flags )
{
  OctTreeCell<X,E>* box = current_box->child( quadrant_flags );
  if( box->leaf() )
  {
      // If a leaf node, get its nodes
    box->append_nodes( search_results );
  }
  else
  {
    assert( box_count < 64 );

      // Get appropriate min and max corners of child box
      
    CubitVector& oldmin = current_min;
    CubitVector& oldmax = current_max;
    CubitVector& newmin = box_min[box_count];
    CubitVector& newmax = box_max[box_count];
    
    if( quadrant_flags & OctTreeCell<X,E>::X_MAX )
    {
      newmin.x( center.x() );
      newmax.x( oldmax.x() );
    }
    else
    {
      newmin.x( oldmin.x() );
      newmax.x( center.x() );
    }
    
    if( quadrant_flags & OctTreeCell<X,E>::Y_MAX )
    {
      newmin.y( center.y() );
      newmax.y( oldmax.y() );
    }
    else
    {
      newmin.y( oldmin.y() );
      newmax.y( center.y() );
    }
    
    if( quadrant_flags & OctTreeCell<X,E>::Z_MAX )
    {
      newmin.z( center.z() );
      newmax.z( oldmax.z() );
    }
    else
    {
      newmin.z( oldmin.z() );
      newmax.z( center.z() );
    }
    
      // Put the box on the stack
    box_list[box_count++] = box;
  }
}

//-------------------------------------------------------------------------
// Purpose       : Pop box/tree-node from stack and store in current_box
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
void OctTree<X,E>::pop_to_current()
{
  assert( box_count );
  box_count--;
  current_min = box_min[box_count];
  current_max = box_max[box_count];
  current_box = box_list[box_count];
  center = (current_min + current_max) * 0.5;
}

//-------------------------------------------------------------------------
// Purpose       : Allocate block of 8 oct-tree nodes for use as children
//                 of some (currently-leaf) node.
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
OctTreeCell<X,E>* OctTree<X,E>::allocate_8()
{
    // Want to pop from end of page
  curr_page_end_ -= 8;
  
    // Any blocks available in current page?
  mem_pages_.last();
  if( curr_page_end_ < mem_pages_.get()  )
  {
      // Allocate new page
    mem_pages_.append( new OctTreeCell<X,E>[OCT_TREE_ALLOC_COUNT] );
    
      // Pop last block from new page
    mem_pages_.last();
    curr_page_end_ = mem_pages_.get() + OCT_TREE_ALLOC_COUNT - 8;
  }
  
  return curr_page_end_;
}


//-------------------------------------------------------------------------
// Purpose       : Recursively subdivide oct-tree nodes until
//                 one of three stop conditions are met:
//                   - min_nodes_ or less nodes in the box
//                   - size of child boxes will be smaller than
//                     min_box_size_ in all three dimensions
//                   - the nodes within this box are all within
//                     2*tolerance_ of each other.
//
// Special Notes : 
//
// Creator       : Jason Kraftcheck
//
// Creation Date : 01/30/02
//-------------------------------------------------------------------------
template<class X, class E>
void OctTree<X,E>::split_node( OctTreeCell<X,E>* box, 
                             const CubitVector& min,
                             const CubitVector& max )
{
  assert( box->leaf() );
  CubitVector diag = max - min;
  diag *= 0.5; // diagonal size of child boxes
  if( (box->node_count() <= min_nodes_) ||  // must have more than min_nodes_
      (diag.x() < min_box_size_ &&          // child boxes will be smaller
       diag.y() < min_box_size_ &&          //   than min_box_size_ in all
       diag.z() < min_box_size_ ) )         //   three dimensions.
    return;
  
    // Check if all nodes are currently within 2*tolerance_
    // of each other.
  double tol2 = 2 * tolerance_;
  CubitVector node_min, node_max, junk;
  box->node_locations( node_min, node_max, junk );
  diag = node_max - node_min;
  if( diag.x() < tol2 &&
      diag.y() < tol2 &&
      diag.z() < tol2 )
    return;
  
    // Split the box
  CubitVector mid = (min + max) * 0.5;
  if( !box->split( mid, allocate_8() ) )
    return;
  
    // Recursively call split_node on all 8 new child boxes
  split_node( 
    box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MIN),
    CubitVector( min.x(), min.y(), min.z() ),
    CubitVector( mid.x(), mid.y(), mid.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MAX),
    CubitVector( min.x(), min.y(), mid.z() ),
    CubitVector( mid.x(), mid.y(), max.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MIN),
    CubitVector( min.x(), mid.y(), min.z() ),
    CubitVector( mid.x(), max.y(), mid.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MIN|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MAX),
    CubitVector( min.x(), mid.y(), mid.z() ),
    CubitVector( mid.x(), max.y(), max.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MIN),
    CubitVector( mid.x(), min.y(), min.z() ),
    CubitVector( max.x(), mid.y(), mid.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MIN|OctTreeCell<X,E>::Z_MAX),
    CubitVector( mid.x(), min.y(), mid.z() ),
    CubitVector( max.x(), mid.y(), max.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MIN),
    CubitVector( mid.x(), mid.y(), min.z() ),
    CubitVector( max.x(), max.y(), mid.z() ) );
  split_node( 
    box->child(OctTreeCell<X,E>::X_MAX|OctTreeCell<X,E>::Y_MAX|OctTreeCell<X,E>::Z_MAX),
    CubitVector( mid.x(), mid.y(), mid.z() ),
    CubitVector( max.x(), max.y(), max.z() ) );
}
  
  
template<class X, class E>
void OctTree<X,E>::tree_size( DLIList<int>& boxes, DLIList<int>& leaves )
{
  boxes.clean_out();
  boxes.reset();
  
  leaves.clean_out();
  leaves.reset();
  
  tree_size( root_, boxes, leaves );
}

template<class X, class E>
void OctTree<X,E>::tree_size( OctTreeCell<X,E>* box, DLIList<int>& boxes, DLIList<int>& leaves )
{
  if( boxes.is_at_end() )
  {
    boxes.append(1);
    boxes.step();
  }
  else
  {
    boxes.step();
    boxes.change_to( boxes.get() + 1 );
  }
  
  if( box->leaf() )
  {
    if( leaves.is_at_end() )
    {
      leaves.append(1);
      leaves.step();
    }
    else
    {
      leaves.step();
      leaves.change_to( leaves.get() + 1 );
    }
  }
  else
  {
    if( leaves.is_at_end() )
      leaves.append(0);
    leaves.step();

    for( int i = 0; i < 8; i++ )
      tree_size( box->child(i), boxes, leaves );
  }
  
  boxes.back();
  leaves.back();
}