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