Mesh Oriented datABase  (version 5.5.1)
An array-based unstructured mesh library
BVHTree.cpp
Go to the documentation of this file.
1 #include "moab/BVHTree.hpp"
2 #include "moab/Interface.hpp"
3 #include "moab/ElemEvaluator.hpp"
4 #include "moab/ReadUtilIface.hpp"
5 #include "moab/CpuTimer.hpp"
6 
7 namespace moab
8 {
9 const char* BVHTree::treeName = "BVHTree";
10 
12 {
13  ErrorCode rval;
14  CpuTimer cp;
15 
16  if( options )
17  {
18  rval = parse_options( *options );
19  if( MB_SUCCESS != rval ) return rval;
20 
21  if( !options->all_seen() ) return MB_FAILURE;
22  }
23 
24  // calculate bounding box of elements
25  BoundBox box;
26  rval = box.update( *moab(), entities );
27  if( MB_SUCCESS != rval ) return rval;
28 
29  // create tree root
30  EntityHandle tmp_root;
31  if( !tree_root_set ) tree_root_set = &tmp_root;
32  rval = create_root( box.bMin.array(), box.bMax.array(), *tree_root_set );
33  if( MB_SUCCESS != rval ) return rval;
34  rval = mbImpl->add_entities( *tree_root_set, entities );
35  if( MB_SUCCESS != rval ) return rval;
36 
37  // a fully balanced tree will have 2*_entities.size()
38  // which is one doubling away..
39  std::vector< Node > tree_nodes;
40  tree_nodes.reserve( entities.size() / maxPerLeaf );
41  std::vector< HandleData > handle_data_vec;
42  rval = construct_element_vec( handle_data_vec, entities, boundBox );
43  if( MB_SUCCESS != rval ) return rval;
44 
45 #ifndef NDEBUG
46  for( std::vector< HandleData >::const_iterator i = handle_data_vec.begin(); i != handle_data_vec.end(); ++i )
47  {
48  if( !boundBox.intersects_box( i->myBox, 0 ) )
49  {
50  std::cerr << "BB:" << boundBox << "EB:" << i->myBox << std::endl;
51  return MB_FAILURE;
52  }
53  }
54 #endif
55  // We only build nonempty trees
56  if( !handle_data_vec.empty() )
57  {
58  // initially all bits are set
59  tree_nodes.push_back( Node() );
60  const int depth = local_build_tree( tree_nodes, handle_data_vec.begin(), handle_data_vec.end(), 0, boundBox );
61 #ifndef NDEBUG
62  std::set< EntityHandle > entity_handles;
63  for( std::vector< Node >::iterator n = tree_nodes.begin(); n != tree_nodes.end(); ++n )
64  {
65  for( HandleDataVec::const_iterator j = n->entities.begin(); j != n->entities.end(); ++j )
66  {
67  entity_handles.insert( j->myHandle );
68  }
69  }
70  if( entity_handles.size() != entities.size() )
71  {
72  std::cout << "Entity Handle Size Mismatch!" << std::endl;
73  }
74  for( Range::iterator i = entities.begin(); i != entities.end(); ++i )
75  {
76  if( entity_handles.find( *i ) == entity_handles.end() )
77  std::cout << "Tree is missing an entity! " << std::endl;
78  }
79 #endif
80  treeDepth = std::max( depth, treeDepth );
81  }
82 
83  // convert vector of Node's to entity sets and vector of TreeNode's
84  rval = convert_tree( tree_nodes );
85  if( MB_SUCCESS != rval ) return rval;
86 
87  treeStats.reset();
90 
91  return rval;
92 }
93 
94 ErrorCode BVHTree::convert_tree( std::vector< Node >& tree_nodes )
95 {
96  // first construct the proper number of entity sets
97  ReadUtilIface* read_util;
98  ErrorCode rval = mbImpl->query_interface( read_util );
99  if( MB_SUCCESS != rval ) return rval;
100 
101  { // isolate potentially-large std::vector so it gets deleted earlier
102  std::vector< unsigned int > tmp_flags( tree_nodes.size(), meshsetFlags );
103  rval = read_util->create_entity_sets( tree_nodes.size(), &tmp_flags[0], 0, startSetHandle );
104  if( MB_SUCCESS != rval ) return rval;
105  rval = mbImpl->release_interface( read_util );
106  if( MB_SUCCESS != rval ) return rval;
107  }
108 
109  // populate the sets and the TreeNode vector
110  EntityHandle set_handle = startSetHandle;
111  std::vector< Node >::iterator it;
112  myTree.reserve( tree_nodes.size() );
113  for( it = tree_nodes.begin(); it != tree_nodes.end(); ++it, set_handle++ )
114  {
115  if( it != tree_nodes.begin() && !it->entities.empty() )
116  {
117  Range range;
118  for( HandleDataVec::iterator hit = it->entities.begin(); hit != it->entities.end(); ++hit )
119  range.insert( hit->myHandle );
120  rval = mbImpl->add_entities( set_handle, range );
121  if( MB_SUCCESS != rval ) return rval;
122  }
123  myTree.push_back( TreeNode( it->dim, it->child, it->Lmax, it->Rmin, it->box ) );
124 
125  if( it->dim != 3 )
126  {
127  rval = mbImpl->add_child_meshset( set_handle, startSetHandle + it->child );
128  if( MB_SUCCESS != rval ) return rval;
129  rval = mbImpl->add_child_meshset( set_handle, startSetHandle + it->child + 1 );
130  if( MB_SUCCESS != rval ) return rval;
131  }
132  }
133 
134  return MB_SUCCESS;
135 }
136 
138 {
139  ErrorCode rval = parse_common_options( opts );
140  if( MB_SUCCESS != rval ) return rval;
141 
142  // SPLITS_PER_DIR: number of candidate splits considered per direction; default = 3
143  int tmp_int;
144  rval = opts.get_int_option( "SPLITS_PER_DIR", tmp_int );
145  if( MB_SUCCESS == rval ) splitsPerDir = tmp_int;
146 
147  return MB_SUCCESS;
148 }
149 
150 void BVHTree::establish_buckets( HandleDataVec::const_iterator begin,
151  HandleDataVec::const_iterator end,
152  const BoundBox& interval,
153  std::vector< std::vector< Bucket > >& buckets ) const
154 {
155  // put each element into its bucket
156  for( HandleDataVec::const_iterator i = begin; i != end; ++i )
157  {
158  const BoundBox& box = i->myBox;
159  for( unsigned int dim = 0; dim < 3; ++dim )
160  {
161  const unsigned int index = Bucket::bucket_index( splitsPerDir, box, interval, dim );
162  assert( index < buckets[dim].size() );
163  Bucket& bucket = buckets[dim][index];
164  if( bucket.mySize > 0 )
165  bucket.boundingBox.update( box );
166  else
167  bucket.boundingBox = box;
168  bucket.mySize++;
169  }
170  }
171 
172 #ifndef NDEBUG
173  BoundBox elt_union = begin->myBox;
174  for( HandleDataVec::const_iterator i = begin; i != end; ++i )
175  {
176  const BoundBox& box = i->myBox;
177  elt_union.update( box );
178  for( unsigned int dim = 0; dim < 3; ++dim )
179  {
180  const unsigned int index = Bucket::bucket_index( splitsPerDir, box, interval, dim );
181  Bucket& bucket = buckets[dim][index];
182  if( !bucket.boundingBox.intersects_box( box ) ) std::cerr << "Buckets not covering elements!" << std::endl;
183  }
184  }
185  if( !elt_union.intersects_box( interval ) )
186  {
187  std::cout << "element union: " << std::endl << elt_union;
188  std::cout << "intervals: " << std::endl << interval;
189  std::cout << "union of elts does not contain original box!" << std::endl;
190  }
191  if( !interval.intersects_box( elt_union ) )
192  {
193  std::cout << "original box does not contain union of elts" << std::endl;
194  std::cout << interval << std::endl << elt_union << std::endl;
195  }
196  for( unsigned int d = 0; d < 3; ++d )
197  {
198  std::vector< unsigned int > nonempty;
199  const std::vector< Bucket >& buckets_ = buckets[d];
200  unsigned int j = 0;
201  for( std::vector< Bucket >::const_iterator i = buckets_.begin(); i != buckets_.end(); ++i, ++j )
202  {
203  if( i->mySize > 0 )
204  {
205  nonempty.push_back( j );
206  }
207  }
208  BoundBox test_box = buckets_[nonempty.front()].boundingBox;
209  for( unsigned int i = 0; i < nonempty.size(); ++i )
210  test_box.update( buckets_[nonempty[i]].boundingBox );
211 
212  if( !test_box.intersects_box( interval ) )
213  std::cout << "union of buckets in dimension: " << d << "does not contain original box!" << std::endl;
214  if( !interval.intersects_box( test_box ) )
215  {
216  std::cout << "original box does "
217  << "not contain union of buckets"
218  << "in dimension: " << d << std::endl;
219  std::cout << interval << std::endl << test_box << std::endl;
220  }
221  }
222 #endif
223 }
224 
225 void BVHTree::initialize_splits( std::vector< std::vector< SplitData > >& splits,
226  const std::vector< std::vector< Bucket > >& buckets,
227  const SplitData& data ) const
228 {
229  for( unsigned int d = 0; d < 3; ++d )
230  {
231  std::vector< SplitData >::iterator splits_begin = splits[d].begin();
232  std::vector< SplitData >::iterator splits_end = splits[d].end();
233  std::vector< Bucket >::const_iterator left_begin = buckets[d].begin();
234  std::vector< Bucket >::const_iterator _end = buckets[d].end();
235  std::vector< Bucket >::const_iterator left_end = buckets[d].begin() + 1;
236  for( std::vector< SplitData >::iterator s = splits_begin; s != splits_end; ++s, ++left_end )
237  {
238  s->nl = set_interval( s->leftBox, left_begin, left_end );
239  if( s->nl == 0 )
240  {
241  s->leftBox = data.boundingBox;
242  s->leftBox.bMax[d] = s->leftBox.bMin[d];
243  }
244  s->nr = set_interval( s->rightBox, left_end, _end );
245  if( s->nr == 0 )
246  {
247  s->rightBox = data.boundingBox;
248  s->rightBox.bMin[d] = s->rightBox.bMax[d];
249  }
250  s->Lmax = s->leftBox.bMax[d];
251  s->Rmin = s->rightBox.bMin[d];
252  s->split = std::distance( splits_begin, s );
253  s->dim = d;
254  }
255 #ifndef NDEBUG
256  for( std::vector< SplitData >::iterator s = splits_begin; s != splits_end; ++s )
257  {
258  BoundBox test_box = s->leftBox;
259  test_box.update( s->rightBox );
260  if( !data.boundingBox.intersects_box( test_box ) )
261  {
262  std::cout << "nr: " << s->nr << std::endl;
263  std::cout << "Test box: " << std::endl << test_box;
264  std::cout << "Left box: " << std::endl << s->leftBox;
265  std::cout << "Right box: " << std::endl << s->rightBox;
266  std::cout << "Interval: " << std::endl << data.boundingBox;
267  std::cout << "Split boxes larger than bb" << std::endl;
268  }
269  if( !test_box.intersects_box( data.boundingBox ) )
270  {
271  std::cout << "bb larger than union of split boxes" << std::endl;
272  }
273  }
274 #endif
275  }
276 }
277 
278 void BVHTree::median_order( HandleDataVec::iterator& begin, HandleDataVec::iterator& end, SplitData& data ) const
279 {
280  int dim = data.dim;
281  for( HandleDataVec::iterator i = begin; i != end; ++i )
282  {
283  i->myDim = 0.5 * ( i->myBox.bMin[dim], i->myBox.bMax[dim] );
284  }
285  std::sort( begin, end, BVHTree::HandleData_comparator() );
286  const unsigned int total = std::distance( begin, end );
287  HandleDataVec::iterator middle = begin + ( total / 2 );
288  double middle_center = middle->myDim;
289  middle_center += ( ++middle )->myDim;
290  middle_center *= 0.5;
291  data.split = middle_center;
292  data.nl = std::distance( begin, middle ) + 1;
293  data.nr = total - data.nl;
294  ++middle;
295  data.leftBox = begin->myBox;
296  data.rightBox = middle->myBox;
297  for( HandleDataVec::iterator i = begin; i != middle; ++i )
298  {
299  i->myDim = 0;
300  data.leftBox.update( i->myBox );
301  }
302  for( HandleDataVec::iterator i = middle; i != end; ++i )
303  {
304  i->myDim = 1;
305  data.rightBox.update( i->myBox );
306  }
307  data.Rmin = data.rightBox.bMin[data.dim];
308  data.Lmax = data.leftBox.bMax[data.dim];
309 #ifndef NDEBUG
310  BoundBox test_box( data.rightBox );
311  if( !data.boundingBox.intersects_box( test_box ) )
312  {
313  std::cerr << "MEDIAN: BB Does not contain splits" << std::endl;
314  std::cerr << "test_box: " << test_box << std::endl;
315  std::cerr << "data.boundingBox: " << data.boundingBox << std::endl;
316  }
317 #endif
318 }
319 
320 void BVHTree::find_split( HandleDataVec::iterator& begin, HandleDataVec::iterator& end, SplitData& data ) const
321 {
322  std::vector< std::vector< Bucket > > buckets( 3, std::vector< Bucket >( splitsPerDir + 1 ) );
323  std::vector< std::vector< SplitData > > splits( 3, std::vector< SplitData >( splitsPerDir, data ) );
324 
325  const BoundBox interval = data.boundingBox;
326  establish_buckets( begin, end, interval, buckets );
327  initialize_splits( splits, buckets, data );
328  choose_best_split( splits, data );
329  const bool use_median = ( 0 == data.nl ) || ( data.nr == 0 );
330  if( !use_median )
331  order_elements( begin, end, data );
332  else
333  median_order( begin, end, data );
334 
335 #ifndef NDEBUG
336  bool seen_one = false, issue = false;
337  bool first_left = true, first_right = true;
338  unsigned int count_left = 0, count_right = 0;
340  for( HandleDataVec::iterator i = begin; i != end; ++i )
341  {
342  int order = i->myDim;
343  if( order != 0 && order != 1 )
344  {
345  std::cerr << "Invalid order element !";
346  std::cerr << order << std::endl;
347  std::exit( -1 );
348  }
349  if( order == 1 )
350  {
351  seen_one = 1;
352  count_right++;
353  if( first_right )
354  {
355  right_box = i->myBox;
356  first_right = false;
357  }
358  else
359  {
360  right_box.update( i->myBox );
361  }
362  if( !right_box.intersects_box( i->myBox ) )
363  {
364  if( !issue )
365  {
366  std::cerr << "Bounding right box issue!" << std::endl;
367  }
368  issue = true;
369  }
370  }
371  if( order == 0 )
372  {
373  count_left++;
374  if( first_left )
375  {
376  left_box = i->myBox;
377  first_left = false;
378  }
379  else
380  {
381  left_box.update( i->myBox );
382  }
383  if( !data.leftBox.intersects_box( i->myBox ) )
384  {
385  if( !issue )
386  {
387  std::cerr << "Bounding left box issue!" << std::endl;
388  }
389  issue = true;
390  }
391  if( seen_one )
392  {
393  std::cerr << "Invalid ordering!" << std::endl;
394  std::cout << ( i - 1 )->myDim << order << std::endl;
395  exit( -1 );
396  }
397  }
398  }
399  if( !left_box.intersects_box( data.leftBox ) ) std::cout << "left elts do not contain left box" << std::endl;
400  if( !data.leftBox.intersects_box( left_box ) ) std::cout << "left box does not contain left elts" << std::endl;
401  if( !right_box.intersects_box( data.rightBox ) ) std::cout << "right elts do not contain right box" << std::endl;
402  if( !data.rightBox.intersects_box( right_box ) ) std::cout << "right box do not contain right elts" << std::endl;
403 
404  if( count_left != data.nl || count_right != data.nr )
405  {
406  std::cerr << "counts are off!" << std::endl;
407  std::cerr << "total: " << std::distance( begin, end ) << std::endl;
408  std::cerr << "Dim: " << data.dim << std::endl;
409  std::cerr << data.Lmax << " , " << data.Rmin << std::endl;
410  std::cerr << "Right box: " << std::endl << data.rightBox << "Left box: " << std::endl << data.leftBox;
411  std::cerr << "supposed to be: " << data.nl << " " << data.nr << std::endl;
412  std::cerr << "accountant says: " << count_left << " " << count_right << std::endl;
413  std::exit( -1 );
414  }
415 #endif
416 }
417 
418 int BVHTree::local_build_tree( std::vector< Node >& tree_nodes,
419  HandleDataVec::iterator begin,
420  HandleDataVec::iterator end,
421  const int index,
422  const BoundBox& box,
423  const int depth )
424 {
425 #ifndef NDEBUG
426  for( HandleDataVec::const_iterator i = begin; i != end; ++i )
427  {
428  if( !box.intersects_box( i->myBox, 0 ) )
429  {
430  std::cerr << "depth: " << depth << std::endl;
431  std::cerr << "BB:" << box << "EB:" << i->myBox << std::endl;
432  std::exit( -1 );
433  }
434  }
435 #endif
436 
437  const unsigned int total_num_elements = std::distance( begin, end );
438  tree_nodes[index].box = box;
439 
440  // logic for splitting conditions
441  if( (int)total_num_elements > maxPerLeaf && depth < maxDepth )
442  {
443  SplitData data;
444  data.boundingBox = box;
445  find_split( begin, end, data );
446  // assign data to node
447  tree_nodes[index].Lmax = data.Lmax;
448  tree_nodes[index].Rmin = data.Rmin;
449  tree_nodes[index].dim = data.dim;
450  tree_nodes[index].child = tree_nodes.size();
451  // insert left, right children;
452  tree_nodes.push_back( Node() );
453  tree_nodes.push_back( Node() );
454  const int left_depth =
455  local_build_tree( tree_nodes, begin, begin + data.nl, tree_nodes[index].child, data.leftBox, depth + 1 );
456  const int right_depth =
457  local_build_tree( tree_nodes, begin + data.nl, end, tree_nodes[index].child + 1, data.rightBox, depth + 1 );
458  return std::max( left_depth, right_depth );
459  }
460 
461  tree_nodes[index].dim = 3;
462  std::copy( begin, end, std::back_inserter( tree_nodes[index].entities ) );
463  return depth;
464 }
465 
466 ErrorCode BVHTree::find_point( const std::vector< double >& point,
467  const unsigned int& index,
468  const double iter_tol,
469  const double inside_tol,
470  std::pair< EntityHandle, CartVect >& result )
471 {
472  if( index == 0 ) treeStats.numTraversals++;
473  const TreeNode& node = myTree[index];
475  CartVect params;
476  int is_inside;
477  ErrorCode rval = MB_SUCCESS;
478  if( node.dim == 3 )
479  {
481  Range entities;
483  if( MB_SUCCESS != rval ) return rval;
484 
485  for( Range::iterator i = entities.begin(); i != entities.end(); ++i )
486  {
488  myEval->set_ent_handle( *i );
489  myEval->reverse_eval( &point[0], iter_tol, inside_tol, params.array(), &is_inside );
490  if( is_inside )
491  {
492  result.first = *i;
493  result.second = params;
494  return MB_SUCCESS;
495  }
496  }
497  result.first = 0;
498  return MB_SUCCESS;
499  }
500  // the extra tol here considers the case where
501  // 0 < Rmin - Lmax < 2tol
502  std::vector< EntityHandle > children;
503  rval = mbImpl->get_child_meshsets( startSetHandle + index, children );
504  if( MB_SUCCESS != rval || children.size() != 2 ) return rval;
505 
506  if( ( node.Lmax + iter_tol ) < ( node.Rmin - iter_tol ) )
507  {
508  if( point[node.dim] <= ( node.Lmax + iter_tol ) )
509  return find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result );
510  else if( point[node.dim] >= ( node.Rmin - iter_tol ) )
511  return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result );
512  result = std::make_pair( 0, CartVect( &point[0] ) ); // point lies in empty space.
513  return MB_SUCCESS;
514  }
515 
516  // Boxes overlap
517  // left of Rmin, you must be on the left
518  // we can't be sure about the boundaries since the boxes overlap
519  // this was a typo in the paper which caused pain.
520  if( point[node.dim] < ( node.Rmin - iter_tol ) )
521  return find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result );
522  // if you are on the right Lmax, you must be on the right
523  else if( point[node.dim] > ( node.Lmax + iter_tol ) )
524  return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result );
525 
526  /* pg5 of paper
527  * However, instead of always traversing either subtree
528  * first (e.g. left always before right), we first traverse
529  * the subtree whose bounding plane has the larger distance to the
530  * sought point. This results in less overall traversal, and the correct
531  * cell is identified more quickly.
532  */
533  // So far all testing confirms that this 'heuristic' is
534  // significantly slower.
535  // I conjecture this is because it gets improperly
536  // branch predicted..
537  // bool dir = (point[ node.dim] - node.Rmin) <=
538  // (node.Lmax - point[ node.dim]);
539  find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result );
540  if( result.first == 0 )
541  {
542  return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result );
543  }
544  return MB_SUCCESS;
545 }
546 
547 EntityHandle BVHTree::bruteforce_find( const double* point, const double iter_tol, const double inside_tol )
548 {
550  CartVect params;
551  for( unsigned int i = 0; i < myTree.size(); i++ )
552  {
553  if( myTree[i].dim != 3 || !myTree[i].box.contains_point( point, iter_tol ) ) continue;
554  if( myEval )
555  {
556  EntityHandle entity = 0;
558  ErrorCode rval = myEval->find_containing_entity( startSetHandle + i, point, iter_tol, inside_tol, entity,
560  if( entity )
561  return entity;
562  else if( MB_SUCCESS != rval )
563  return 0;
564  }
565  else
566  return startSetHandle + i;
567  }
568  return 0;
569 }
570 
572 {
573  if( !tree_node || *tree_node == startSetHandle )
574  {
575  box = boundBox;
576  return MB_SUCCESS;
577  }
578  else if( ( tree_node && !startSetHandle ) || *tree_node < startSetHandle ||
579  *tree_node - startSetHandle > myTree.size() )
580  return MB_FAILURE;
581 
582  box = myTree[*tree_node - startSetHandle].box;
583  return MB_SUCCESS;
584 }
585 
586 ErrorCode BVHTree::point_search( const double* point,
587  EntityHandle& leaf_out,
588  const double iter_tol,
589  const double inside_tol,
590  bool* multiple_leaves,
591  EntityHandle* start_node,
592  CartVect* params )
593 {
595 
596  EntityHandle this_set = ( start_node ? *start_node : startSetHandle );
597  // convoluted check because the root is different from startSetHandle
598  if( this_set != myRoot && ( this_set < startSetHandle || this_set >= startSetHandle + myTree.size() ) )
599  return MB_FAILURE;
600  else if( this_set == myRoot )
601  this_set = startSetHandle;
602 
603  std::vector< EntityHandle > candidates,
604  result_list; // list of subtrees to traverse, and results
605  candidates.push_back( this_set - startSetHandle );
606 
607  BoundBox box;
608  while( !candidates.empty() )
609  {
610  EntityHandle ind = candidates.back();
612  if( myTree[ind].dim == 3 ) treeStats.leavesVisited++;
613  this_set = startSetHandle + ind;
614  candidates.pop_back();
615 
616  // test box of this node
617  ErrorCode rval = get_bounding_box( box, &this_set );
618  if( MB_SUCCESS != rval ) return rval;
619  if( !box.contains_point( point, iter_tol ) ) continue;
620 
621  // else if not a leaf, test children & put on list
622  else if( myTree[ind].dim != 3 )
623  {
624  candidates.push_back( myTree[ind].child );
625  candidates.push_back( myTree[ind].child + 1 );
626  continue;
627  }
628  else if( myTree[ind].dim == 3 && myEval && params )
629  {
630  rval = myEval->find_containing_entity( startSetHandle + ind, point, iter_tol, inside_tol, leaf_out,
632  if( leaf_out || MB_SUCCESS != rval ) return rval;
633  }
634  else
635  {
636  // leaf node within distance; return in list
637  result_list.push_back( this_set );
638  }
639  }
640 
641  if( !result_list.empty() ) leaf_out = result_list[0];
642  if( multiple_leaves && result_list.size() > 1 ) *multiple_leaves = true;
643  return MB_SUCCESS;
644 }
645 
646 ErrorCode BVHTree::distance_search( const double from_point[3],
647  const double distance,
648  std::vector< EntityHandle >& result_list,
649  const double iter_tol,
650  const double inside_tol,
651  std::vector< double >* result_dists,
652  std::vector< CartVect >* result_params,
653  EntityHandle* tree_root )
654 {
655  // non-NULL root should be in tree
656  // convoluted check because the root is different from startSetHandle
657  EntityHandle this_set = ( tree_root ? *tree_root : startSetHandle );
658  if( this_set != myRoot && ( this_set < startSetHandle || this_set >= startSetHandle + myTree.size() ) )
659  return MB_FAILURE;
660  else if( this_set == myRoot )
661  this_set = startSetHandle;
662 
664 
665  const double dist_sqr = distance * distance;
666  const CartVect from( from_point );
667  std::vector< EntityHandle > candidates; // list of subtrees to traverse
668  // pre-allocate space for default max tree depth
669  candidates.reserve( maxDepth );
670 
671  // misc temporary values
672  ErrorCode rval;
673  BoundBox box;
674 
675  candidates.push_back( this_set - startSetHandle );
676 
677  while( !candidates.empty() )
678  {
679 
680  EntityHandle ind = candidates.back();
681  this_set = startSetHandle + ind;
682  candidates.pop_back();
684  if( myTree[ind].dim == 3 ) treeStats.leavesVisited++;
685 
686  // test box of this node
687  rval = get_bounding_box( box, &this_set );
688  if( MB_SUCCESS != rval ) return rval;
689  double d_sqr = box.distance_squared( from_point );
690 
691  // if greater than cutoff, continue
692  if( d_sqr > dist_sqr ) continue;
693 
694  // else if not a leaf, test children & put on list
695  else if( myTree[ind].dim != 3 )
696  {
697  candidates.push_back( myTree[ind].child );
698  candidates.push_back( myTree[ind].child + 1 );
699  continue;
700  }
701 
702  if( myEval && result_params )
703  {
704  EntityHandle ent;
705  CartVect params;
706  rval = myEval->find_containing_entity( startSetHandle + ind, from_point, iter_tol, inside_tol, ent,
708  if( MB_SUCCESS != rval )
709  return rval;
710  else if( ent )
711  {
712  result_list.push_back( ent );
713  result_params->push_back( params );
714  if( result_dists ) result_dists->push_back( 0.0 );
715  }
716  }
717  else
718  {
719  // leaf node within distance; return in list
720  result_list.push_back( this_set );
721  if( result_dists ) result_dists->push_back( sqrt( d_sqr ) );
722  }
723  }
724 
725  return MB_SUCCESS;
726 }
727 
728 ErrorCode BVHTree::print_nodes( std::vector< Node >& nodes )
729 {
730  int i;
731  std::vector< Node >::iterator it;
732  for( it = nodes.begin(), i = 0; it != nodes.end(); ++it, i++ )
733  {
734  std::cout << "Node " << i << ": dim = " << it->dim << ", child = " << it->child << ", Lmax/Rmin = " << it->Lmax
735  << "/" << it->Rmin << ", box = " << it->box << std::endl;
736  }
737  return MB_SUCCESS;
738 }
739 
741 {
742  int i;
743  std::vector< TreeNode >::iterator it;
744  for( it = myTree.begin(), i = 0; it != myTree.end(); ++it, i++ )
745  {
746  std::cout << "Node " << i << ": dim = " << it->dim << ", child = " << it->child << ", Lmax/Rmin = " << it->Lmax
747  << "/" << it->Rmin << ", box = " << it->box << std::endl;
748  }
749  return MB_SUCCESS;
750 }
751 
752 } // namespace moab