moab
BVHTree.cpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines