moab
OrientedBoxTreeTool.cpp
Go to the documentation of this file.
00001 /*
00002  * MOAB, a Mesh-Oriented datABase, is a software component for creating,
00003  * storing and accessing finite element mesh data.
00004  * 
00005  * Copyright 2004 Sandia Corporation.  Under the terms of Contract
00006  * DE-AC04-94AL85000 with Sandia Coroporation, the U.S. Government
00007  * retains certain rights in this software.
00008  * 
00009  * This library is free software; you can redistribute it and/or
00010  * modify it under the terms of the GNU Lesser General Public
00011  * License as published by the Free Software Foundation; either
00012  * version 2.1 of the License, or (at your option) any later version.
00013  * 
00014  */
00015 
00021 #include "moab/Interface.hpp"
00022 #include "Internals.hpp"
00023 #include "moab/OrientedBoxTreeTool.hpp"
00024 #include "OrientedBox.hpp"
00025 #include "moab/Range.hpp"
00026 #include "moab/CN.hpp"
00027 #include "moab/GeomUtil.hpp"
00028 #include "MBTagConventions.hpp"
00029 #include <iostream>
00030 #include <iomanip>
00031 #include <algorithm>
00032 #include <limits>
00033 #include <assert.h>
00034 #include <math.h>
00035 
00036 //#define MB_OBB_USE_VECTOR_QUERIES
00037 //#define MB_OBB_USE_TYPE_QUERIES
00038 
00039 namespace moab {
00040 
00053 static
00054 bool edge_node_intersect(const EntityHandle                tri,
00055                          const CartVect&                   ray_direction,
00056                          const GeomUtil::intersection_type int_type,
00057                          const std::vector<EntityHandle>&  close_tris,
00058                          const std::vector<int>&           close_senses,
00059                          const Interface*                  MBI,
00060                          std::vector<EntityHandle>*        neighborhood_tris = 0 ) {
00061 
00062   // get the node of the triangle
00063   const EntityHandle* conn;
00064   int len;
00065   ErrorCode rval = MBI->get_connectivity( tri, conn, len );
00066   if(MB_SUCCESS!=rval || 3!=len) return MB_FAILURE;
00067 
00068   // get adjacent tris (and keep their corresponding senses)
00069   std::vector<EntityHandle> adj_tris;
00070   std::vector<int>          adj_senses;
00071 
00072   // node intersection
00073   if(GeomUtil::NODE0==int_type || GeomUtil::NODE1==int_type || GeomUtil::NODE2==int_type) {
00074  
00075     // get the intersected node
00076     EntityHandle node;
00077     if     (GeomUtil::NODE0==int_type) node = conn[0];
00078     else if(GeomUtil::NODE1==int_type) node = conn[1];
00079     else                               node = conn[2];
00080 
00081     // get tris adjacent to node
00082     for(unsigned i=0; i<close_tris.size(); ++i) {
00083       const EntityHandle* con;
00084       rval = MBI->get_connectivity( close_tris[i], con, len );
00085       if(MB_SUCCESS!=rval || 3!=len) return MB_FAILURE;
00086 
00087       if(node==con[0] || node==con[1] || node==con[2]) {
00088         adj_tris.push_back(   close_tris[i]   );
00089         adj_senses.push_back( close_senses[i] );
00090       }
00091     }
00092     if( adj_tris.empty() ) {
00093       std::cerr << "error: no tris are adjacent to the node" << std::endl;
00094       return MB_FAILURE;
00095     }
00096   // edge intersection
00097   } else if(GeomUtil::EDGE0==int_type || GeomUtil::EDGE1==int_type || GeomUtil::EDGE2==int_type) {
00098 
00099     // get the endpoints of the edge
00100     EntityHandle endpts[2];
00101     if       (GeomUtil::EDGE0==int_type) {
00102       endpts[0] = conn[0];
00103       endpts[1] = conn[1]; 
00104     } else if(GeomUtil::EDGE1==int_type) {
00105       endpts[0] = conn[1];
00106       endpts[1] = conn[2];
00107     } else {
00108       endpts[0] = conn[2];
00109       endpts[1] = conn[0];
00110     }
00111 
00112     // get tris adjacent to edge
00113     for(unsigned i=0; i<close_tris.size(); ++i) {
00114       const EntityHandle* con;
00115       rval = MBI->get_connectivity( close_tris[i], con, len );
00116       if(MB_SUCCESS!=rval || 3!=len) return MB_FAILURE;
00117 
00118       // check both orientations in case close_tris are not on the same surface
00119       if( (endpts[0]==con[0] && endpts[1]==con[1]) ||
00120           (endpts[0]==con[1] && endpts[1]==con[0]) ||
00121           (endpts[0]==con[1] && endpts[1]==con[2]) ||
00122           (endpts[0]==con[2] && endpts[1]==con[1]) ||
00123           (endpts[0]==con[2] && endpts[1]==con[0]) ||
00124           (endpts[0]==con[0] && endpts[1]==con[2]) ) {
00125         adj_tris.push_back(   close_tris[i]   );
00126         adj_senses.push_back( close_senses[i] );
00127       }
00128     }   
00129     // In a 2-manifold each edge is adjacent to exactly 2 tris
00130     if(2 != adj_tris.size() ) {
00131       std::cerr << "error: edge of a manifold must be topologically adjacent to exactly 2 tris" 
00132                 << std::endl;
00133       MBI->list_entities( endpts, 2 );
00134       return true;
00135     }
00136   } else {
00137     std::cerr << "error: special case not an node/edge intersection" << std::endl;
00138     return MB_FAILURE;
00139   }
00140 
00141   // The close tris were in proximity to the intersection. The adj_tris are
00142   // topologically adjacent to the intersection (the neighborhood).
00143   if(neighborhood_tris) (*neighborhood_tris).assign( adj_tris.begin(), adj_tris.end() );
00144 
00145   // determine glancing/piercing
00146   // If a desired_orientation was used in this call to ray_intersect_sets, 
00147   // the plucker_ray_tri_intersect will have already used it. For a piercing
00148   // intersection, the normal of all tris must have the same orientation.
00149   int sign = 0;
00150   for(unsigned i=0; i<adj_tris.size(); ++i) {
00151     const EntityHandle* con;
00152     rval = MBI->get_connectivity( adj_tris[i], con, len );
00153     if(MB_SUCCESS!=rval || 3!=len) return MB_FAILURE;
00154     CartVect coords[3];
00155     rval = MBI->get_coords( con, len, coords[0].array() );      
00156     if(MB_SUCCESS != rval) return MB_FAILURE;
00157 
00158     // get normal of triangle
00159     CartVect v0 = coords[1] - coords[0];
00160     CartVect v1 = coords[2] - coords[0];
00161     CartVect norm = adj_senses[i]*(v0*v1);
00162     double dot_prod = norm%ray_direction;
00163 
00164     // if the sign has not yet been decided, choose it
00165     if(0==sign && 0!=dot_prod) {
00166       if(0<dot_prod) sign = 1;
00167       else           sign = -1;
00168     }
00169 
00170     // intersection is glancing if tri and ray do not point in same direction
00171     // for every triangle
00172     if(0!=sign && 0>sign*dot_prod) return false;
00173 
00174   }
00175   return true;
00176 
00177 }
00178 
00179 #if defined(MB_OBB_USE_VECTOR_QUERIES) && defined(MB_OBB_USE_TYPE_QUERIES)
00180 # undef MB_OBB_USE_TYPE_QUERIES
00181 #endif
00182 
00183 const char DEFAULT_TAG_NAME[] = "OBB";
00184 
00185 OrientedBoxTreeTool::Op::~Op() {}
00186 
00187 OrientedBoxTreeTool::OrientedBoxTreeTool( Interface* i,
00188                                               const char* tag_name,
00189                                               bool destroy_created_trees )
00190   : instance( i ), cleanUpTrees(destroy_created_trees)
00191 {
00192   if (!tag_name)
00193     tag_name = DEFAULT_TAG_NAME;
00194   ErrorCode rval = OrientedBox::tag_handle( tagHandle, instance, tag_name);
00195   if (MB_SUCCESS != rval)
00196     tagHandle = 0;
00197 }
00198 
00199 OrientedBoxTreeTool::~OrientedBoxTreeTool()
00200 {
00201   if (!cleanUpTrees)
00202     return;
00203     
00204   while (!createdTrees.empty()) {
00205     EntityHandle tree = createdTrees.back();
00206       // make sure this is a tree (rather than some other, stale handle)
00207     const void* data_ptr = 0;
00208     ErrorCode rval = instance->tag_get_by_ptr( tagHandle, &tree, 1, &data_ptr );
00209     if (MB_SUCCESS == rval)
00210       rval = delete_tree( tree );
00211     if (MB_SUCCESS != rval)
00212       createdTrees.pop_back();
00213   }
00214 }
00215 
00216 OrientedBoxTreeTool::Settings::Settings() 
00217   : max_leaf_entities( 8 ),
00218     max_depth( 0 ),
00219     worst_split_ratio( 0.95 ),
00220     best_split_ratio( 0.4 ),
00221     set_options( MESHSET_SET )
00222   {}
00223 
00224 bool OrientedBoxTreeTool::Settings::valid() const
00225 {
00226   return max_leaf_entities > 0 
00227       && max_depth >= 0
00228       && worst_split_ratio <= 1.0
00229       && best_split_ratio >= 0.0
00230       && worst_split_ratio >= best_split_ratio
00231       ;
00232 }
00233 
00234 
00235 /********************** Simple Tree Access Methods ****************************/
00236 
00237 
00238 ErrorCode OrientedBoxTreeTool::box( EntityHandle set, OrientedBox& obb )
00239 {
00240   return instance->tag_get_data( tagHandle, &set, 1, &obb );
00241 }
00242 
00243 ErrorCode OrientedBoxTreeTool::box( EntityHandle set,
00244                                         double center[3],
00245                                         double axis1[3],
00246                                         double axis2[3],
00247                                         double axis3[3] )
00248 {
00249   OrientedBox obb;
00250   ErrorCode rval = this->box( set, obb );
00251   obb.center.get( center );
00252   obb.scaled_axis(0).get( axis1 );
00253   obb.scaled_axis(1).get( axis2 );
00254   obb.scaled_axis(2).get( axis3 );
00255   return rval;
00256 }
00257 
00258 
00259 /********************** Tree Construction Code ****************************/
00260 
00261 
00262 struct OrientedBoxTreeTool::SetData {
00263   EntityHandle handle;
00264   OrientedBox::CovarienceData box_data;
00265   //Range vertices;
00266 };
00267 
00268 
00269 ErrorCode OrientedBoxTreeTool::build( const Range& entities,
00270                                           EntityHandle& set_handle_out,
00271                                           const Settings* settings )
00272 {
00273   if (!entities.all_of_dimension(2))
00274     return MB_TYPE_OUT_OF_RANGE;
00275   if (settings && !settings->valid())
00276     return MB_FAILURE;
00277     
00278   return build_tree( entities, set_handle_out, 0, 
00279                      settings ? *settings : Settings() );
00280 }
00281 
00282 ErrorCode OrientedBoxTreeTool::join_trees( const Range& sets,
00283                                                EntityHandle& set_handle_out,
00284                                                const Settings* settings )
00285 {
00286   if (!sets.all_of_type(MBENTITYSET))
00287     return MB_TYPE_OUT_OF_RANGE;
00288   if (settings && !settings->valid())
00289     return MB_FAILURE;
00290   
00291     // Build initial set data list.
00292   std::list<SetData> data;
00293   for (Range::const_iterator i = sets.begin(); i != sets.end(); ++i) {
00294     Range elements;
00295     ErrorCode rval = instance->get_entities_by_dimension( *i, 2, elements, true );
00296     if (MB_SUCCESS != rval)
00297       return rval;
00298     if (elements.empty())
00299       continue;
00300     
00301     data.push_back( SetData() );
00302     SetData& set_data = data.back();
00303     set_data.handle = *i;
00304     rval = OrientedBox::covariance_data_from_tris( set_data.box_data, instance, elements );
00305     if (MB_SUCCESS != rval)
00306       return rval;
00307   }
00308 
00309   ErrorCode result = build_sets( data, set_handle_out, 0, 
00310                           settings ? *settings : Settings() );
00311   if (MB_SUCCESS != result)
00312     return result;
00313   
00314   for (Range::reverse_iterator i = sets.rbegin(); i != sets.rend(); ++i) {
00315     createdTrees.erase(
00316       std::remove( createdTrees.begin(), createdTrees.end(), *i ), 
00317       createdTrees.end() );
00318   }
00319   createdTrees.push_back( set_handle_out );
00320   return MB_SUCCESS;
00321 }
00322   
00323 
00336 static ErrorCode split_box( Interface* instance, 
00337                               const OrientedBox& box, 
00338                               int axis, 
00339                               const Range& entities, 
00340                               Range& left_list, 
00341                               Range& right_list )
00342 {
00343   ErrorCode rval;
00344   left_list.clear();
00345   right_list.clear();
00346 
00347   std::vector<CartVect> coords;
00348   for (Range::reverse_iterator i = entities.rbegin(); i != entities.rend(); ++i) {
00349     const EntityHandle *conn;
00350     int conn_len;
00351     rval = instance->get_connectivity( *i, conn, conn_len );
00352     if (MB_SUCCESS != rval)
00353       return rval;
00354     
00355     coords.resize( conn_len );
00356     rval = instance->get_coords( conn, conn_len, coords[0].array() );
00357     if (MB_SUCCESS != rval)
00358       return rval;
00359     
00360     CartVect centroid(0.0);
00361     for (int j = 0; j < conn_len; ++j)
00362       centroid += coords[j];
00363     centroid /= conn_len;
00364     
00365     if ((box.axis[axis] % (centroid - box.center)) < 0.0)
00366       left_list.insert( *i );
00367     else
00368       right_list.insert( *i );
00369   }
00370   
00371   return MB_SUCCESS;
00372 }
00373 
00374 
00375 ErrorCode OrientedBoxTreeTool::build_tree( const Range& entities,
00376                                                EntityHandle& set,
00377                                                int depth,
00378                                                const Settings& settings )
00379 {
00380   OrientedBox tmp_box;
00381   ErrorCode rval;
00382   
00383   if (entities.empty()) {
00384     CartVect axis[3] = { CartVect(0.), CartVect(0.), CartVect(0.) };
00385     tmp_box = OrientedBox( axis, CartVect(0.) );
00386   }
00387   else {
00388     rval = OrientedBox::compute_from_2d_cells( tmp_box, instance, entities );
00389     if (MB_SUCCESS != rval)
00390       return rval;
00391   }
00392   
00393     // create an entity set for the tree node
00394   rval = instance->create_meshset( settings.set_options, set );
00395   if (MB_SUCCESS != rval)
00396     return rval;
00397   
00398   rval = instance->tag_set_data( tagHandle, &set, 1, &tmp_box );
00399   if (MB_SUCCESS != rval) 
00400     { delete_tree( set ); return rval; }
00401   
00402     // check if should create children
00403   bool leaf = true;
00404   ++depth;
00405   if ((!settings.max_depth || depth < settings.max_depth) && 
00406       entities.size() > (unsigned)settings.max_leaf_entities) {
00407       // try splitting with planes normal to each axis of the box
00408       // until we find an acceptable split
00409     double best_ratio = settings.worst_split_ratio; // worst case ratio
00410     Range best_left_list, best_right_list;
00411       // Axes are sorted from shortest to longest, so search backwards
00412     for (int axis = 2; best_ratio > settings.best_split_ratio && axis >= 0; --axis) {
00413       Range left_list, right_list;
00414 
00415       rval = split_box( instance, tmp_box, axis, entities, left_list, right_list );
00416       if (MB_SUCCESS != rval) 
00417         { delete_tree( set ); return rval; }
00418         
00419       double ratio = fabs((double)right_list.size() - left_list.size()) / entities.size();
00420       
00421       if (ratio < best_ratio) {
00422         best_ratio = ratio;
00423         best_left_list.swap( left_list );
00424         best_right_list.swap( right_list );
00425       }
00426     }
00427     
00428       // create children
00429     if (!best_left_list.empty())
00430     {
00431       EntityHandle child = 0;
00432       
00433       rval = build_tree( best_left_list, child, depth, settings );
00434       if (MB_SUCCESS != rval)
00435         { delete_tree( set ); return rval; }
00436       rval = instance->add_child_meshset( set, child );
00437       if (MB_SUCCESS != rval)
00438         { delete_tree( set ); delete_tree( child ); return rval; }
00439       
00440       rval = build_tree( best_right_list, child, depth, settings );
00441       if (MB_SUCCESS != rval)
00442         { delete_tree( set ); return rval; }
00443       rval = instance->add_child_meshset( set, child );
00444       if (MB_SUCCESS != rval)
00445         { delete_tree( set ); delete_tree( child ); return rval; }
00446       
00447       leaf = false;
00448     }
00449   }
00450   
00451   if (leaf)
00452   {
00453     rval = instance->add_entities( set, entities );
00454     if (MB_SUCCESS != rval) 
00455       { delete_tree( set ); return rval; }
00456   }
00457   
00458   createdTrees.push_back( set );
00459   return MB_SUCCESS;
00460 }
00461 
00462 
00463 static ErrorCode split_sets( Interface* , 
00464                                const OrientedBox& box, 
00465                                int axis, 
00466                                const std::list<OrientedBoxTreeTool::SetData>& sets,
00467                                std::list<OrientedBoxTreeTool::SetData>& left,
00468                                std::list<OrientedBoxTreeTool::SetData>& right )
00469 {
00470   left.clear();
00471   right.clear();
00472 
00473   std::list<OrientedBoxTreeTool::SetData>::const_iterator i;
00474   for (i = sets.begin(); i != sets.end(); ++i) {
00475     CartVect centroid(i->box_data.center / i->box_data.area);
00476     if ((box.axis[axis] % (centroid - box.center)) < 0.0)
00477       left.push_back( *i );
00478     else
00479       right.push_back( *i );
00480   }
00481   
00482   return MB_SUCCESS;
00483 }
00484 
00485 
00486 ErrorCode OrientedBoxTreeTool::build_sets( std::list<SetData>& sets,
00487                                                EntityHandle& node_set,
00488                                                int depth,
00489                                                const Settings& settings )
00490 {
00491   ErrorCode rval;
00492   int count = sets.size();
00493   if (0 == count)
00494     return MB_FAILURE;
00495   
00496     // calculate box
00497   OrientedBox obox;
00498 
00499   // make vector go out of scope when done, so memory is released
00500   { 
00501     Range elems;
00502     std::vector<OrientedBox::CovarienceData> data(sets.size());
00503     data.clear();
00504     for (std::list<SetData>::iterator i = sets.begin(); i != sets.end(); ++i) {
00505       data.push_back( i->box_data );
00506       rval = instance->get_entities_by_dimension( i->handle, 2, elems, true );
00507       if (MB_SUCCESS != rval)
00508         return rval;
00509     }
00510     
00511     Range points;
00512     rval = instance->get_adjacencies( elems, 0, false, points, Interface::UNION );
00513     if (MB_SUCCESS != rval)
00514       return rval;
00515     
00516     rval = OrientedBox::compute_from_covariance_data( obox, instance, &data[0], data.size(), points );
00517     if (MB_SUCCESS != rval)
00518       return rval;
00519   }
00520   
00521     // If only one set in list...
00522   if (count == 1) {
00523     node_set = sets.front().handle;
00524     return instance->tag_set_data( tagHandle, &node_set, 1, &obox );
00525   }
00526   
00527     // create an entity set for the tree node
00528   rval = instance->create_meshset( settings.set_options, node_set );
00529   if (MB_SUCCESS != rval)
00530     return rval;
00531   
00532   rval = instance->tag_set_data( tagHandle, &node_set, 1, &obox );
00533   if (MB_SUCCESS != rval) 
00534     { delete_tree( node_set ); return rval; }
00535   
00536   double best_ratio = 2.0; 
00537   std::list<SetData> best_left_list, best_right_list;
00538   for (int axis = 0; axis < 2; ++axis) {
00539     std::list<SetData> left_list, right_list;
00540     rval = split_sets( instance, obox, axis, sets, left_list, right_list );
00541     if (MB_SUCCESS != rval) 
00542       { delete_tree( node_set ); return rval; }
00543 
00544     double ratio = fabs((double)right_list.size() - left_list.size()) / sets.size();
00545 
00546     if (ratio < best_ratio) {
00547       best_ratio = ratio;
00548       best_left_list.swap( left_list );
00549       best_right_list.swap( right_list );
00550     }
00551   }
00552   
00553     // We must subdivide the list of sets, because we want to guarantee that
00554     // there is a node in the tree corresponding to each of the sets.  So if
00555     // we couldn't find a usable split plane, just split them in an arbitrary
00556     // manner.
00557   if (best_left_list.empty() || best_right_list.empty()) {
00558     best_left_list.clear();
00559     best_right_list.clear();
00560     std::list<SetData>* lists[2] = {&best_left_list,&best_right_list};
00561     int i = 0;
00562     while (!sets.empty()) {
00563       lists[i]->push_back( sets.front() );
00564       sets.pop_front();
00565       i = 1 - i;
00566     }
00567   }
00568   else {
00569     sets.clear(); // release memory before recursion
00570   }
00571   
00572     // Create child sets
00573     
00574   EntityHandle child = 0;
00575       
00576   rval = build_sets( best_left_list, child, depth+1, settings );
00577   if (MB_SUCCESS != rval)
00578     { delete_tree( node_set ); return rval; }
00579   rval = instance->add_child_meshset( node_set, child );
00580   if (MB_SUCCESS != rval)
00581     { delete_tree( node_set ); delete_tree( child ); return rval; }
00582 
00583   rval = build_sets( best_right_list, child, depth+1, settings );
00584   if (MB_SUCCESS != rval)
00585     { delete_tree( node_set ); return rval; }
00586   rval = instance->add_child_meshset( node_set, child );
00587   if (MB_SUCCESS != rval)
00588     { delete_tree( node_set ); delete_tree( child ); return rval; }
00589   
00590   return MB_SUCCESS;
00591 }
00592 
00593 ErrorCode OrientedBoxTreeTool::delete_tree( EntityHandle set )
00594 {
00595   std::vector<EntityHandle> children;
00596   ErrorCode rval = instance->get_child_meshsets( set, children, 0 );
00597   if (MB_SUCCESS != rval)
00598     return rval;
00599   
00600   createdTrees.erase( 
00601     std::remove( createdTrees.begin(), createdTrees.end(), set ),
00602     createdTrees.end() );
00603   children.insert( children.begin(), set );
00604   return instance->delete_entities( &children[0], children.size() );
00605 }
00606 
00607 
00608 /********************** Generic Tree Traversal ****************************/
00609 struct Data { EntityHandle set; int depth; };
00610 ErrorCode OrientedBoxTreeTool::preorder_traverse( EntityHandle set,
00611                                                       Op& operation, 
00612                                                       TrvStats* accum )
00613 {
00614   ErrorCode rval;
00615   std::vector<EntityHandle> children;
00616   std::vector<Data> the_stack;
00617   Data data = { set, 0 };
00618   the_stack.push_back( data );
00619   int max_depth = -1;
00620   
00621   while (!the_stack.empty())
00622   {
00623     data = the_stack.back();
00624     the_stack.pop_back();
00625     
00626     // increment traversal statistics
00627     if( accum ){
00628       accum->increment( data.depth );
00629       max_depth = std::max( max_depth, data.depth );
00630     }
00631 
00632     bool descend = true;
00633     rval = operation.visit( data.set, data.depth, descend );
00634     assert(MB_SUCCESS == rval);
00635     if (MB_SUCCESS != rval)
00636       return rval;
00637     
00638     if (!descend)
00639       continue;
00640     
00641     children.clear();
00642     rval = instance->get_child_meshsets( data.set, children );
00643     assert(MB_SUCCESS == rval);
00644     if (MB_SUCCESS != rval)
00645       return rval;
00646     if (children.empty()) {
00647       if( accum ){ accum->increment_leaf( data.depth ); }
00648       rval = operation.leaf( data.set );
00649       assert(MB_SUCCESS == rval);
00650       if (MB_SUCCESS != rval)
00651         return rval;
00652     }
00653     else if (children.size() == 2) {
00654       data.depth++;
00655       data.set = children[0];
00656       the_stack.push_back( data );
00657       data.set = children[1];
00658       the_stack.push_back( data );
00659     }
00660     else
00661       return MB_MULTIPLE_ENTITIES_FOUND;
00662   }
00663   
00664   if( accum ){
00665     accum->end_traversal( max_depth );
00666   }
00667 
00668   return MB_SUCCESS;
00669 }
00670 
00671 /********************** General Sphere/Triangle Intersection ***************/
00672 
00673 struct OBBTreeSITFrame { 
00674   OBBTreeSITFrame( EntityHandle n, EntityHandle s, int dp )
00675     : node(n), surf(s), depth(dp) {}
00676   EntityHandle node;
00677   EntityHandle surf;
00678   int depth;
00679 };
00680 
00681 ErrorCode OrientedBoxTreeTool::sphere_intersect_triangles( 
00682                                         const double* center_v,
00683                                         double radius,
00684                                         EntityHandle tree_root,
00685                                         std::vector<EntityHandle>& facets_out,
00686                                         std::vector<EntityHandle>* sets_out, 
00687                                         TrvStats* accum )
00688 {
00689   const double radsqr = radius * radius;
00690   OrientedBox b;
00691   ErrorCode rval;
00692   Range sets;
00693   const CartVect center(center_v);
00694   CartVect closest, coords[3];
00695   const EntityHandle* conn;
00696   int num_conn;
00697 #ifndef MB_OBB_USE_VECTOR_QUERIES
00698   Range tris;
00699   Range::const_iterator t;
00700 #else
00701   std::vector<EntityHandle> tris;
00702   std::vector<EntityHandle>::const_iterator t;
00703 #endif
00704   
00705   std::vector<OBBTreeSITFrame> stack;
00706   std::vector<EntityHandle> children;
00707   stack.reserve(30);
00708   stack.push_back( OBBTreeSITFrame( tree_root, 0, 0 ) );
00709 
00710   int max_depth = -1;
00711 
00712   while (!stack.empty()) {
00713     EntityHandle surf = stack.back().surf; 
00714     EntityHandle node = stack.back().node;
00715     int current_depth   = stack.back().depth;
00716     stack.pop_back();
00717     
00718       // increment traversal statistics.  
00719     if( accum ){
00720       accum->increment( current_depth );
00721       max_depth = std::max( max_depth, current_depth );
00722     }
00723 
00724     if (!surf && sets_out) {
00725       rval = get_moab_instance()->get_entities_by_type( node, MBENTITYSET, sets );
00726       if (!sets.empty())
00727         surf = sets.front();
00728       sets.clear();
00729     }
00730     
00731       // check if sphere intersects box
00732     rval = box( node, b );
00733     if (MB_SUCCESS != rval)
00734       return rval;
00735     b.closest_location_in_box( center, closest );
00736     closest -= center;
00737     if ((closest % closest) > radsqr)
00738       continue;
00739     
00740       // push child boxes on stack
00741     children.clear();
00742     rval = instance->get_child_meshsets( node, children );
00743     if (MB_SUCCESS != rval)
00744       return rval;
00745     if (!children.empty()) {
00746       assert(children.size() == 2);
00747       stack.push_back( OBBTreeSITFrame( children[0], surf, current_depth + 1 ) );
00748       stack.push_back( OBBTreeSITFrame( children[1], surf, current_depth + 1 ) );
00749       continue;
00750     }
00751     
00752     if(accum){ accum->increment_leaf( current_depth ); }
00753       // if leaf, intersect sphere with triangles
00754 #ifndef MB_OBB_USE_VECTOR_QUERIES
00755 # ifdef MB_OBB_USE_TYPE_QUERIES
00756     rval = get_moab_instance()->get_entities_by_type( node, MBTRI, tris );
00757 # else
00758     rval = get_moab_instance()->get_entities_by_handle( node, tris );
00759 # endif
00760     t = tris.begin();
00761 #else
00762     rval = get_moab_instance()->get_entities_by_handle( node, tris );
00763     t = tris.lower_bound( MBTRI );
00764 #endif
00765     if (MB_SUCCESS != rval)
00766       return rval;
00767     
00768     for (t = tris.begin(); t != tris.end(); ++t) {
00769 #ifndef MB_OBB_USE_VECTOR_QUERIES
00770       if (TYPE_FROM_HANDLE(*t) != MBTRI)
00771         break;
00772 #elif !defined(MB_OBB_USE_TYPE_QUERIES)
00773       if (TYPE_FROM_HANDLE(*t) != MBTRI)
00774         continue;
00775 #endif      
00776       rval = get_moab_instance()->get_connectivity( *t, conn, num_conn, true );
00777       if (MB_SUCCESS != rval)
00778         return rval;
00779       if (num_conn != 3)
00780         continue;
00781       
00782       rval = get_moab_instance()->get_coords( conn, num_conn, coords[0].array() );
00783       if (MB_SUCCESS != rval)
00784         return rval;
00785       
00786       GeomUtil::closest_location_on_tri( center, coords, closest );
00787       closest -= center;
00788       if ((closest % closest) <= radsqr &&
00789           std::find(facets_out.begin(),facets_out.end(),*t) == facets_out.end()) {
00790         facets_out.push_back( *t );
00791         if (sets_out)
00792           sets_out->push_back( surf );
00793       }
00794     }
00795   }
00796 
00797   if( accum ){
00798     accum->end_traversal( max_depth );
00799   }
00800   
00801   return MB_SUCCESS;
00802 }
00803       
00804 
00805 
00806 /********************** General Ray/Tree and Ray/Triangle Intersection ***************/
00807 
00808 
00809 class RayIntersector : public OrientedBoxTreeTool::Op
00810 {
00811   private:
00812     OrientedBoxTreeTool* tool;
00813     const CartVect b, m;
00814     const double* len;
00815     const double tol;
00816     Range& boxes;
00817     
00818 
00819   public:
00820     RayIntersector( OrientedBoxTreeTool* tool_ptr,
00821                     const double* ray_point,
00822                     const double* unit_ray_dir,
00823                     const double *ray_length,
00824                     double tolerance,
00825                     Range& leaf_boxes )
00826       : tool(tool_ptr),
00827         b(ray_point), m(unit_ray_dir),
00828         len(ray_length), tol(tolerance),
00829         boxes(leaf_boxes) 
00830       { }
00831   
00832     virtual ErrorCode visit( EntityHandle node,
00833                              int          depth,
00834                              bool&        descend );
00835     virtual ErrorCode leaf( EntityHandle node );
00836 };
00837 
00838 //#include <stdio.h>
00839 //inline void dump_fragmentation( const Range& range ) {
00840 //  static FILE* file = fopen( "fragmentation", "w" );
00841 //  unsigned ranges = 0, entities = 0;
00842 //  for (Range::const_pair_iterator i = range.const_pair_begin(); i != range.const_pair_end(); ++i)
00843 //  {
00844 //    ++ranges;
00845 //    entities += i->second - i->first + 1;
00846 //  }
00847 //  fprintf( file, "%u %u\n", ranges, entities );
00848 //}
00849 
00850 ErrorCode OrientedBoxTreeTool::ray_intersect_triangles( 
00851                           std::vector<double>& intersection_distances_out,
00852                           std::vector<EntityHandle>& intersection_facets_out,
00853                           const Range& boxes,
00854                           double tolerance,
00855                           const double ray_point[3],
00856                           const double unit_ray_dir[3],
00857                           const double* ray_length, 
00858                           unsigned int* raytri_test_count )
00859 {
00860   ErrorCode rval;
00861   intersection_distances_out.clear();
00862 #ifdef MB_OBB_USE_VECTOR_QUERIES
00863   std::vector<EntityHandle> tris;
00864 #endif
00865     
00866   const CartVect point( ray_point );
00867   const CartVect dir( unit_ray_dir );
00868   
00869   for (Range::iterator b = boxes.begin(); b != boxes.end(); ++b)
00870   {
00871 #ifndef MB_OBB_USE_VECTOR_QUERIES
00872     Range tris;
00873 # ifdef MB_OBB_USE_TYPE_QUERIES
00874     rval = instance->get_entities_by_type( *b, MBTRI, tris );
00875 # else
00876     rval = instance->get_entities_by_handle( *b, tris );
00877 # endif
00878 #else
00879     tris.clear();
00880     rval = instance->get_entities_by_handle( *b, tris );
00881 #endif
00882     if (MB_SUCCESS != rval)
00883       return rval;
00884 //dump_fragmentation( tris );
00885     
00886 #ifndef MB_OBB_USE_VECTOR_QUERIES
00887     for (Range::iterator t = tris.begin(); t != tris.end(); ++t)
00888 #else
00889     for (std::vector<EntityHandle>::iterator t = tris.begin(); t != tris.end(); ++t)
00890 #endif
00891     {
00892 #ifndef MB_OBB_USE_TYPE_QUERIES
00893       if (TYPE_FROM_HANDLE(*t) != MBTRI)
00894         continue;
00895 #endif
00896     
00897       const EntityHandle* conn;
00898       int len;
00899       rval = instance->get_connectivity( *t, conn, len, true );
00900       if (MB_SUCCESS != rval)
00901         return rval;
00902       
00903       CartVect coords[3];
00904       rval = instance->get_coords( conn, 3, coords[0].array() );
00905       if (MB_SUCCESS != rval)
00906         return rval;
00907       
00908       if( raytri_test_count ) *raytri_test_count += 1; 
00909 
00910       double td;
00911       if (GeomUtil::ray_tri_intersect( coords, point, dir, tolerance, td, ray_length )){
00912         intersection_distances_out.push_back(td);
00913         intersection_facets_out.push_back( *t );
00914       }
00915     }
00916   }
00917   
00918   return MB_SUCCESS;
00919 }                    
00920 
00921 ErrorCode OrientedBoxTreeTool::ray_intersect_triangles( 
00922                           std::vector<double>& intersection_distances_out,
00923                           std::vector<EntityHandle>& intersection_facets_out,
00924                           EntityHandle root_set,
00925                           double tolerance,
00926                           const double ray_point[3],
00927                           const double unit_ray_dir[3],
00928                           const double* ray_length, 
00929                           TrvStats* accum )
00930 {
00931   Range boxes;
00932   ErrorCode rval;
00933   
00934   rval = ray_intersect_boxes( boxes, root_set, tolerance, ray_point, unit_ray_dir, ray_length, accum );
00935   if (MB_SUCCESS != rval)
00936     return rval;
00937     
00938   return ray_intersect_triangles( intersection_distances_out, intersection_facets_out, boxes, 
00939                                   tolerance, ray_point, unit_ray_dir, ray_length, 
00940                                   accum ? &(accum->ray_tri_tests_count) : NULL );
00941 }
00942 
00943 ErrorCode OrientedBoxTreeTool::ray_intersect_boxes( 
00944                           Range& boxes_out,
00945                           EntityHandle root_set,
00946                           double tolerance,
00947                           const double ray_point[3],
00948                           const double unit_ray_dir[3],
00949                           const double* ray_length, 
00950                           TrvStats* accum )
00951 {
00952   RayIntersector op( this, ray_point, unit_ray_dir, ray_length, tolerance, boxes_out );
00953   return preorder_traverse( root_set, op, accum );
00954 }
00955 
00956 ErrorCode RayIntersector::visit( EntityHandle node,
00957                                  int ,
00958                                  bool&        descend ) 
00959 {
00960   OrientedBox box;
00961   ErrorCode rval = tool->box( node, box );
00962   if (MB_SUCCESS != rval)
00963     return rval;
00964   
00965   descend = box.intersect_ray( b, m, tol, len);
00966   return MB_SUCCESS;
00967 }
00968 
00969 
00970 ErrorCode RayIntersector::leaf( EntityHandle node )
00971 {
00972   boxes.insert(node);
00973   return MB_SUCCESS;
00974 }
00975 
00976 
00977 /********************** Ray/Set Intersection ****************************/
00978 
00979 
00980 class RayIntersectSets : public OrientedBoxTreeTool::Op
00981 {
00982   private:
00983     // Input
00984     OrientedBoxTreeTool* tool;
00985     const CartVect       ray_origin;
00986     const CartVect       ray_direction;
00987     const double*        nonneg_ray_len;  /* length to search ahead of ray origin */
00988     const double*        neg_ray_len;     /* length to search behind ray origin */
00989     const double         tol;             /* used for box.intersect_ray, radius of
00990                                              neighborhood for adjacent triangles,
00991                                              and old mode of add_intersection */
00992     const int            minTolInt;       /* used for old mode of add_intersection */
00993 
00994     // Output
00995     std::vector<double>&       intersections;
00996     std::vector<EntityHandle>& sets;
00997     std::vector<EntityHandle>& facets;
00998 
00999     // Optional Input - to screen RTIs by orientation and edge/node intersection
01000     const EntityHandle*  rootSet;         /* used for sphere_intersect */
01001     const EntityHandle*  geomVol;         /* used for determining surface sense */
01002     const Tag*           senseTag;        /* allows screening by triangle orientation.
01003                                              both geomVol and senseTag must be used together. */
01004     const int*           desiredOrient;   /* points to desired orientation of ray with
01005                                              respect to surf normal, if this feature is used.
01006                                              Must point to -1 (reverse) or 1 (forward).
01007                                              geomVol and senseTag are needed for this feature */
01008     int*                 surfTriOrient;   /* holds desired orientation of tri wrt surface */
01009     int                  surfTriOrient_val;
01010 
01011     // Optional Input - to avoid returning these as RTIs
01012     const std::vector<EntityHandle>* prevFacets; /* intersections on these triangles 
01013                                                     will not be returned */
01014 
01015     // Other Variables
01016     unsigned int*        raytri_test_count;
01017     EntityHandle         lastSet;
01018     int                  lastSetDepth;
01019     std::vector< std::vector<EntityHandle> > neighborhoods;
01020     std::vector<EntityHandle> neighborhood;
01021 
01022     void add_intersection( double t, EntityHandle facet );
01023     
01024   public:
01025     RayIntersectSets( OrientedBoxTreeTool*       tool_ptr,
01026                       const double*              ray_point,
01027                       const double*              unit_ray_dir,
01028                       const double*              nonneg_ray_length,
01029               const double*              neg_ray_length,
01030                       double                     tolerance,
01031                       int                        min_tol_intersections,
01032                       std::vector<double>&       inters,
01033                       std::vector<EntityHandle>& surfaces,
01034                       std::vector<EntityHandle>& facts,
01035                   EntityHandle*              root_set,
01036               const EntityHandle*        geom_volume,
01037               const Tag*                 sense_tag,
01038               const int*                 desired_orient,
01039                   const std::vector<EntityHandle>* prev_facets,
01040                       unsigned int*              tmp_count   )
01041       : tool(tool_ptr),
01042         ray_origin(ray_point), ray_direction(unit_ray_dir),
01043         nonneg_ray_len(nonneg_ray_length), neg_ray_len(neg_ray_length), 
01044         tol(tolerance), minTolInt(min_tol_intersections),
01045         intersections(inters), sets(surfaces), facets(facts), 
01046         rootSet(root_set), geomVol(geom_volume), senseTag(sense_tag),
01047         desiredOrient(desired_orient), prevFacets(prev_facets),
01048         raytri_test_count(tmp_count), lastSet(0)
01049       {
01050     // specified orientation should be 1 or -1, indicating ray and surface
01051         // normal in the same or opposite directions, respectively.
01052         if(desiredOrient) {
01053           assert(1==*desiredOrient || -1==*desiredOrient);
01054           surfTriOrient = &surfTriOrient_val;
01055         } else {
01056           surfTriOrient = NULL;
01057         }
01058 
01059         // check the limits  
01060         if(nonneg_ray_len) {
01061           assert(0 <= *nonneg_ray_len);
01062         } 
01063         if(neg_ray_len) {
01064           assert(0 > *neg_ray_len);
01065         }
01066 
01067       }
01068    
01069 
01070     virtual ErrorCode visit( EntityHandle node,
01071                              int          depth,
01072                  bool&        descend );
01073     virtual ErrorCode leaf( EntityHandle node );
01074 };
01075 
01076 ErrorCode RayIntersectSets::visit( EntityHandle node,
01077                                    int          depth,
01078                    bool&        descend ) 
01079 {
01080   OrientedBox box;
01081   ErrorCode rval = tool->box( node, box );
01082   assert(MB_SUCCESS == rval);
01083   if (MB_SUCCESS != rval)
01084     return rval;
01085   
01086   descend = box.intersect_ray( ray_origin, ray_direction, tol, nonneg_ray_len, 
01087                                neg_ray_len );
01088 
01089   if (lastSet && depth <= lastSetDepth)
01090     lastSet = 0;
01091 
01092   if (descend && !lastSet) {
01093     Range tmp_sets;
01094     rval = tool->get_moab_instance()->get_entities_by_type( node, MBENTITYSET, tmp_sets );
01095     assert(MB_SUCCESS == rval);
01096     if (MB_SUCCESS != rval)
01097       return rval;
01098  
01099     if (!tmp_sets.empty()) {
01100       if (tmp_sets.size() > 1)
01101         return MB_FAILURE;
01102       lastSet = *tmp_sets.begin();
01103       lastSetDepth = depth;
01104       // Get desired orientation of surface wrt volume. Use this to return only 
01105       // exit or entrance intersections.
01106       if(geomVol && senseTag && desiredOrient && surfTriOrient) {
01107         if(1!=*desiredOrient && -1!=*desiredOrient) {
01108       std::cerr << "error: desired orientation must be 1 (forward) or -1 (reverse)" 
01109                     << std::endl;
01110         }
01111     EntityHandle vols[2];
01112     rval = tool->get_moab_instance()->tag_get_data( *senseTag, &lastSet, 1, vols );
01113     assert(MB_SUCCESS == rval);
01114     if(MB_SUCCESS != rval) return rval;
01115     if(vols[0] == vols[1]) {
01116       std::cerr << "error: surface has positive and negative sense wrt same volume" 
01117                     << std::endl;
01118       return MB_FAILURE;
01119     }
01120         // surfTriOrient will be used by plucker_ray_tri_intersect to avoid
01121         // intersections with wrong orientation.
01122     if       (*geomVol == vols[0]) {
01123       *surfTriOrient = *desiredOrient*1;
01124     } else if(*geomVol == vols[1]) {
01125       *surfTriOrient = *desiredOrient*(-1);
01126     } else {
01127       assert(false);
01128       return MB_FAILURE;
01129     }
01130       }
01131     }
01132   }
01133     
01134   return MB_SUCCESS;
01135 }
01136 
01137 ErrorCode RayIntersectSets::leaf( EntityHandle node )
01138 {
01139   assert(lastSet);
01140   if (!lastSet) // if no surface has been visited yet, something's messed up.
01141     return MB_FAILURE;
01142   
01143 #ifndef MB_OBB_USE_VECTOR_QUERIES
01144   Range tris;
01145 # ifdef MB_OBB_USE_TYPE_QUERIES
01146   ErrorCode rval = tool->get_moab_instance()->get_entities_by_type( node, MBTRI, tris );
01147 # else
01148   ErrorCode rval = tool->get_moab_instance()->get_entities_by_handle( node, tris );
01149 # endif
01150 #else
01151   std::vector<EntityHandle> tris;
01152   ErrorCode rval = tool->get_moab_instance()->get_entities_by_handle( node, tris );
01153 #endif
01154   assert(MB_SUCCESS == rval);
01155   if (MB_SUCCESS != rval)
01156     return rval;
01157 
01158 #ifndef MB_OBB_USE_VECTOR_QUERIES
01159   for (Range::iterator t = tris.begin(); t != tris.end(); ++t)
01160 #else
01161   for (std::vector<EntityHandle>::iterator t = tris.begin(); t != tris.end(); ++t)
01162 #endif
01163   {
01164 #ifndef MB_OBB_USE_TYPE_QUERIES
01165     if (TYPE_FROM_HANDLE(*t) != MBTRI)
01166       continue;
01167 #endif
01168     
01169     const EntityHandle* conn;
01170     int num_conn;
01171     rval = tool->get_moab_instance()->get_connectivity( *t, conn, num_conn, true );
01172     assert(MB_SUCCESS == rval);
01173     if (MB_SUCCESS != rval)
01174       return rval;
01175 
01176     CartVect coords[3];
01177     rval = tool->get_moab_instance()->get_coords( conn, 3, coords[0].array() );
01178     assert(MB_SUCCESS == rval);
01179     if (MB_SUCCESS != rval)
01180       return rval;
01181 
01182     if( raytri_test_count ) *raytri_test_count += 1; 
01183 
01184     double int_dist;
01185     GeomUtil::intersection_type int_type = GeomUtil::NONE;
01186     // Note: tol is not used in the ray-tri test.
01187     if (GeomUtil::plucker_ray_tri_intersect( coords, ray_origin, ray_direction, tol, int_dist, 
01188                                      nonneg_ray_len, neg_ray_len, surfTriOrient, &int_type )) {
01189       // Do not accept intersections if they are in the vector of previously intersected
01190       // facets.
01191       if( prevFacets &&
01192       ((*prevFacets).end() != find((*prevFacets).begin(), (*prevFacets).end(), *t) ) ) continue;
01193 
01194       // Do not accept intersections if they are in the neighborhood of previous
01195       // intersections.
01196       bool same_neighborhood = false;
01197       for(unsigned i=0; i<neighborhoods.size(); ++i) {
01198         if( neighborhoods[i].end() != find(neighborhoods[i].begin(), 
01199                        neighborhoods[i].end(), *t ) ) {
01200           same_neighborhood = true;
01201           continue;
01202         }
01203       }
01204       if(same_neighborhood) continue;
01205 
01206       // Handle special case of edge/node intersection. Accept piercing 
01207       // intersections and reject glancing intersections.
01208       // The edge_node_intersection function needs to know surface sense wrt volume.
01209       // A less-robust implementation could work without sense information.
01210       // Would it ever be useful to accept a glancing intersection?
01211       if(GeomUtil::INTERIOR != int_type && rootSet && geomVol && senseTag) {
01212         // get triangles in the proximity of the intersection
01213         CartVect int_pt = ray_origin + int_dist*ray_direction;
01214     std::vector<EntityHandle> close_tris;
01215     std::vector<EntityHandle> close_surfs;
01216         rval = tool->sphere_intersect_triangles( int_pt.array(), tol, *rootSet,
01217                                              close_tris, &close_surfs );
01218         assert(MB_SUCCESS == rval);
01219         if(MB_SUCCESS != rval) return rval; 
01220 
01221         // for each surface, get the surf sense wrt parent volume
01222     std::vector<int> close_senses(close_surfs.size());
01223         for(unsigned i=0; i<close_surfs.size(); ++i) {
01224           EntityHandle vols[2];
01225           rval = tool->get_moab_instance()->tag_get_data( *senseTag, &lastSet, 1, vols );
01226           assert(MB_SUCCESS == rval);
01227           if(MB_SUCCESS != rval) return rval;
01228           if(vols[0] == vols[1]) {
01229             std::cerr << "error: surf has positive and negative sense wrt same volume" << std::endl;
01230             return MB_FAILURE;
01231           }
01232           if       (*geomVol == vols[0]) {
01233             close_senses[i] = 1;
01234           } else if(*geomVol == vols[1]) {
01235             close_senses[i] = -1;
01236           } else {
01237             return MB_FAILURE;
01238           }
01239         }
01240 
01241         neighborhood.clear();
01242     bool piercing = edge_node_intersect( *t, ray_direction, int_type, close_tris,
01243                                              close_senses, tool->get_moab_instance(), &neighborhood );
01244         if(!piercing) continue;
01245 
01246       } else {
01247         neighborhood.clear();
01248     neighborhood.push_back( *t );
01249       }      
01250      
01251         // NOTE: add_intersection may modify the 'neg_ray_len' and 'nonneg_ray_len'
01252         //       members, which will affect subsequent calls to ray_tri_intersect 
01253         //       in this loop.
01254       add_intersection( int_dist, *t );
01255     }
01256   }
01257   return MB_SUCCESS;
01258 }
01259 
01260 /* Mode 1: Used if neg_ray_len and nonneg_ray_len are specified
01261      variables used:     nonneg_ray_len, neg_ray_len
01262      variables not used: min_tol_int, tol
01263      1) keep the closest nonneg intersection and one negative intersection, if closer
01264 
01265    Mode 2: Used if neg_ray_len is not specified
01266      variables used:     min_tol_int, tol, nonneg_ray_len
01267      variables not used: neg_ray_len
01268      1) if(min_tol_int<0) return all intersections
01269      2) otherwise return all inside tolerance and unless there are >min_tol_int
01270      inside of tolerance, return the closest outside of tolerance */
01271 void RayIntersectSets::add_intersection( double t, EntityHandle facet ) {
01272 
01273   // Mode 1, detected by non-null neg_ray_len pointer
01274   // keep the closest nonneg intersection and one negative intersection, if closer
01275   if(neg_ray_len && nonneg_ray_len) {
01276     if(2 != intersections.size()) {
01277       intersections.resize(2,0);
01278       sets.resize(2,0);
01279       facets.resize(2,0);
01280       // must initialize this for comparison below
01281       intersections[0] = -std::numeric_limits<double>::max();
01282     }
01283 
01284     // negative case
01285     if(0.0>t) {
01286       intersections[0] = t;
01287       sets[0]          = lastSet;
01288       facets[0]        = facet;
01289       neg_ray_len      = &intersections[0];
01290     // nonnegative case
01291     } else {
01292       intersections[1] = t;
01293       sets[1]          = lastSet;
01294       facets[1]        = facet;
01295       nonneg_ray_len   = &intersections[1];
01296       // if the intersection is closer than the negative one, remove the negative one
01297       if(t < -(*neg_ray_len) ) {
01298         intersections[0] = -intersections[1];
01299         sets[0]          = 0;
01300         facets[0]        = 0;
01301         neg_ray_len      = &intersections[0]; 
01302       }
01303     }
01304     //    std::cout << "add_intersection: t = " << t << " neg_ray_len=" << *neg_ray_len
01305     //          << " nonneg_ray_len=" << *nonneg_ray_len << std::endl;
01306     return;
01307   }
01308 
01309   // ---------------------------------------------------------------------------
01310   // Mode 2
01311   // If minTolInt is less than zero, return all intersections
01312   if (minTolInt < 0 && t > -tol) {
01313     intersections.push_back(t);
01314     sets.push_back(lastSet);
01315     facets.push_back(facet);
01316     neighborhoods.push_back(neighborhood);
01317     return;
01318   }
01319 
01320     // Check if the 'len' pointer is pointing into the intersection
01321     // list.  If this is the case, then the list contains, at that
01322     // location, an intersection greater than the tolerance away from
01323     // the base point of the ray.
01324   int len_idx = -1;
01325   if (nonneg_ray_len && nonneg_ray_len >= &intersections[0] && 
01326       nonneg_ray_len < &intersections[0] + intersections.size())
01327     len_idx = nonneg_ray_len - &intersections[0];
01328 
01329     // If the intersection is within tol of the ray base point, we 
01330     // always add it to the list.
01331   if (t <= tol) {
01332       // If the list contains an intersection outside the tolerance...
01333     if (len_idx >= 0) {
01334         // If we no longer want an intersection outside the tolerance,
01335         // remove it.
01336       if ((int)intersections.size() >= minTolInt) {
01337         intersections[len_idx] = t;
01338         sets[len_idx] = lastSet;
01339         facets[len_idx] = facet;
01340           // From now on, we want only intersections within the tolerance,
01341           // so update length accordingly
01342         nonneg_ray_len = &tol;
01343       }
01344         // Otherwise appended to the list and update pointer
01345       else {
01346         intersections.push_back(t);
01347         sets.push_back(lastSet);
01348         facets.push_back(facet);
01349         nonneg_ray_len = &intersections[len_idx];
01350       }
01351     }
01352       // Otherwise just append it
01353     else {
01354       intersections.push_back(t);
01355       sets.push_back(lastSet);
01356       facets.push_back(facet);
01357         // If we have all the intersections we want, set
01358         // length such that we will only find further intersections
01359         // within the tolerance
01360       if ((int)intersections.size() >= minTolInt)
01361         nonneg_ray_len = &tol;
01362     }
01363   }
01364     // Otherwise the intersection is outside the tolerance
01365     // If we already have an intersection outside the tolerance and
01366     // this one is closer, replace the existing one with this one.
01367   else if (len_idx >= 0) {
01368     if (t <= *nonneg_ray_len) {
01369       intersections[len_idx] = t;
01370       sets[len_idx] = lastSet;
01371       facets[len_idx] = facet;
01372     }
01373   }
01374     // Otherwise if we want an intersection outside the tolerance
01375     // and don'thave one yet, add it.
01376   else if ((int)intersections.size() < minTolInt) {
01377     intersections.push_back( t );
01378     sets.push_back( lastSet );
01379     facets.push_back(facet);
01380       // update length.  this is currently the closest intersection, so
01381       // only want further intersections that are closer than this one.
01382     nonneg_ray_len = &intersections.back();
01383   }
01384 }
01385   
01386 
01387 ErrorCode OrientedBoxTreeTool::ray_intersect_sets( 
01388                                     std::vector<double>&             distances_out,
01389                                     std::vector<EntityHandle>&       sets_out,
01390                                     std::vector<EntityHandle>&       facets_out,
01391                                     EntityHandle                     root_set,
01392                                     double                           tolerance,
01393                                     int                              min_tolerace_intersections,
01394                                     const double                     ray_point[3],
01395                                     const double                     unit_ray_dir[3],
01396                                     const double*                    nonneg_ray_length,
01397                                     TrvStats*                        accum,
01398                                     const double*                    neg_ray_length,
01399                                     const EntityHandle*              geom_vol,
01400                                     const Tag*                       sense_tag,
01401                                     const int*                       desired_orient,
01402                                     const std::vector<EntityHandle>* prev_facets )
01403 {
01404   RayIntersectSets op( this, ray_point, unit_ray_dir, nonneg_ray_length, neg_ray_length, tolerance, 
01405                        min_tolerace_intersections, distances_out, sets_out, facets_out,
01406                        &root_set, geom_vol, sense_tag, desired_orient, prev_facets, 
01407                        accum ? &(accum->ray_tri_tests_count) : NULL ); 
01408   return preorder_traverse( root_set, op, accum );
01409 }
01410 
01411 
01412 
01413 /********************** Closest Point code ***************/
01414 
01415 struct OBBTreeCPFrame {
01416   OBBTreeCPFrame( double d, EntityHandle n, EntityHandle s, int dp )
01417     : dist_sqr(d), node(n), mset(s), depth(dp) {}
01418   double dist_sqr;
01419   EntityHandle node;
01420   EntityHandle mset;
01421   int depth;
01422 };
01423 
01424 ErrorCode OrientedBoxTreeTool::closest_to_location( 
01425                                      const double* point,
01426                                      EntityHandle root,
01427                                      double* point_out,
01428                                      EntityHandle& facet_out,
01429                                      EntityHandle* set_out,
01430                                      TrvStats* accum ) 
01431 {
01432   ErrorCode rval;
01433   const CartVect loc( point );
01434   double smallest_dist_sqr = std::numeric_limits<double>::max();
01435   
01436   EntityHandle current_set = 0;
01437   Range sets;
01438   std::vector<EntityHandle> children(2);
01439   std::vector<double> coords;
01440   std::vector<OBBTreeCPFrame> stack;
01441   int max_depth = -1;
01442 
01443   stack.push_back( OBBTreeCPFrame(0.0, root, current_set, 0) );
01444   
01445   while( !stack.empty() ) {
01446 
01447       // pop from top of stack
01448     EntityHandle node = stack.back().node;
01449     double dist_sqr = stack.back().dist_sqr;
01450     current_set = stack.back().mset;
01451     int current_depth = stack.back().depth;
01452     stack.pop_back();
01453 
01454       // If current best result is closer than the box, skip this tree node.
01455     if (dist_sqr > smallest_dist_sqr)
01456       continue;
01457 
01458       // increment traversal statistics.  
01459     if( accum ){
01460       accum->increment( current_depth );
01461       max_depth = std::max( max_depth, current_depth );
01462     }
01463 
01464       // Check if this node has a set associated with it
01465     if (set_out && !current_set) {
01466       sets.clear();
01467       rval = instance->get_entities_by_type( node, MBENTITYSET, sets );
01468       if (MB_SUCCESS != rval)
01469         return rval;
01470       if (!sets.empty()) {
01471         if (sets.size() != 1)
01472           return MB_MULTIPLE_ENTITIES_FOUND;
01473         current_set = sets.front();
01474       }
01475     }
01476 
01477       // Get child boxes
01478     children.clear();
01479     rval = instance->get_child_meshsets( node, children );
01480     if (MB_SUCCESS != rval)
01481       return rval;
01482 
01483       // if not a leaf node
01484     if (!children.empty()) {
01485       if (children.size() != 2)
01486         return MB_MULTIPLE_ENTITIES_FOUND;
01487     
01488         // get boxes
01489       OrientedBox box1, box2;
01490       rval = box( children[0], box1 );
01491       if (MB_SUCCESS != rval) return rval;
01492       rval = box( children[1], box2 );
01493       if (MB_SUCCESS != rval) return rval;
01494       
01495         // get distance from each box
01496       CartVect pt1, pt2;
01497       box1.closest_location_in_box( loc, pt1 );
01498       box2.closest_location_in_box( loc, pt2 );
01499       pt1 -= loc;
01500       pt2 -= loc;
01501       const double dsqr1 = pt1 % pt1;
01502       const double dsqr2 = pt2 % pt2;
01503       
01504         // push children on tree such that closer one is on top
01505       if (dsqr1 < dsqr2) {
01506         stack.push_back( OBBTreeCPFrame(dsqr2, children[1], current_set, current_depth+1 ) );
01507         stack.push_back( OBBTreeCPFrame(dsqr1, children[0], current_set, current_depth+1 ) );
01508       }
01509       else {
01510         stack.push_back( OBBTreeCPFrame(dsqr1, children[0], current_set, current_depth+1 ) );
01511         stack.push_back( OBBTreeCPFrame(dsqr2, children[1], current_set, current_depth+1 ) );
01512       }
01513     }
01514     else { // LEAF NODE
01515       if( accum ){ accum->increment_leaf( current_depth ); }
01516 
01517       Range facets;
01518       rval = instance->get_entities_by_dimension( node, 2, facets );
01519       if (MB_SUCCESS != rval)
01520         return rval;
01521       
01522       const EntityHandle* conn;
01523       int len;
01524       CartVect tmp, diff;
01525       for (Range::iterator i = facets.begin(); i != facets.end(); ++i) {
01526         rval = instance->get_connectivity( *i, conn, len, true );
01527         if (MB_SUCCESS != rval)
01528           return rval;
01529         
01530         coords.resize(3*len);
01531         rval = instance->get_coords( conn, len, &coords[0] );
01532         if (MB_SUCCESS != rval)
01533           return rval;
01534         
01535         if (len == 3) 
01536           GeomUtil::closest_location_on_tri( loc, (CartVect*)(&coords[0]), tmp );
01537         else
01538           GeomUtil::closest_location_on_polygon( loc, (CartVect*)(&coords[0]), len, tmp );
01539         
01540         diff = tmp - loc;
01541         dist_sqr = diff % diff;
01542         if (dist_sqr < smallest_dist_sqr) {
01543           smallest_dist_sqr = dist_sqr;
01544           facet_out = *i;
01545           tmp.get( point_out );
01546           if (set_out)
01547             *set_out = current_set;
01548         }
01549       }
01550     } // LEAF NODE
01551   }
01552 
01553   if( accum ){
01554     accum->end_traversal( max_depth );
01555   }
01556   
01557   return MB_SUCCESS;
01558 }
01559                                      
01560 ErrorCode OrientedBoxTreeTool::closest_to_location( const double* point,
01561                                      EntityHandle root,
01562                                      double tolerance,
01563                                      std::vector<EntityHandle>& facets_out,
01564                                      std::vector<EntityHandle>* sets_out, 
01565                                      TrvStats* accum )
01566 {
01567   ErrorCode rval;
01568   const CartVect loc( point );
01569   double smallest_dist_sqr = std::numeric_limits<double>::max();
01570   double smallest_dist = smallest_dist_sqr;
01571   
01572   EntityHandle current_set = 0;
01573   Range sets;
01574   std::vector<EntityHandle> children(2);
01575   std::vector<double> coords;
01576   std::vector<OBBTreeCPFrame> stack;
01577   int max_depth = -1;
01578 
01579   stack.push_back( OBBTreeCPFrame(0.0, root, current_set, 0) );
01580   
01581   while( !stack.empty() ) {
01582 
01583       // pop from top of stack
01584     EntityHandle node = stack.back().node;
01585     double dist_sqr = stack.back().dist_sqr;
01586     current_set = stack.back().mset;
01587     int current_depth = stack.back().depth;
01588     stack.pop_back();
01589 
01590       // If current best result is closer than the box, skip this tree node.
01591     if (dist_sqr > smallest_dist_sqr + tolerance)
01592       continue;
01593 
01594       // increment traversal statistics.  
01595     if( accum ){
01596       accum->increment( current_depth );
01597       max_depth = std::max( max_depth, current_depth );
01598     }
01599 
01600       // Check if this node has a set associated with it
01601     if (sets_out && !current_set) {
01602       sets.clear();
01603       rval = instance->get_entities_by_type( node, MBENTITYSET, sets );
01604       if (MB_SUCCESS != rval)
01605         return rval;
01606       if (!sets.empty()) {
01607         if (sets.size() != 1)
01608           return MB_MULTIPLE_ENTITIES_FOUND;
01609         current_set = *sets.begin();
01610       }
01611     }
01612 
01613       // Get child boxes
01614     children.clear();
01615     rval = instance->get_child_meshsets( node, children );
01616     if (MB_SUCCESS != rval)
01617       return rval;
01618 
01619       // if not a leaf node
01620     if (!children.empty()) {
01621       if (children.size() != 2)
01622         return MB_MULTIPLE_ENTITIES_FOUND;
01623     
01624         // get boxes
01625       OrientedBox box1, box2;
01626       rval = box( children[0], box1 );
01627       if (MB_SUCCESS != rval) return rval;
01628       rval = box( children[1], box2 );
01629       if (MB_SUCCESS != rval) return rval;
01630       
01631         // get distance from each box
01632       CartVect pt1, pt2;
01633       box1.closest_location_in_box( loc, pt1 );
01634       box2.closest_location_in_box( loc, pt2 );
01635       pt1 -= loc;
01636       pt2 -= loc;
01637       const double dsqr1 = pt1 % pt1;
01638       const double dsqr2 = pt2 % pt2;
01639       
01640         // push children on tree such that closer one is on top
01641       if (dsqr1 < dsqr2) {
01642         stack.push_back( OBBTreeCPFrame(dsqr2, children[1], current_set, current_depth+1 ) );
01643         stack.push_back( OBBTreeCPFrame(dsqr1, children[0], current_set, current_depth+1 ) );
01644       }
01645       else {
01646         stack.push_back( OBBTreeCPFrame(dsqr1, children[0], current_set, current_depth+1 ) );
01647         stack.push_back( OBBTreeCPFrame(dsqr2, children[1], current_set, current_depth+1 ) );
01648       }
01649     }
01650     else { // LEAF NODE
01651       if( accum ){ accum->increment_leaf( current_depth ); }
01652 
01653       Range facets;
01654       rval = instance->get_entities_by_dimension( node, 2, facets );
01655       if (MB_SUCCESS != rval)
01656         return rval;
01657       
01658       const EntityHandle* conn;
01659       int len;
01660       CartVect tmp, diff;
01661       for (Range::iterator i = facets.begin(); i != facets.end(); ++i) {
01662         rval = instance->get_connectivity( *i, conn, len, true );
01663         if (MB_SUCCESS != rval)
01664           return rval;
01665         
01666         coords.resize(3*len);
01667         rval = instance->get_coords( conn, len, &coords[0] );
01668         if (MB_SUCCESS != rval)
01669           return rval;
01670         
01671         if (len == 3) 
01672           GeomUtil::closest_location_on_tri( loc, (CartVect*)(&coords[0]), tmp );
01673         else
01674           GeomUtil::closest_location_on_polygon( loc, (CartVect*)(&coords[0]), len, tmp );
01675         
01676         diff = tmp - loc;
01677         dist_sqr = diff % diff;
01678         if (dist_sqr < smallest_dist_sqr) {
01679           if (0.5*dist_sqr < 0.5*smallest_dist_sqr + tolerance*(0.5*tolerance - smallest_dist)) {
01680             facets_out.clear();
01681             if (sets_out)
01682               sets_out->clear();
01683           }
01684           smallest_dist_sqr = dist_sqr;
01685           smallest_dist = sqrt(smallest_dist_sqr);
01686             // put closest result at start of lists
01687           facets_out.push_back( *i );
01688           std::swap( facets_out.front(), facets_out.back() );
01689           if (sets_out) {
01690             sets_out->push_back( current_set );
01691             std::swap( sets_out->front(), sets_out->back() );
01692           }
01693         }
01694         else if (dist_sqr <= smallest_dist_sqr + tolerance*(tolerance + 2*smallest_dist)) {
01695           facets_out.push_back(*i);
01696           if (sets_out)
01697             sets_out->push_back( current_set );
01698         }
01699       }
01700     } // LEAF NODE
01701   }
01702 
01703   if( accum ){
01704     accum->end_traversal( max_depth );
01705   }
01706   
01707   return MB_SUCCESS;
01708 }
01709     
01710     
01711 
01712 /********************** Tree Printing Code ****************************/
01713 
01714 
01715 class TreeLayoutPrinter : public OrientedBoxTreeTool::Op
01716 {
01717   public:
01718     TreeLayoutPrinter( std::ostream& stream,
01719                        Interface* instance );
01720     
01721     virtual ErrorCode visit( EntityHandle node, 
01722                              int          depth,
01723                  bool&        descend );
01724     virtual ErrorCode leaf( EntityHandle node );
01725   private:
01726 
01727     Interface* instance;
01728     std::ostream& outputStream;
01729     std::vector<bool> path;
01730 };
01731 
01732 TreeLayoutPrinter::TreeLayoutPrinter( std::ostream& stream,
01733                                       Interface* interface )
01734   : instance(interface),
01735     outputStream(stream)
01736   {}
01737 
01738 ErrorCode TreeLayoutPrinter::visit( EntityHandle node, 
01739                                     int          depth,
01740                     bool&        descend )
01741 {
01742   descend = true;
01743   
01744   if ((unsigned)depth > path.size()) {
01745     //assert(depth+1 == path.size); // preorder traversal
01746     path.push_back(true);
01747   }
01748   else {
01749     path.resize( depth );
01750     if (depth)
01751       path.back() = false;
01752   }
01753   
01754   for (unsigned i = 0; i+1 < path.size(); ++i) {
01755     if (path[i])
01756       outputStream << "|   ";
01757     else
01758       outputStream << "    ";
01759   }
01760   if (depth) {
01761     if (path.back())
01762       outputStream << "+---";
01763     else
01764       outputStream << "\\---";
01765   }
01766   outputStream << instance->id_from_handle( node ) << std::endl;
01767   return MB_SUCCESS;
01768 }
01769 
01770 ErrorCode TreeLayoutPrinter::leaf( EntityHandle ) { return MB_SUCCESS; }
01771     
01772 
01773 class TreeNodePrinter : public OrientedBoxTreeTool::Op
01774 {
01775   public:
01776     TreeNodePrinter( std::ostream& stream,
01777                      bool list_contents,
01778                      bool list_box,
01779                      const char* id_tag_name,
01780                      OrientedBoxTreeTool* tool_ptr );
01781     
01782     virtual ErrorCode visit( EntityHandle node, 
01783                              int          depth,
01784                  bool&        descend );
01785                                
01786     virtual ErrorCode leaf( EntityHandle ) { return MB_SUCCESS; }
01787   private:
01788   
01789     ErrorCode print_geometry( EntityHandle node );
01790     ErrorCode print_contents( EntityHandle node );
01791     ErrorCode print_counts( EntityHandle node );
01792   
01793     bool printContents;
01794     bool printGeometry;
01795     bool haveTag;
01796     Tag tag, gidTag, geomTag;
01797     Interface* instance;
01798     OrientedBoxTreeTool* tool;
01799     std::ostream& outputStream;
01800 };
01801 
01802 
01803 TreeNodePrinter::TreeNodePrinter( std::ostream& stream,
01804                                   bool list_contents,
01805                                   bool list_box,
01806                                   const char* id_tag_name,
01807                                   OrientedBoxTreeTool* tool_ptr )
01808   : printContents( list_contents ),
01809     printGeometry( list_box ),
01810     haveTag( false ),
01811     tag( 0 ),
01812     gidTag(0), geomTag(0),
01813     instance( tool_ptr->get_moab_instance() ),
01814     tool(tool_ptr),
01815     outputStream( stream )
01816 {
01817   ErrorCode rval;
01818   if (id_tag_name) {
01819     rval = instance->tag_get_handle( id_tag_name, 1, MB_TYPE_INTEGER, tag );
01820     if (!rval) {
01821       std::cerr << "Could not get tag \"" << id_tag_name << "\"\n";
01822       stream << "Could not get tag \"" << id_tag_name << "\"\n";
01823     }
01824     else {
01825       haveTag = true;
01826     }
01827   }
01828   
01829   rval = instance->tag_get_handle( GLOBAL_ID_TAG_NAME, 1, MB_TYPE_INTEGER, gidTag );
01830   if (MB_SUCCESS != rval)
01831     gidTag = 0;
01832   rval = instance->tag_get_handle( GEOM_DIMENSION_TAG_NAME, 1, MB_TYPE_INTEGER, geomTag );
01833   if (MB_SUCCESS != rval)
01834     geomTag = 0;
01835 }   
01836 
01837 ErrorCode TreeNodePrinter::visit( EntityHandle node, int, bool& descend )
01838 {
01839   descend = true;
01840   EntityHandle setid = instance->id_from_handle( node );
01841   outputStream << setid << ":" << std::endl;
01842   
01843   Range surfs;
01844   ErrorCode r3 = MB_SUCCESS;
01845   if (geomTag) {
01846     const int two = 2;
01847     const void* tagdata[] = {&two};
01848     r3 = instance->get_entities_by_type_and_tag( node, MBENTITYSET, &geomTag, tagdata, 1, surfs );
01849   
01850     if (MB_SUCCESS == r3 && surfs.size() == 1) {
01851       EntityHandle surf = *surfs.begin();
01852       int id;
01853       if (gidTag && MB_SUCCESS == instance->tag_get_data( gidTag, &surf, 1, &id ))
01854         outputStream << "  Surface " << id << std::endl;
01855       else
01856         outputStream << "  Surface w/ unknown ID (" << surf << ")" << std::endl;
01857     }
01858   }
01859   
01860   ErrorCode r1 = printGeometry ? print_geometry( node ) : MB_SUCCESS;
01861   ErrorCode r2 = printContents ? print_contents( node ) : print_counts( node );
01862   outputStream << std::endl;
01863   
01864   if (MB_SUCCESS != r1)
01865     return r1;
01866   else if (MB_SUCCESS != r2)
01867     return r2;
01868   else
01869     return r3;
01870 }
01871 
01872 ErrorCode TreeNodePrinter::print_geometry( EntityHandle node )
01873 {
01874   OrientedBox box;
01875   ErrorCode rval= tool->box( node, box );
01876   if (MB_SUCCESS != rval)
01877     return rval;
01878   
01879   CartVect length = box.dimensions();
01880   
01881   outputStream << box.center << "  Radius: " 
01882                << box.inner_radius() << " - " << box.outer_radius() << std::endl
01883                << '+' << box.axis[0] << " : " << length[0] << std::endl
01884                << 'x' << box.axis[1] << " : " << length[1] << std::endl
01885                << 'x' << box.axis[2] << " : " << length[2] << std::endl;
01886   return MB_SUCCESS;
01887 }
01888 
01889 ErrorCode TreeNodePrinter::print_counts( EntityHandle node )
01890 {
01891   for (EntityType type = MBVERTEX; type != MBMAXTYPE; ++type) {
01892     int count = 0;
01893     ErrorCode rval = instance->get_number_entities_by_type( node, type, count );
01894     if (MB_SUCCESS != rval)
01895       return rval;
01896     if(count > 0)
01897       outputStream << " " << count << " " << CN::EntityTypeName(type) << std::endl;
01898   }
01899   return MB_SUCCESS;
01900 }
01901 
01902 ErrorCode TreeNodePrinter::print_contents( EntityHandle node )
01903 {
01904     // list contents
01905   for (EntityType type = MBVERTEX; type != MBMAXTYPE; ++type) {
01906     Range range;
01907     ErrorCode rval = instance->get_entities_by_type( node, type, range );
01908     if (MB_SUCCESS != rval)
01909       return rval;
01910     if (range.empty())
01911       continue;
01912     outputStream << " " << CN::EntityTypeName(type) << " ";  
01913     std::vector<int> ids( range.size() );
01914     if (haveTag) {
01915       rval = instance->tag_get_data( tag, range, &ids[0] );
01916       std::sort( ids.begin(), ids.end() );
01917     }
01918     else {
01919       Range::iterator ri = range.begin();
01920       std::vector<int>::iterator vi = ids.begin();
01921       while (ri != range.end()) {
01922         *vi = instance->id_from_handle( *ri );
01923         ++ri;
01924         ++vi;
01925       }
01926     }
01927     
01928     unsigned i = 0;
01929     for(;;) {
01930       unsigned beg = i, end;
01931       do { end = i++; } while (i < ids.size() && ids[end]+1 == ids[i]);
01932       if (end == beg)
01933         outputStream << ids[end];
01934       else if (end == beg+1) 
01935         outputStream << ids[beg] << ", " << ids[end];
01936       else
01937         outputStream << ids[beg] << "-" << ids[end];
01938         
01939       if (i == ids.size()) {
01940         outputStream << std::endl;
01941         break;
01942       }
01943       else 
01944         outputStream << ", ";
01945     }
01946   }
01947   
01948   return MB_SUCCESS;
01949 }
01950 
01951   
01952 void OrientedBoxTreeTool::print( EntityHandle set, std::ostream& str, bool list, const char* tag )
01953 {
01954   TreeLayoutPrinter op1( str, instance );
01955   TreeNodePrinter op2( str, list, true, tag, this );
01956   ErrorCode r1 = preorder_traverse( set, op1 );
01957   str << std::endl;
01958   ErrorCode r2 = preorder_traverse( set, op2 );
01959   if (r1 != MB_SUCCESS || r2 != MB_SUCCESS) {
01960     std::cerr << "Errors encountered while printing tree\n";
01961     str << "Errors encountered while printing tree\n";
01962   }
01963 }
01964 
01965 
01966 /********************* Traversal Metrics Code  **************************/
01967 
01968 void OrientedBoxTreeTool::TrvStats::reset(){
01969   nodes_visited_count.clear();
01970   leaves_visited_count.clear();
01971   traversals_ended_count.clear();
01972   ray_tri_tests_count = 0;
01973 }
01974 
01975 void OrientedBoxTreeTool::TrvStats::increment( unsigned depth ){
01976 
01977   while( nodes_visited_count.size() <= depth ){
01978     nodes_visited_count.push_back(0);
01979     leaves_visited_count.push_back(0);
01980     traversals_ended_count.push_back(0);
01981   }
01982   nodes_visited_count[depth] += 1;
01983 }
01984 
01985 void OrientedBoxTreeTool::TrvStats::increment_leaf( unsigned depth ){
01986   //assume safe depth, because increment is called first
01987   leaves_visited_count[depth] += 1;
01988 }
01989 
01990 void OrientedBoxTreeTool::TrvStats::end_traversal( unsigned depth ){
01991   // assume safe depth, because increment is always called on a given 
01992   // tree level first
01993   traversals_ended_count[depth] += 1;
01994 }
01995 
01996 void OrientedBoxTreeTool::TrvStats::print( std::ostream& str ) const {
01997 
01998   const std::string h1 = "OBBTree Depth";
01999   const std::string h2 = " - NodesVisited";
02000   const std::string h3 = " - LeavesVisited";
02001   const std::string h4 = " - TraversalsEnded";
02002 
02003   str << h1 << h2 << h3 << h4 << std::endl;
02004 
02005   unsigned num_visited = 0, num_leaves = 0, num_traversals = 0;
02006   for( unsigned i = 0; i < traversals_ended_count.size(); ++i){
02007 
02008     num_visited    += nodes_visited_count[i];
02009     num_leaves     += leaves_visited_count[i];
02010     num_traversals += traversals_ended_count[i];
02011    
02012     str << std::setw(h1.length()) << i 
02013         << std::setw(h2.length()) << nodes_visited_count[i] 
02014         << std::setw(h3.length()) << leaves_visited_count[i]
02015         << std::setw(h4.length()) << traversals_ended_count[i] 
02016         << std::endl;
02017   }
02018   
02019   str << std::setw(h1.length()) << "---- Totals:" 
02020       << std::setw(h2.length()) << num_visited 
02021       << std::setw(h3.length()) << num_leaves
02022       << std::setw(h4.length()) << num_traversals 
02023       << std::endl;
02024   
02025   if( ray_tri_tests_count ){
02026     str << std::setw(h1.length()) << "---- Total ray-tri tests: " 
02027     << ray_tri_tests_count << std::endl;
02028   }
02029 
02030 }
02031 
02032 /********************** Tree Statistics Code ****************************/
02033 
02034 
02035 class StatData { 
02036 public:
02037   struct Ratio {
02038     double min, max, sum, sqr;
02039     int hist[10];
02040     Ratio() 
02041       : min(std::numeric_limits<double>::max()), 
02042         max(-std::numeric_limits<double>::max()), 
02043         sum(0.0), sqr(0.0)
02044       { hist[0] = hist[1] = hist[2] = hist[3] = hist[4] = hist[5] =
02045         hist[6] = hist[7] = hist[8] = hist[9] = 0; }
02046     void accum( double v ) {
02047       if (v < min) min = v;
02048       if (v > max) max = v;
02049       sum += v;
02050       sqr += v*v;
02051       int i = (int)(10*v);
02052       if (i < 0) i = 0;
02053       else if (i > 9) i = 9;
02054       ++hist[i];
02055     }
02056   };
02057   
02058   template <typename T> struct Stat {
02059     T min, max;
02060     double sum, sqr;
02061     Stat() : sum(0.0), sqr(0.0) {
02062       std::numeric_limits<T> lim;
02063       min = lim.max();
02064       if (lim.is_integer)
02065         max = lim.min();
02066       else
02067         max = -lim.max();
02068     }
02069     void accum( T v ) {
02070       if (v < min) min = v;
02071       if (v > max) max = v;
02072       sum += v;
02073       sqr += (double)v * v;
02074     }
02075   };
02076 
02077   StatData() :
02078     count(0) 
02079     {}
02080 
02081   Ratio volume;
02082   Ratio entities;
02083   Ratio radius;
02084   Stat<unsigned> leaf_ent;
02085   Stat<double> vol;
02086   Stat<double> area;
02087   std::vector<unsigned> leaf_depth;
02088   unsigned count;
02089 };
02090 
02091 static int measure( const CartVect& v, double& result )
02092 {
02093   const double tol = 1e-6;
02094   int dims = 0;
02095   result = 1;
02096   for (int i = 0; i < 3; ++i)
02097     if (v[i] > tol) {
02098       ++dims; 
02099       result *= v[i];
02100   }
02101   return dims;
02102 }
02103   
02104 
02105 ErrorCode OrientedBoxTreeTool::recursive_stats( OrientedBoxTreeTool* tool,
02106                                     Interface* inst,
02107                                     EntityHandle set,
02108                                     int depth,
02109                                     StatData& data,
02110                                     unsigned& count_out,
02111                                     CartVect& dimensions_out )
02112 {
02113   ErrorCode rval;
02114   OrientedBox tmp_box;
02115   std::vector<EntityHandle> children(2);
02116   unsigned counts[2];
02117   bool isleaf;
02118   
02119   ++data.count;
02120   
02121   rval = tool->box( set, tmp_box );
02122   if (MB_SUCCESS != rval) return rval;
02123   children.clear();
02124   rval = inst->get_child_meshsets( set, children );
02125   if (MB_SUCCESS != rval) return rval;
02126   isleaf = children.empty();
02127   if (!isleaf && children.size() != 2)
02128     return MB_MULTIPLE_ENTITIES_FOUND;
02129   
02130   dimensions_out = tmp_box.dimensions();
02131   data.radius.accum( tmp_box.inner_radius() / tmp_box.outer_radius());
02132   data.vol.accum( tmp_box.volume() );
02133   data.area.accum( tmp_box.area() );
02134   
02135   if (isleaf) {
02136     if (data.leaf_depth.size() <= (unsigned)depth)
02137       data.leaf_depth.resize( depth+1, 0 );
02138     ++data.leaf_depth[depth];
02139     
02140     int count;
02141     rval = inst->get_number_entities_by_handle( set, count );
02142     if (MB_SUCCESS != rval) return rval;
02143     count_out = count;
02144     data.leaf_ent.accum( count_out );
02145   }
02146   else {
02147     for (int i = 0; i < 2; ++i) {
02148       CartVect dims;
02149       rval = recursive_stats( tool, inst, children[i], depth+1, data, counts[i], dims );
02150       if (MB_SUCCESS != rval) return rval;
02151       double this_measure, chld_measure;
02152       int this_dim = measure( dimensions_out, this_measure );
02153       int chld_dim = measure( dims, chld_measure );
02154       double ratio;
02155       if (chld_dim < this_dim)
02156         ratio = 0;
02157       else
02158         ratio = chld_measure / this_measure;
02159   
02160       data.volume.accum( ratio );
02161     }
02162     count_out = counts[0] + counts[1];
02163     data.entities.accum( (double)counts[0] / count_out );
02164     data.entities.accum( (double)counts[1] / count_out );
02165   }
02166   return MB_SUCCESS;
02167 }
02168 
02169 static inline double std_dev( double sqr, double sum, double count )
02170 {
02171   sum /= count;
02172   sqr /= count;
02173   return sqrt( sqr - sum*sum );
02174 }
02175 
02176 //#define WW <<std::setw(10)<<std::fixed<<
02177 #define WE <<std::setw(10)<<
02178 #define WW WE
02179 ErrorCode OrientedBoxTreeTool::stats( EntityHandle set, 
02180                                           unsigned &total_entities,
02181                                           double &rv,
02182                                           double &tot_node_volume,
02183                                           double &tot_to_root_volume,
02184                                           unsigned &tree_height,
02185                                           unsigned &node_count,
02186                                           unsigned &num_leaves)
02187 {
02188   StatData d;
02189   ErrorCode rval;
02190   unsigned i;
02191   CartVect total_dim;
02192   
02193   rval = recursive_stats( this, instance, set, 0, d, total_entities, total_dim );
02194   if (MB_SUCCESS != rval)
02195     return rval;
02196   
02197   tree_height = d.leaf_depth.size();
02198   unsigned min_leaf_depth = tree_height;
02199   num_leaves = 0;
02200   unsigned max_leaf_per_depth = 0;
02201   double sum_leaf_depth = 0, sqr_leaf_depth = 0;
02202   for (i = 0; i < d.leaf_depth.size(); ++i) {
02203     unsigned val = d.leaf_depth[i];
02204     num_leaves += val;
02205     sum_leaf_depth += (double)val*i;
02206     sqr_leaf_depth += (double)val*i*i;
02207     if (val && i < min_leaf_depth)
02208       min_leaf_depth = i;
02209     if (max_leaf_per_depth < val)
02210       max_leaf_per_depth = val;
02211   }
02212   rv = total_dim[0]*total_dim[1]*total_dim[2];
02213   tot_node_volume = d.vol.sum;
02214   tot_to_root_volume = d.vol.sum/rv;
02215   node_count = d.count;
02216 
02217   return MB_SUCCESS;
02218 }
02219 
02220 ErrorCode OrientedBoxTreeTool::stats( EntityHandle set, std::ostream& s )
02221 {
02222   StatData d;
02223   ErrorCode rval;
02224   unsigned total_entities, i;
02225   CartVect total_dim;
02226   
02227   rval = recursive_stats( this, instance, set, 0, d, total_entities, total_dim );
02228   if (MB_SUCCESS != rval)
02229     return rval;
02230   
02231   unsigned tree_height = d.leaf_depth.size();
02232   unsigned min_leaf_depth = tree_height, num_leaves = 0;
02233   unsigned max_leaf_per_depth = 0;
02234   double sum_leaf_depth = 0, sqr_leaf_depth = 0;
02235   for (i = 0; i < d.leaf_depth.size(); ++i) {
02236     unsigned val = d.leaf_depth[i];
02237     num_leaves += val;
02238     sum_leaf_depth += (double)val*i;
02239     sqr_leaf_depth += (double)val*i*i;
02240     if (val && i < min_leaf_depth)
02241       min_leaf_depth = i;
02242     if (max_leaf_per_depth < val)
02243       max_leaf_per_depth = val;
02244   }
02245   unsigned num_non_leaf = d.count - num_leaves;
02246   
02247   double rv = total_dim[0]*total_dim[1]*total_dim[2];
02248   s << "entities in tree:  " << total_entities << std::endl
02249     << "root volume:       " << rv << std::endl
02250     << "total node volume: " << d.vol.sum << std::endl
02251     << "total/root volume: " << d.vol.sum/rv << std::endl
02252     << "tree height:       " << tree_height << std::endl
02253     << "node count:        " << d.count << std::endl
02254     << "leaf count:        " << num_leaves << std::endl
02255     << std::endl;
02256   
02257   double avg_leaf_depth = sum_leaf_depth / num_leaves;
02258   double rms_leaf_depth = sqrt( sqr_leaf_depth / num_leaves );
02259   double std_leaf_depth = std_dev( sqr_leaf_depth, sum_leaf_depth, num_leaves );
02260 
02261   double avg_leaf_ent = d.leaf_ent.sum / num_leaves;
02262   double rms_leaf_ent = sqrt( d.leaf_ent.sqr / num_leaves );
02263   double std_leaf_ent = std_dev( d.leaf_ent.sqr, d.leaf_ent.sum, num_leaves );
02264 
02265   unsigned num_child = 2 * num_non_leaf;
02266 
02267   double avg_vol_ratio = d.volume.sum / num_child;
02268   double rms_vol_ratio = sqrt( d.volume.sqr / num_child );
02269   double std_vol_ratio = std_dev( d.volume.sqr, d.volume.sum, num_child);
02270 
02271   double avg_ent_ratio = d.entities.sum / num_child;
02272   double rms_ent_ratio = sqrt( d.entities.sqr / num_child );
02273   double std_ent_ratio = std_dev( d.entities.sqr, d.entities.sum, num_child);
02274 
02275   double avg_rad_ratio = d.radius.sum / d.count;
02276   double rms_rad_ratio = sqrt( d.radius.sqr / d.count );
02277   double std_rad_ratio = std_dev( d.radius.sqr, d.radius.sum, d.count );
02278   
02279   double avg_vol = d.vol.sum / d.count;
02280   double rms_vol = sqrt( d.vol.sqr / d.count );
02281   double std_vol = std_dev( d.vol.sqr, d.vol.sum, d.count );
02282   
02283   double avg_area = d.area.sum / d.count;
02284   double rms_area = sqrt( d.area.sqr / d.count );
02285   double std_area = std_dev( d.area.sqr, d.area.sum, d.count );
02286       
02287   int prec = s.precision();
02288   s <<                         "                   " WW "Minimum"      WW "Average"      WW "RMS"          WW "Maximum"             WW "Std.Dev."     << std::endl;
02289   s << std::setprecision(1) << "Leaf Depth         " WW min_leaf_depth WW avg_leaf_depth WW rms_leaf_depth WW d.leaf_depth.size()-1 WW std_leaf_depth << std::endl; 
02290   s << std::setprecision(0) << "Entities/Leaf      " WW d.leaf_ent.min WW avg_leaf_ent   WW rms_leaf_ent   WW d.leaf_ent.max        WW std_leaf_ent   << std::endl;
02291   s << std::setprecision(3) << "Child Volume Ratio " WW d.volume.min   WW avg_vol_ratio  WW rms_vol_ratio  WW d.volume.max          WW std_vol_ratio  << std::endl;
02292   s << std::setprecision(3) << "Child Entity Ratio " WW d.entities.min WW avg_ent_ratio  WW rms_ent_ratio  WW d.entities.max        WW std_ent_ratio  << std::endl;
02293   s << std::setprecision(3) << "Box Radius Ratio   " WW d.radius.min   WW avg_rad_ratio  WW rms_rad_ratio  WW d.radius.max          WW std_rad_ratio  << std::endl;
02294   s << std::setprecision(0) << "Box volume         " WE d.vol.min      WE avg_vol        WE rms_vol        WE d.vol.max             WE std_vol        << std::endl;
02295   s << std::setprecision(0) << "Largest side area  " WE d.area.min     WE avg_area       WE rms_area       WE d.area.max            WE std_area       << std::endl;
02296   s << std::setprecision(prec) << std::endl;
02297   
02298   s << "Leaf Depth Histogram (Root depth is 0)" << std::endl;
02299   double f = 60.0 / max_leaf_per_depth;
02300   for (i = min_leaf_depth; i < d.leaf_depth.size(); ++i)
02301     s << std::setw(2) << i << " " << std::setw(5) << d.leaf_depth[i] << " |"
02302       << std::setfill('*') << std::setw((int)floor(f*d.leaf_depth[i]+0.5)) << "" 
02303       << std::setfill(' ') << std::endl;
02304   s <<std::endl;
02305   
02306   s << "Child/Parent Volume Ratio Histogram" << std::endl;
02307   f = 60.0 / *(std::max_element(d.volume.hist, d.volume.hist+10));
02308   for (i = 0; i < 10u; ++i)
02309     s << "0." << i << " " << std::setw(5) << d.volume.hist[i] << " |"
02310       << std::setfill('*') << std::setw((int)floor(f*d.volume.hist[i]+0.5)) << ""
02311       << std::setfill(' ') << std::endl;
02312   s <<std::endl;
02313   
02314   s << "Child/Parent Entity Count Ratio Histogram" << std::endl;
02315   f = 60.0 / *(std::max_element(d.entities.hist, d.entities.hist+10));
02316   for (i = 0; i < 10u; ++i)
02317     s << "0." << i << " " << std::setw(5) << d.entities.hist[i] << " |"
02318       << std::setfill('*') << std::setw((int)floor(f*d.entities.hist[i]+0.5)) << ""
02319       << std::setfill(' ') << std::endl;
02320   s <<std::endl;
02321   
02322   s << "Inner/Outer Radius Ratio Histogram (~0.70 for cube)" << std::endl;
02323     // max radius ratio for a box is about 0.7071.  Move any boxes
02324     // in the .7 bucket into .6 and print .0 to .6.
02325   d.radius.hist[6] += d.radius.hist[7]; 
02326   f = 60.0 / *(std::max_element(d.entities.hist, d.entities.hist+7));
02327   for (i = 0; i < 7u; ++i)
02328     s << "0." << i << " " << std::setw(5) << d.entities.hist[i] << " |"
02329       << std::setfill('*') << std::setw((int)floor(f*d.entities.hist[i]+0.5)) << ""
02330       << std::setfill(' ') << std::endl;
02331   s <<std::endl;
02332   
02333   return MB_SUCCESS;
02334 }  
02335 
02336 } // namespace moab
02337 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines