MOAB: Mesh Oriented datABase
(version 5.2.1)
|
00001 #include "moab/BVHTree.hpp" 00002 #include "moab/Interface.hpp" 00003 #include "moab/ElemEvaluator.hpp" 00004 #include "moab/ReadUtilIface.hpp" 00005 #include "moab/CpuTimer.hpp" 00006 00007 namespace moab 00008 { 00009 const char* BVHTree::treeName = "BVHTree"; 00010 00011 ErrorCode BVHTree::build_tree( const Range& entities, EntityHandle* tree_root_set, FileOptions* options ) 00012 { 00013 ErrorCode rval; 00014 CpuTimer cp; 00015 00016 if( options ) 00017 { 00018 rval = parse_options( *options ); 00019 if( MB_SUCCESS != rval ) return rval; 00020 00021 if( !options->all_seen() ) return MB_FAILURE; 00022 } 00023 00024 // calculate bounding box of elements 00025 BoundBox box; 00026 rval = box.update( *moab(), entities ); 00027 if( MB_SUCCESS != rval ) return rval; 00028 00029 // create tree root 00030 EntityHandle tmp_root; 00031 if( !tree_root_set ) tree_root_set = &tmp_root; 00032 rval = create_root( box.bMin.array(), box.bMax.array(), *tree_root_set ); 00033 if( MB_SUCCESS != rval ) return rval; 00034 rval = mbImpl->add_entities( *tree_root_set, entities ); 00035 if( MB_SUCCESS != rval ) return rval; 00036 00037 // a fully balanced tree will have 2*_entities.size() 00038 // which is one doubling away.. 00039 std::vector< Node > tree_nodes; 00040 tree_nodes.reserve( entities.size() / maxPerLeaf ); 00041 std::vector< HandleData > handle_data_vec; 00042 rval = construct_element_vec( handle_data_vec, entities, boundBox ); 00043 if( MB_SUCCESS != rval ) return rval; 00044 00045 #ifndef NDEBUG 00046 for( std::vector< HandleData >::const_iterator i = handle_data_vec.begin(); i != handle_data_vec.end(); ++i ) 00047 { 00048 if( !boundBox.intersects_box( i->myBox, 0 ) ) 00049 { 00050 std::cerr << "BB:" << boundBox << "EB:" << i->myBox << std::endl; 00051 return MB_FAILURE; 00052 } 00053 } 00054 #endif 00055 // We only build nonempty trees 00056 if( !handle_data_vec.empty() ) 00057 { 00058 // initially all bits are set 00059 tree_nodes.push_back( Node() ); 00060 const int depth = local_build_tree( tree_nodes, handle_data_vec.begin(), handle_data_vec.end(), 0, boundBox ); 00061 #ifndef NDEBUG 00062 std::set< EntityHandle > entity_handles; 00063 for( std::vector< Node >::iterator n = tree_nodes.begin(); n != tree_nodes.end(); ++n ) 00064 { 00065 for( HandleDataVec::const_iterator j = n->entities.begin(); j != n->entities.end(); ++j ) 00066 { 00067 entity_handles.insert( j->myHandle ); 00068 } 00069 } 00070 if( entity_handles.size() != entities.size() ) { std::cout << "Entity Handle Size Mismatch!" << std::endl; } 00071 for( Range::iterator i = entities.begin(); i != entities.end(); ++i ) 00072 { 00073 if( entity_handles.find( *i ) == entity_handles.end() ) 00074 std::cout << "Tree is missing an entity! " << std::endl; 00075 } 00076 #endif 00077 treeDepth = std::max( depth, treeDepth ); 00078 } 00079 00080 // convert vector of Node's to entity sets and vector of TreeNode's 00081 rval = convert_tree( tree_nodes ); 00082 if( MB_SUCCESS != rval ) return rval; 00083 00084 treeStats.reset(); 00085 rval = treeStats.compute_stats( mbImpl, startSetHandle ); 00086 treeStats.initTime = cp.time_elapsed(); 00087 00088 return rval; 00089 } 00090 00091 ErrorCode BVHTree::convert_tree( std::vector< Node >& tree_nodes ) 00092 { 00093 // first construct the proper number of entity sets 00094 ReadUtilIface* read_util; 00095 ErrorCode rval = mbImpl->query_interface( read_util ); 00096 if( MB_SUCCESS != rval ) return rval; 00097 00098 { // isolate potentially-large std::vector so it gets deleted earlier 00099 std::vector< unsigned int > tmp_flags( tree_nodes.size(), meshsetFlags ); 00100 rval = read_util->create_entity_sets( tree_nodes.size(), &tmp_flags[0], 0, startSetHandle ); 00101 if( MB_SUCCESS != rval ) return rval; 00102 rval = mbImpl->release_interface( read_util ); 00103 if( MB_SUCCESS != rval ) return rval; 00104 } 00105 00106 // populate the sets and the TreeNode vector 00107 EntityHandle set_handle = startSetHandle; 00108 std::vector< Node >::iterator it; 00109 myTree.reserve( tree_nodes.size() ); 00110 for( it = tree_nodes.begin(); it != tree_nodes.end(); ++it, set_handle++ ) 00111 { 00112 if( it != tree_nodes.begin() && !it->entities.empty() ) 00113 { 00114 Range range; 00115 for( HandleDataVec::iterator hit = it->entities.begin(); hit != it->entities.end(); ++hit ) 00116 range.insert( hit->myHandle ); 00117 rval = mbImpl->add_entities( set_handle, range ); 00118 if( MB_SUCCESS != rval ) return rval; 00119 } 00120 myTree.push_back( TreeNode( it->dim, it->child, it->Lmax, it->Rmin, it->box ) ); 00121 00122 if( it->dim != 3 ) 00123 { 00124 rval = mbImpl->add_child_meshset( set_handle, startSetHandle + it->child ); 00125 if( MB_SUCCESS != rval ) return rval; 00126 rval = mbImpl->add_child_meshset( set_handle, startSetHandle + it->child + 1 ); 00127 if( MB_SUCCESS != rval ) return rval; 00128 } 00129 } 00130 00131 return MB_SUCCESS; 00132 } 00133 00134 ErrorCode BVHTree::parse_options( FileOptions& opts ) 00135 { 00136 ErrorCode rval = parse_common_options( opts ); 00137 if( MB_SUCCESS != rval ) return rval; 00138 00139 // SPLITS_PER_DIR: number of candidate splits considered per direction; default = 3 00140 int tmp_int; 00141 rval = opts.get_int_option( "SPLITS_PER_DIR", tmp_int ); 00142 if( MB_SUCCESS == rval ) splitsPerDir = tmp_int; 00143 00144 return MB_SUCCESS; 00145 } 00146 00147 void BVHTree::establish_buckets( HandleDataVec::const_iterator begin, HandleDataVec::const_iterator end, 00148 const BoundBox& interval, std::vector< std::vector< Bucket > >& buckets ) const 00149 { 00150 // put each element into its bucket 00151 for( HandleDataVec::const_iterator i = begin; i != end; ++i ) 00152 { 00153 const BoundBox& box = i->myBox; 00154 for( unsigned int dim = 0; dim < 3; ++dim ) 00155 { 00156 const unsigned int index = Bucket::bucket_index( splitsPerDir, box, interval, dim ); 00157 assert( index < buckets[dim].size() ); 00158 Bucket& bucket = buckets[dim][index]; 00159 if( bucket.mySize > 0 ) 00160 bucket.boundingBox.update( box ); 00161 else 00162 bucket.boundingBox = box; 00163 bucket.mySize++; 00164 } 00165 } 00166 00167 #ifndef NDEBUG 00168 BoundBox elt_union = begin->myBox; 00169 for( HandleDataVec::const_iterator i = begin; i != end; ++i ) 00170 { 00171 const BoundBox& box = i->myBox; 00172 elt_union.update( box ); 00173 for( unsigned int dim = 0; dim < 3; ++dim ) 00174 { 00175 const unsigned int index = Bucket::bucket_index( splitsPerDir, box, interval, dim ); 00176 Bucket& bucket = buckets[dim][index]; 00177 if( !bucket.boundingBox.intersects_box( box ) ) std::cerr << "Buckets not covering elements!" << std::endl; 00178 } 00179 } 00180 if( !elt_union.intersects_box( interval ) ) 00181 { 00182 std::cout << "element union: " << std::endl << elt_union; 00183 std::cout << "intervals: " << std::endl << interval; 00184 std::cout << "union of elts does not contain original box!" << std::endl; 00185 } 00186 if( !interval.intersects_box( elt_union ) ) 00187 { 00188 std::cout << "original box does not contain union of elts" << std::endl; 00189 std::cout << interval << std::endl << elt_union << std::endl; 00190 } 00191 for( unsigned int d = 0; d < 3; ++d ) 00192 { 00193 std::vector< unsigned int > nonempty; 00194 const std::vector< Bucket >& buckets_ = buckets[d]; 00195 unsigned int j = 0; 00196 for( std::vector< Bucket >::const_iterator i = buckets_.begin(); i != buckets_.end(); ++i, ++j ) 00197 { 00198 if( i->mySize > 0 ) { nonempty.push_back( j ); } 00199 } 00200 BoundBox test_box = buckets_[nonempty.front()].boundingBox; 00201 for( unsigned int i = 0; i < nonempty.size(); ++i ) 00202 test_box.update( buckets_[nonempty[i]].boundingBox ); 00203 00204 if( !test_box.intersects_box( interval ) ) 00205 std::cout << "union of buckets in dimension: " << d << "does not contain original box!" << std::endl; 00206 if( !interval.intersects_box( test_box ) ) 00207 { 00208 std::cout << "original box does " 00209 << "not contain union of buckets" 00210 << "in dimension: " << d << std::endl; 00211 std::cout << interval << std::endl << test_box << std::endl; 00212 } 00213 } 00214 #endif 00215 } 00216 00217 void BVHTree::initialize_splits( std::vector< std::vector< SplitData > >& splits, 00218 const std::vector< std::vector< Bucket > >& buckets, const SplitData& data ) const 00219 { 00220 for( unsigned int d = 0; d < 3; ++d ) 00221 { 00222 std::vector< SplitData >::iterator splits_begin = splits[d].begin(); 00223 std::vector< SplitData >::iterator splits_end = splits[d].end(); 00224 std::vector< Bucket >::const_iterator left_begin = buckets[d].begin(); 00225 std::vector< Bucket >::const_iterator _end = buckets[d].end(); 00226 std::vector< Bucket >::const_iterator left_end = buckets[d].begin() + 1; 00227 for( std::vector< SplitData >::iterator s = splits_begin; s != splits_end; ++s, ++left_end ) 00228 { 00229 s->nl = set_interval( s->leftBox, left_begin, left_end ); 00230 if( s->nl == 0 ) 00231 { 00232 s->leftBox = data.boundingBox; 00233 s->leftBox.bMax[d] = s->leftBox.bMin[d]; 00234 } 00235 s->nr = set_interval( s->rightBox, left_end, _end ); 00236 if( s->nr == 0 ) 00237 { 00238 s->rightBox = data.boundingBox; 00239 s->rightBox.bMin[d] = s->rightBox.bMax[d]; 00240 } 00241 s->Lmax = s->leftBox.bMax[d]; 00242 s->Rmin = s->rightBox.bMin[d]; 00243 s->split = std::distance( splits_begin, s ); 00244 s->dim = d; 00245 } 00246 #ifndef NDEBUG 00247 for( std::vector< SplitData >::iterator s = splits_begin; s != splits_end; ++s ) 00248 { 00249 BoundBox test_box = s->leftBox; 00250 test_box.update( s->rightBox ); 00251 if( !data.boundingBox.intersects_box( test_box ) ) 00252 { 00253 std::cout << "nr: " << s->nr << std::endl; 00254 std::cout << "Test box: " << std::endl << test_box; 00255 std::cout << "Left box: " << std::endl << s->leftBox; 00256 std::cout << "Right box: " << std::endl << s->rightBox; 00257 std::cout << "Interval: " << std::endl << data.boundingBox; 00258 std::cout << "Split boxes larger than bb" << std::endl; 00259 } 00260 if( !test_box.intersects_box( data.boundingBox ) ) 00261 { std::cout << "bb larger than union of split boxes" << std::endl; } 00262 } 00263 #endif 00264 } 00265 } 00266 00267 void BVHTree::median_order( HandleDataVec::iterator& begin, HandleDataVec::iterator& end, SplitData& data ) const 00268 { 00269 int dim = data.dim; 00270 for( HandleDataVec::iterator i = begin; i != end; ++i ) 00271 { 00272 i->myDim = 0.5 * ( i->myBox.bMin[dim], i->myBox.bMax[dim] ); 00273 } 00274 std::sort( begin, end, BVHTree::HandleData_comparator() ); 00275 const unsigned int total = std::distance( begin, end ); 00276 HandleDataVec::iterator middle = begin + ( total / 2 ); 00277 double middle_center = middle->myDim; 00278 middle_center += ( ++middle )->myDim; 00279 middle_center *= 0.5; 00280 data.split = middle_center; 00281 data.nl = std::distance( begin, middle ) + 1; 00282 data.nr = total - data.nl; 00283 ++middle; 00284 data.leftBox = begin->myBox; 00285 data.rightBox = middle->myBox; 00286 for( HandleDataVec::iterator i = begin; i != middle; ++i ) 00287 { 00288 i->myDim = 0; 00289 data.leftBox.update( i->myBox ); 00290 } 00291 for( HandleDataVec::iterator i = middle; i != end; ++i ) 00292 { 00293 i->myDim = 1; 00294 data.rightBox.update( i->myBox ); 00295 } 00296 data.Rmin = data.rightBox.bMin[data.dim]; 00297 data.Lmax = data.leftBox.bMax[data.dim]; 00298 #ifndef NDEBUG 00299 BoundBox test_box( data.rightBox ); 00300 if( !data.boundingBox.intersects_box( test_box ) ) 00301 { 00302 std::cerr << "MEDIAN: BB Does not contain splits" << std::endl; 00303 std::cerr << "test_box: " << test_box << std::endl; 00304 std::cerr << "data.boundingBox: " << data.boundingBox << std::endl; 00305 } 00306 #endif 00307 } 00308 00309 void BVHTree::find_split( HandleDataVec::iterator& begin, HandleDataVec::iterator& end, SplitData& data ) const 00310 { 00311 std::vector< std::vector< Bucket > > buckets( 3, std::vector< Bucket >( splitsPerDir + 1 ) ); 00312 std::vector< std::vector< SplitData > > splits( 3, std::vector< SplitData >( splitsPerDir, data ) ); 00313 00314 const BoundBox interval = data.boundingBox; 00315 establish_buckets( begin, end, interval, buckets ); 00316 initialize_splits( splits, buckets, data ); 00317 choose_best_split( splits, data ); 00318 const bool use_median = ( 0 == data.nl ) || ( data.nr == 0 ); 00319 if( !use_median ) 00320 order_elements( begin, end, data ); 00321 else 00322 median_order( begin, end, data ); 00323 00324 #ifndef NDEBUG 00325 bool seen_one = false, issue = false; 00326 bool first_left = true, first_right = true; 00327 unsigned int count_left = 0, count_right = 0; 00328 BoundBox left_box, right_box; 00329 for( HandleDataVec::iterator i = begin; i != end; ++i ) 00330 { 00331 int order = i->myDim; 00332 if( order != 0 && order != 1 ) 00333 { 00334 std::cerr << "Invalid order element !"; 00335 std::cerr << order << std::endl; 00336 std::exit( -1 ); 00337 } 00338 if( order == 1 ) 00339 { 00340 seen_one = 1; 00341 count_right++; 00342 if( first_right ) 00343 { 00344 right_box = i->myBox; 00345 first_right = false; 00346 } 00347 else 00348 { 00349 right_box.update( i->myBox ); 00350 } 00351 if( !right_box.intersects_box( i->myBox ) ) 00352 { 00353 if( !issue ) { std::cerr << "Bounding right box issue!" << std::endl; } 00354 issue = true; 00355 } 00356 } 00357 if( order == 0 ) 00358 { 00359 count_left++; 00360 if( first_left ) 00361 { 00362 left_box = i->myBox; 00363 first_left = false; 00364 } 00365 else 00366 { 00367 left_box.update( i->myBox ); 00368 } 00369 if( !data.leftBox.intersects_box( i->myBox ) ) 00370 { 00371 if( !issue ) { std::cerr << "Bounding left box issue!" << std::endl; } 00372 issue = true; 00373 } 00374 if( seen_one ) 00375 { 00376 std::cerr << "Invalid ordering!" << std::endl; 00377 std::cout << ( i - 1 )->myDim << order << std::endl; 00378 exit( -1 ); 00379 } 00380 } 00381 } 00382 if( !left_box.intersects_box( data.leftBox ) ) std::cout << "left elts do not contain left box" << std::endl; 00383 if( !data.leftBox.intersects_box( left_box ) ) std::cout << "left box does not contain left elts" << std::endl; 00384 if( !right_box.intersects_box( data.rightBox ) ) std::cout << "right elts do not contain right box" << std::endl; 00385 if( !data.rightBox.intersects_box( right_box ) ) std::cout << "right box do not contain right elts" << std::endl; 00386 00387 if( count_left != data.nl || count_right != data.nr ) 00388 { 00389 std::cerr << "counts are off!" << std::endl; 00390 std::cerr << "total: " << std::distance( begin, end ) << std::endl; 00391 std::cerr << "Dim: " << data.dim << std::endl; 00392 std::cerr << data.Lmax << " , " << data.Rmin << std::endl; 00393 std::cerr << "Right box: " << std::endl << data.rightBox << "Left box: " << std::endl << data.leftBox; 00394 std::cerr << "supposed to be: " << data.nl << " " << data.nr << std::endl; 00395 std::cerr << "accountant says: " << count_left << " " << count_right << std::endl; 00396 std::exit( -1 ); 00397 } 00398 #endif 00399 } 00400 00401 int BVHTree::local_build_tree( std::vector< Node >& tree_nodes, HandleDataVec::iterator begin, 00402 HandleDataVec::iterator end, const int index, const BoundBox& box, const int depth ) 00403 { 00404 #ifndef NDEBUG 00405 for( HandleDataVec::const_iterator i = begin; i != end; ++i ) 00406 { 00407 if( !box.intersects_box( i->myBox, 0 ) ) 00408 { 00409 std::cerr << "depth: " << depth << std::endl; 00410 std::cerr << "BB:" << box << "EB:" << i->myBox << std::endl; 00411 std::exit( -1 ); 00412 } 00413 } 00414 #endif 00415 00416 const unsigned int total_num_elements = std::distance( begin, end ); 00417 tree_nodes[index].box = box; 00418 00419 // logic for splitting conditions 00420 if( (int)total_num_elements > maxPerLeaf && depth < maxDepth ) 00421 { 00422 SplitData data; 00423 data.boundingBox = box; 00424 find_split( begin, end, data ); 00425 // assign data to node 00426 tree_nodes[index].Lmax = data.Lmax; 00427 tree_nodes[index].Rmin = data.Rmin; 00428 tree_nodes[index].dim = data.dim; 00429 tree_nodes[index].child = tree_nodes.size(); 00430 // insert left, right children; 00431 tree_nodes.push_back( Node() ); 00432 tree_nodes.push_back( Node() ); 00433 const int left_depth = 00434 local_build_tree( tree_nodes, begin, begin + data.nl, tree_nodes[index].child, data.leftBox, depth + 1 ); 00435 const int right_depth = 00436 local_build_tree( tree_nodes, begin + data.nl, end, tree_nodes[index].child + 1, data.rightBox, depth + 1 ); 00437 return std::max( left_depth, right_depth ); 00438 } 00439 00440 tree_nodes[index].dim = 3; 00441 std::copy( begin, end, std::back_inserter( tree_nodes[index].entities ) ); 00442 return depth; 00443 } 00444 00445 ErrorCode BVHTree::find_point( const std::vector< double >& point, const unsigned int& index, const double iter_tol, 00446 const double inside_tol, std::pair< EntityHandle, CartVect >& result ) 00447 { 00448 if( index == 0 ) treeStats.numTraversals++; 00449 const TreeNode& node = myTree[index]; 00450 treeStats.nodesVisited++; 00451 CartVect params; 00452 int is_inside; 00453 ErrorCode rval = MB_SUCCESS; 00454 if( node.dim == 3 ) 00455 { 00456 treeStats.leavesVisited++; 00457 Range entities; 00458 rval = mbImpl->get_entities_by_handle( startSetHandle + index, entities ); 00459 if( MB_SUCCESS != rval ) return rval; 00460 00461 for( Range::iterator i = entities.begin(); i != entities.end(); ++i ) 00462 { 00463 treeStats.traversalLeafObjectTests++; 00464 myEval->set_ent_handle( *i ); 00465 myEval->reverse_eval( &point[0], iter_tol, inside_tol, params.array(), &is_inside ); 00466 if( is_inside ) 00467 { 00468 result.first = *i; 00469 result.second = params; 00470 return MB_SUCCESS; 00471 } 00472 } 00473 result.first = 0; 00474 return MB_SUCCESS; 00475 } 00476 // the extra tol here considers the case where 00477 // 0 < Rmin - Lmax < 2tol 00478 std::vector< EntityHandle > children; 00479 rval = mbImpl->get_child_meshsets( startSetHandle + index, children ); 00480 if( MB_SUCCESS != rval || children.size() != 2 ) return rval; 00481 00482 if( ( node.Lmax + iter_tol ) < ( node.Rmin - iter_tol ) ) 00483 { 00484 if( point[node.dim] <= ( node.Lmax + iter_tol ) ) 00485 return find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result ); 00486 else if( point[node.dim] >= ( node.Rmin - iter_tol ) ) 00487 return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result ); 00488 result = std::make_pair( 0, CartVect( &point[0] ) ); // point lies in empty space. 00489 return MB_SUCCESS; 00490 } 00491 00492 // Boxes overlap 00493 // left of Rmin, you must be on the left 00494 // we can't be sure about the boundaries since the boxes overlap 00495 // this was a typo in the paper which caused pain. 00496 if( point[node.dim] < ( node.Rmin - iter_tol ) ) 00497 return find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result ); 00498 // if you are on the right Lmax, you must be on the right 00499 else if( point[node.dim] > ( node.Lmax + iter_tol ) ) 00500 return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result ); 00501 00502 /* pg5 of paper 00503 * However, instead of always traversing either subtree 00504 * first (e.g. left always before right), we first traverse 00505 * the subtree whose bounding plane has the larger distance to the 00506 * sought point. This results in less overall traversal, and the correct 00507 * cell is identified more quickly. 00508 */ 00509 // So far all testing confirms that this 'heuristic' is 00510 // significantly slower. 00511 // I conjecture this is because it gets improperly 00512 // branch predicted.. 00513 // bool dir = (point[ node.dim] - node.Rmin) <= 00514 // (node.Lmax - point[ node.dim]); 00515 find_point( point, children[0] - startSetHandle, iter_tol, inside_tol, result ); 00516 if( result.first == 0 ) { return find_point( point, children[1] - startSetHandle, iter_tol, inside_tol, result ); } 00517 return MB_SUCCESS; 00518 } 00519 00520 EntityHandle BVHTree::bruteforce_find( const double* point, const double iter_tol, const double inside_tol ) 00521 { 00522 treeStats.numTraversals++; 00523 CartVect params; 00524 for( unsigned int i = 0; i < myTree.size(); i++ ) 00525 { 00526 if( myTree[i].dim != 3 || !myTree[i].box.contains_point( point, iter_tol ) ) continue; 00527 if( myEval ) 00528 { 00529 EntityHandle entity = 0; 00530 treeStats.leavesVisited++; 00531 ErrorCode rval = myEval->find_containing_entity( startSetHandle + i, point, iter_tol, inside_tol, entity, 00532 params.array(), &treeStats.traversalLeafObjectTests ); 00533 if( entity ) 00534 return entity; 00535 else if( MB_SUCCESS != rval ) 00536 return 0; 00537 } 00538 else 00539 return startSetHandle + i; 00540 } 00541 return 0; 00542 } 00543 00544 ErrorCode BVHTree::get_bounding_box( BoundBox& box, EntityHandle* tree_node ) const 00545 { 00546 if( !tree_node || *tree_node == startSetHandle ) 00547 { 00548 box = boundBox; 00549 return MB_SUCCESS; 00550 } 00551 else if( ( tree_node && !startSetHandle ) || *tree_node < startSetHandle || 00552 *tree_node - startSetHandle > myTree.size() ) 00553 return MB_FAILURE; 00554 00555 box = myTree[*tree_node - startSetHandle].box; 00556 return MB_SUCCESS; 00557 } 00558 00559 ErrorCode BVHTree::point_search( const double* point, EntityHandle& leaf_out, const double iter_tol, 00560 const double inside_tol, bool* multiple_leaves, EntityHandle* start_node, 00561 CartVect* params ) 00562 { 00563 treeStats.numTraversals++; 00564 00565 EntityHandle this_set = ( start_node ? *start_node : startSetHandle ); 00566 // convoluted check because the root is different from startSetHandle 00567 if( this_set != myRoot && ( this_set < startSetHandle || this_set >= startSetHandle + myTree.size() ) ) 00568 return MB_FAILURE; 00569 else if( this_set == myRoot ) 00570 this_set = startSetHandle; 00571 00572 std::vector< EntityHandle > candidates, 00573 result_list; // list of subtrees to traverse, and results 00574 candidates.push_back( this_set - startSetHandle ); 00575 00576 BoundBox box; 00577 while( !candidates.empty() ) 00578 { 00579 EntityHandle ind = candidates.back(); 00580 treeStats.nodesVisited++; 00581 if( myTree[ind].dim == 3 ) treeStats.leavesVisited++; 00582 this_set = startSetHandle + ind; 00583 candidates.pop_back(); 00584 00585 // test box of this node 00586 ErrorCode rval = get_bounding_box( box, &this_set ); 00587 if( MB_SUCCESS != rval ) return rval; 00588 if( !box.contains_point( point, iter_tol ) ) continue; 00589 00590 // else if not a leaf, test children & put on list 00591 else if( myTree[ind].dim != 3 ) 00592 { 00593 candidates.push_back( myTree[ind].child ); 00594 candidates.push_back( myTree[ind].child + 1 ); 00595 continue; 00596 } 00597 else if( myTree[ind].dim == 3 && myEval && params ) 00598 { 00599 rval = myEval->find_containing_entity( startSetHandle + ind, point, iter_tol, inside_tol, leaf_out, 00600 params->array(), &treeStats.traversalLeafObjectTests ); 00601 if( leaf_out || MB_SUCCESS != rval ) return rval; 00602 } 00603 else 00604 { 00605 // leaf node within distance; return in list 00606 result_list.push_back( this_set ); 00607 } 00608 } 00609 00610 if( !result_list.empty() ) leaf_out = result_list[0]; 00611 if( multiple_leaves && result_list.size() > 1 ) *multiple_leaves = true; 00612 return MB_SUCCESS; 00613 } 00614 00615 ErrorCode BVHTree::distance_search( const double from_point[3], const double distance, 00616 std::vector< EntityHandle >& result_list, const double iter_tol, 00617 const double inside_tol, std::vector< double >* result_dists, 00618 std::vector< CartVect >* result_params, EntityHandle* tree_root ) 00619 { 00620 // non-NULL root should be in tree 00621 // convoluted check because the root is different from startSetHandle 00622 EntityHandle this_set = ( tree_root ? *tree_root : startSetHandle ); 00623 if( this_set != myRoot && ( this_set < startSetHandle || this_set >= startSetHandle + myTree.size() ) ) 00624 return MB_FAILURE; 00625 else if( this_set == myRoot ) 00626 this_set = startSetHandle; 00627 00628 treeStats.numTraversals++; 00629 00630 const double dist_sqr = distance * distance; 00631 const CartVect from( from_point ); 00632 std::vector< EntityHandle > candidates; // list of subtrees to traverse 00633 // pre-allocate space for default max tree depth 00634 candidates.reserve( maxDepth ); 00635 00636 // misc temporary values 00637 ErrorCode rval; 00638 BoundBox box; 00639 00640 candidates.push_back( this_set - startSetHandle ); 00641 00642 while( !candidates.empty() ) 00643 { 00644 00645 EntityHandle ind = candidates.back(); 00646 this_set = startSetHandle + ind; 00647 candidates.pop_back(); 00648 treeStats.nodesVisited++; 00649 if( myTree[ind].dim == 3 ) treeStats.leavesVisited++; 00650 00651 // test box of this node 00652 rval = get_bounding_box( box, &this_set ); 00653 if( MB_SUCCESS != rval ) return rval; 00654 double d_sqr = box.distance_squared( from_point ); 00655 00656 // if greater than cutoff, continue 00657 if( d_sqr > dist_sqr ) continue; 00658 00659 // else if not a leaf, test children & put on list 00660 else if( myTree[ind].dim != 3 ) 00661 { 00662 candidates.push_back( myTree[ind].child ); 00663 candidates.push_back( myTree[ind].child + 1 ); 00664 continue; 00665 } 00666 00667 if( myEval && result_params ) 00668 { 00669 EntityHandle ent; 00670 CartVect params; 00671 rval = myEval->find_containing_entity( startSetHandle + ind, from_point, iter_tol, inside_tol, ent, 00672 params.array(), &treeStats.traversalLeafObjectTests ); 00673 if( MB_SUCCESS != rval ) 00674 return rval; 00675 else if( ent ) 00676 { 00677 result_list.push_back( ent ); 00678 result_params->push_back( params ); 00679 if( result_dists ) result_dists->push_back( 0.0 ); 00680 } 00681 } 00682 else 00683 { 00684 // leaf node within distance; return in list 00685 result_list.push_back( this_set ); 00686 if( result_dists ) result_dists->push_back( sqrt( d_sqr ) ); 00687 } 00688 } 00689 00690 return MB_SUCCESS; 00691 } 00692 00693 ErrorCode BVHTree::print_nodes( std::vector< Node >& nodes ) 00694 { 00695 int i; 00696 std::vector< Node >::iterator it; 00697 for( it = nodes.begin(), i = 0; it != nodes.end(); ++it, i++ ) 00698 { 00699 std::cout << "Node " << i << ": dim = " << it->dim << ", child = " << it->child << ", Lmax/Rmin = " << it->Lmax 00700 << "/" << it->Rmin << ", box = " << it->box << std::endl; 00701 } 00702 return MB_SUCCESS; 00703 } 00704 00705 ErrorCode BVHTree::print() 00706 { 00707 int i; 00708 std::vector< TreeNode >::iterator it; 00709 for( it = myTree.begin(), i = 0; it != myTree.end(); ++it, i++ ) 00710 { 00711 std::cout << "Node " << i << ": dim = " << it->dim << ", child = " << it->child << ", Lmax/Rmin = " << it->Lmax 00712 << "/" << it->Rmin << ", box = " << it->box << std::endl; 00713 } 00714 return MB_SUCCESS; 00715 } 00716 00717 } // namespace moab