MOAB: Mesh Oriented datABase  (version 5.4.1)
AdaptiveKDTree.cpp
Go to the documentation of this file.
00001 /**\file AdaptiveKDTree.cpp
00002  */
00003 
00004 #include "moab/AdaptiveKDTree.hpp"
00005 #include "moab/Interface.hpp"
00006 #include "moab/GeomUtil.hpp"
00007 #include "moab/Range.hpp"
00008 #include "moab/ElemEvaluator.hpp"
00009 #include "moab/CpuTimer.hpp"
00010 #include "Internals.hpp"
00011 #include "moab/Util.hpp"
00012 #include <cmath>
00013 
00014 #include <cassert>
00015 #include <algorithm>
00016 #include <limits>
00017 #include <iostream>
00018 #include <cstdio>
00019 
00020 namespace moab
00021 {
00022 
00023 const char* AdaptiveKDTree::treeName = "AKDTree";
00024 
00025 #define MB_AD_KD_TREE_DEFAULT_TAG_NAME
00026 
00027 // If defined, use single tag for both axis and location of split plane
00028 #define MB_AD_KD_TREE_USE_SINGLE_TAG
00029 
00030 // No effect if MB_AD_KD_TREE_USE_SINGLE_TAG is not defined.
00031 // If defined, store plane axis as double so tag has consistent
00032 // type (doubles for both location and axis).  If not defined,
00033 // store struct Plane as opaque.
00034 #define MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG
00035 
00036 AdaptiveKDTree::AdaptiveKDTree( Interface* iface )
00037     : Tree( iface ), planeTag( 0 ), axisTag( 0 ), splitsPerDir( 3 ), planeSet( SUBDIVISION_SNAP ), spherical( false ),
00038       radius( 1.0 )
00039 {
00040     boxTagName = treeName;
00041 
00042     ErrorCode rval = init();
00043     if( MB_SUCCESS != rval ) throw rval;
00044 }
00045 
00046 AdaptiveKDTree::AdaptiveKDTree( Interface* iface,
00047                                 const Range& entities,
00048                                 EntityHandle* tree_root_set,
00049                                 FileOptions* opts )
00050     : Tree( iface ), planeTag( 0 ), axisTag( 0 ), splitsPerDir( 3 ), planeSet( SUBDIVISION_SNAP ), spherical( false ),
00051       radius( 1.0 )
00052 {
00053     boxTagName = treeName;
00054 
00055     ErrorCode rval;
00056     if( opts )
00057     {
00058         rval = parse_options( *opts );
00059         if( MB_SUCCESS != rval ) throw rval;
00060     }
00061 
00062     rval = init();
00063     if( MB_SUCCESS != rval ) throw rval;
00064 
00065     rval = build_tree( entities, tree_root_set, opts );
00066     if( MB_SUCCESS != rval ) throw rval;
00067 }
00068 
00069 AdaptiveKDTree::~AdaptiveKDTree()
00070 {
00071     if( !cleanUp ) return;
00072 
00073     if( myRoot )
00074     {
00075         reset_tree();
00076         myRoot = 0;
00077     }
00078 }
00079 
00080 ErrorCode AdaptiveKDTree::build_tree( const Range& entities, EntityHandle* tree_root_set, FileOptions* options )
00081 {
00082     ErrorCode rval;
00083     CpuTimer cp;
00084 
00085     if( options )
00086     {
00087         rval = parse_options( *options );
00088         if( MB_SUCCESS != rval ) return rval;
00089 
00090         if( !options->all_seen() ) return MB_FAILURE;
00091     }
00092 
00093     // calculate bounding box of elements
00094     BoundBox box;
00095     rval = box.update( *moab(), entities, spherical, radius );
00096     if( MB_SUCCESS != rval ) return rval;
00097 
00098     // create tree root
00099     EntityHandle tmp_root;
00100     if( !tree_root_set ) tree_root_set = &tmp_root;
00101     rval = create_root( box.bMin.array(), box.bMax.array(), *tree_root_set );
00102     if( MB_SUCCESS != rval ) return rval;
00103     rval = moab()->add_entities( *tree_root_set, entities );
00104     if( MB_SUCCESS != rval ) return rval;
00105 
00106     AdaptiveKDTreeIter iter;
00107     iter.initialize( this, *tree_root_set, box.bMin.array(), box.bMax.array(), AdaptiveKDTreeIter::LEFT );
00108 
00109     std::vector< double > tmp_data;
00110     std::vector< EntityHandle > tmp_data2;
00111     for( ;; )
00112     {
00113 
00114         int pcount;
00115         rval = moab()->get_number_entities_by_handle( iter.handle(), pcount );
00116         if( MB_SUCCESS != rval ) break;
00117 
00118         const size_t p_count = pcount;
00119         Range best_left, best_right, best_both;
00120         Plane best_plane = { HUGE_VAL, -1 };
00121         if( (int)p_count > maxPerLeaf && (int)iter.depth() < maxDepth )
00122         {
00123             switch( planeSet )
00124             {
00125                 case AdaptiveKDTree::SUBDIVISION:
00126                     rval = best_subdivision_plane( splitsPerDir, iter, best_left, best_right, best_both, best_plane,
00127                                                    minWidth );
00128                     break;
00129                 case AdaptiveKDTree::SUBDIVISION_SNAP:
00130                     rval = best_subdivision_snap_plane( splitsPerDir, iter, best_left, best_right, best_both,
00131                                                         best_plane, tmp_data, minWidth );
00132                     break;
00133                 case AdaptiveKDTree::VERTEX_MEDIAN:
00134                     rval = best_vertex_median_plane( splitsPerDir, iter, best_left, best_right, best_both, best_plane,
00135                                                      tmp_data, minWidth );
00136                     break;
00137                 case AdaptiveKDTree::VERTEX_SAMPLE:
00138                     rval = best_vertex_sample_plane( splitsPerDir, iter, best_left, best_right, best_both, best_plane,
00139                                                      tmp_data, tmp_data2, minWidth );
00140                     break;
00141                 default:
00142                     rval = MB_FAILURE;
00143             }
00144 
00145             if( MB_SUCCESS != rval ) return rval;
00146         }
00147 
00148         if( best_plane.norm >= 0 )
00149         {
00150             best_left.merge( best_both );
00151             best_right.merge( best_both );
00152             rval = split_leaf( iter, best_plane, best_left, best_right );
00153             if( MB_SUCCESS != rval ) return rval;
00154         }
00155         else
00156         {
00157             rval = iter.step();
00158             if( MB_ENTITY_NOT_FOUND == rval )
00159             {
00160                 rval               = treeStats.compute_stats( mbImpl, myRoot );
00161                 treeStats.initTime = cp.time_elapsed();
00162                 return rval;  // at end
00163             }
00164             else if( MB_SUCCESS != rval )
00165                 break;
00166         }
00167     }
00168 
00169     reset_tree();
00170 
00171     treeStats.reset();
00172 
00173     return rval;
00174 }
00175 
00176 ErrorCode AdaptiveKDTree::parse_options( FileOptions& opts )
00177 {
00178     ErrorCode rval = parse_common_options( opts );
00179     if( MB_SUCCESS != rval ) return rval;
00180 
00181     //  SPLITS_PER_DIR: number of candidate splits considered per direction; default = 3
00182     int tmp_int;
00183     rval = opts.get_int_option( "SPLITS_PER_DIR", tmp_int );
00184     if( MB_SUCCESS == rval ) splitsPerDir = tmp_int;
00185 
00186     //  PLANE_SET: method used to decide split planes; see CandidatePlaneSet enum (below)
00187     //           for possible values; default = 1 (SUBDIVISION_SNAP)
00188     rval = opts.get_int_option( "PLANE_SET", tmp_int );
00189     if( MB_SUCCESS == rval && ( tmp_int < SUBDIVISION || tmp_int > VERTEX_SAMPLE ) )
00190         return MB_FAILURE;
00191     else if( MB_ENTITY_NOT_FOUND == rval )
00192         planeSet = SUBDIVISION;
00193     else
00194         planeSet = (CandidatePlaneSet)( tmp_int );
00195 
00196     rval = opts.get_toggle_option( "SPHERICAL", false, spherical );
00197     if( MB_SUCCESS != rval ) spherical = false;
00198 
00199     double tmp = 1.0;
00200     rval       = opts.get_real_option( "RADIUS", tmp );
00201     if( MB_SUCCESS != rval )
00202         radius = 1.0;
00203     else
00204         radius = tmp;
00205 
00206     return MB_SUCCESS;
00207 }
00208 
00209 ErrorCode AdaptiveKDTree::make_tag( Interface* iface,
00210                                     std::string name,
00211                                     TagType storage,
00212                                     DataType type,
00213                                     int count,
00214                                     void* default_val,
00215                                     Tag& tag_handle,
00216                                     std::vector< Tag >& created_tags )
00217 {
00218     ErrorCode rval =
00219         iface->tag_get_handle( name.c_str(), count, type, tag_handle, MB_TAG_CREAT | storage, default_val );
00220 
00221     if( MB_SUCCESS == rval )
00222     {
00223         if( std::find( created_tags.begin(), created_tags.end(), tag_handle ) == created_tags.end() )
00224             created_tags.push_back( tag_handle );
00225     }
00226     else
00227     {
00228         while( !created_tags.empty() )
00229         {
00230             iface->tag_delete( created_tags.back() );
00231             created_tags.pop_back();
00232         }
00233 
00234         planeTag = axisTag = (Tag)-1;
00235     }
00236 
00237     return rval;
00238 }
00239 
00240 ErrorCode AdaptiveKDTree::init()
00241 {
00242     std::vector< Tag > ctl;
00243 
00244 #ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
00245     // create two tags, one for axis direction and one for axis coordinate
00246     std::string n1( treeName ), n2( treeName );
00247     n1 += "_coord";
00248     n2 += "_norm";
00249     ErrorCode rval = make_tag( moab(), n1, MB_TAG_DENSE, MB_TYPE_DOUBLE, 1, 0, planeTag, ctl );
00250     if( MB_SUCCESS != rval ) return rval;
00251     rval = make_tag( moab(), n2, MB_TAG_DENSE, MB_TYPE_INT, 1, 0, axisTag, ctl );
00252     if( MB_SUCCESS != rval ) return rval;
00253 
00254 #elif defined( MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG )
00255     // create tag to hold two doubles, one for location and one for axis
00256     std::string double_tag_name = std::string( treeName ) + std::string( "_coord_norm" );
00257     ErrorCode rval = make_tag( moab(), double_tag_name, MB_TAG_DENSE, MB_TYPE_DOUBLE, 2, 0, planeTag, ctl );
00258     if( MB_SUCCESS != rval ) return rval;
00259 #else
00260     // create opaque tag to hold struct Plane
00261     ErrorCode rval = make_tag( moab(), tagname, MB_TAG_DENSE, MB_TYPE_OPAQUE, sizeof( Plane ), 0, planeTag, ctl );
00262     if( MB_SUCCESS != rval ) return rval;
00263 
00264 #ifdef MOAB_HAVE_HDF5
00265     // create a mesh tag holding the HDF5 type for a struct Plane
00266     Tag type_tag;
00267     std::string type_tag_name = "__hdf5_tag_type_";
00268     type_tag_name += boxTagName;
00269     rval = make_tag( moab(), type_tag_name, MB_TAG_MESH, MB_TYPE_OPAQUE, sizeof( hid_t ), 0, type_tag, ctl );
00270     if( MB_SUCCESS != rval ) return rval;
00271     // create HDF5 type object describing struct Plane
00272     Plane p;
00273     hid_t handle = H5Tcreate( H5T_COMPOUND, sizeof( Plane ) );
00274     H5Tinsert( handle, "coord", &( p.coord ) - &p, H5T_NATIVE_DOUBLE );
00275     H5Tinsert( handle, "norm", &( p.axis ) - &p, H5T_NATIVE_INT );
00276     EntityHandle root = 0;
00277     rval              = mbImpl->tag_set_data( type_tag, &root, 1, &handle );
00278     if( MB_SUCCESS != rval ) return rval;
00279 #endif
00280 #endif
00281 
00282     return rval;
00283 }
00284 
00285 ErrorCode AdaptiveKDTree::get_split_plane( EntityHandle entity, Plane& plane )
00286 {
00287 #ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
00288     ErrorCode r1, r2;
00289     r1 = moab()->tag_get_data( planeTag, &entity, 1, &plane.coord );
00290     r2 = moab()->tag_get_data( axisTag, &entity, 1, &plane.norm );
00291     return MB_SUCCESS == r1 ? r2 : r1;
00292 #elif defined( MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG )
00293     double values[2];
00294     ErrorCode rval = moab()->tag_get_data( planeTag, &entity, 1, values );
00295     plane.coord    = values[0];
00296     plane.norm     = (int)values[1];
00297     return rval;
00298 #else
00299     return moab()->tag_get_data( planeTag, &entity, 1, &plane );
00300 #endif
00301 }
00302 
00303 ErrorCode AdaptiveKDTree::set_split_plane( EntityHandle entity, const Plane& plane )
00304 {
00305 #ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
00306     ErrorCode r1, r2;
00307     r1 = moab()->tag_set_data( planeTag, &entity, 1, &plane.coord );
00308     r2 = moab()->tag_set_data( axisTag, &entity, 1, &plane.norm );
00309     return MB_SUCCESS == r1 ? r2 : r1;
00310 #elif defined( MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG )
00311     double values[2] = { plane.coord, static_cast< double >( plane.norm ) };
00312     return moab()->tag_set_data( planeTag, &entity, 1, values );
00313 #else
00314     return moab()->tag_set_data( planeTag, &entity, 1, &plane );
00315 #endif
00316 }
00317 
00318 ErrorCode AdaptiveKDTree::get_tree_iterator( EntityHandle root, AdaptiveKDTreeIter& iter )
00319 {
00320     double box[6];
00321     ErrorCode rval = moab()->tag_get_data( boxTag, &root, 1, box );
00322     if( MB_SUCCESS != rval ) return rval;
00323 
00324     return get_sub_tree_iterator( root, box, box + 3, iter );
00325 }
00326 
00327 ErrorCode AdaptiveKDTree::get_last_iterator( EntityHandle root, AdaptiveKDTreeIter& iter )
00328 {
00329     double box[6];
00330     ErrorCode rval = moab()->tag_get_data( boxTag, &root, 1, box );
00331     if( MB_SUCCESS != rval ) return rval;
00332 
00333     return iter.initialize( this, root, box, box + 3, AdaptiveKDTreeIter::RIGHT );
00334 }
00335 
00336 ErrorCode AdaptiveKDTree::get_sub_tree_iterator( EntityHandle root,
00337                                                  const double min[3],
00338                                                  const double max[3],
00339                                                  AdaptiveKDTreeIter& result )
00340 {
00341     return result.initialize( this, root, min, max, AdaptiveKDTreeIter::LEFT );
00342 }
00343 
00344 ErrorCode AdaptiveKDTree::split_leaf( AdaptiveKDTreeIter& leaf, Plane plane, EntityHandle& left, EntityHandle& right )
00345 {
00346     ErrorCode rval;
00347 
00348     rval = moab()->create_meshset( meshsetFlags, left );
00349     if( MB_SUCCESS != rval ) return rval;
00350 
00351     rval = moab()->create_meshset( meshsetFlags, right );
00352     if( MB_SUCCESS != rval )
00353     {
00354         moab()->delete_entities( &left, 1 );
00355         return rval;
00356     }
00357 
00358     if( MB_SUCCESS != set_split_plane( leaf.handle(), plane ) ||
00359         MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), left ) ||
00360         MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), right ) ||
00361         MB_SUCCESS != leaf.step_to_first_leaf( AdaptiveKDTreeIter::LEFT ) )
00362     {
00363         EntityHandle children[] = { left, right };
00364         moab()->delete_entities( children, 2 );
00365         return MB_FAILURE;
00366     }
00367 
00368     return MB_SUCCESS;
00369 }
00370 
00371 ErrorCode AdaptiveKDTree::split_leaf( AdaptiveKDTreeIter& leaf, Plane plane )
00372 {
00373     EntityHandle left, right;
00374     return split_leaf( leaf, plane, left, right );
00375 }
00376 
00377 ErrorCode AdaptiveKDTree::split_leaf( AdaptiveKDTreeIter& leaf,
00378                                       Plane plane,
00379                                       const Range& left_entities,
00380                                       const Range& right_entities )
00381 {
00382     EntityHandle left, right, parent = leaf.handle();
00383     ErrorCode rval = split_leaf( leaf, plane, left, right );
00384     if( MB_SUCCESS != rval ) return rval;
00385 
00386     if( MB_SUCCESS == moab()->add_entities( left, left_entities ) &&
00387         MB_SUCCESS == moab()->add_entities( right, right_entities ) &&
00388         MB_SUCCESS == moab()->clear_meshset( &parent, 1 ) )
00389         return MB_SUCCESS;
00390 
00391     moab()->remove_child_meshset( parent, left );
00392     moab()->remove_child_meshset( parent, right );
00393     EntityHandle children[] = { left, right };
00394     moab()->delete_entities( children, 2 );
00395     return MB_FAILURE;
00396 }
00397 
00398 ErrorCode AdaptiveKDTree::split_leaf( AdaptiveKDTreeIter& leaf,
00399                                       Plane plane,
00400                                       const std::vector< EntityHandle >& left_entities,
00401                                       const std::vector< EntityHandle >& right_entities )
00402 {
00403     EntityHandle left, right, parent = leaf.handle();
00404     ErrorCode rval = split_leaf( leaf, plane, left, right );
00405     if( MB_SUCCESS != rval ) return rval;
00406 
00407     if( MB_SUCCESS == moab()->add_entities( left, &left_entities[0], left_entities.size() ) &&
00408         MB_SUCCESS == moab()->add_entities( right, &right_entities[0], right_entities.size() ) &&
00409         MB_SUCCESS == moab()->clear_meshset( &parent, 1 ) )
00410         return MB_SUCCESS;
00411 
00412     moab()->remove_child_meshset( parent, left );
00413     moab()->remove_child_meshset( parent, right );
00414     EntityHandle children[] = { left, right };
00415     moab()->delete_entities( children, 2 );
00416     return MB_FAILURE;
00417 }
00418 
00419 ErrorCode AdaptiveKDTree::merge_leaf( AdaptiveKDTreeIter& iter )
00420 {
00421     ErrorCode rval;
00422     if( iter.depth() == 1 )  // at root
00423         return MB_FAILURE;
00424 
00425     // Move iter to parent
00426 
00427     AdaptiveKDTreeIter::StackObj node = iter.mStack.back();
00428     iter.mStack.pop_back();
00429 
00430     iter.childVect.clear();
00431     rval = moab()->get_child_meshsets( iter.mStack.back().entity, iter.childVect );
00432     if( MB_SUCCESS != rval ) return rval;
00433     Plane plane;
00434     rval = get_split_plane( iter.mStack.back().entity, plane );
00435     if( MB_SUCCESS != rval ) return rval;
00436 
00437     int child_idx = iter.childVect[0] == node.entity ? 0 : 1;
00438     assert( iter.childVect[child_idx] == node.entity );
00439     iter.mBox[1 - child_idx][plane.norm] = node.coord;
00440 
00441     // Get all entities from children and put them in parent
00442     EntityHandle parent = iter.handle();
00443     moab()->remove_child_meshset( parent, iter.childVect[0] );
00444     moab()->remove_child_meshset( parent, iter.childVect[1] );
00445     std::vector< EntityHandle > stack( iter.childVect );
00446 
00447     Range range;
00448     while( !stack.empty() )
00449     {
00450         EntityHandle h = stack.back();
00451         stack.pop_back();
00452         range.clear();
00453         rval = moab()->get_entities_by_handle( h, range );
00454         if( MB_SUCCESS != rval ) return rval;
00455         rval = moab()->add_entities( parent, range );
00456         if( MB_SUCCESS != rval ) return rval;
00457 
00458         iter.childVect.clear();
00459         rval = moab()->get_child_meshsets( h, iter.childVect );MB_CHK_ERR( rval );
00460         if( !iter.childVect.empty() )
00461         {
00462             moab()->remove_child_meshset( h, iter.childVect[0] );
00463             moab()->remove_child_meshset( h, iter.childVect[1] );
00464             stack.push_back( iter.childVect[0] );
00465             stack.push_back( iter.childVect[1] );
00466         }
00467 
00468         rval = moab()->delete_entities( &h, 1 );
00469         if( MB_SUCCESS != rval ) return rval;
00470     }
00471 
00472     return MB_SUCCESS;
00473 }
00474 
00475 ErrorCode AdaptiveKDTreeIter::initialize( AdaptiveKDTree* ttool,
00476                                           EntityHandle root,
00477                                           const double bmin[3],
00478                                           const double bmax[3],
00479                                           Direction direction )
00480 {
00481     mStack.clear();
00482     treeTool      = ttool;
00483     mBox[BMIN][0] = bmin[0];
00484     mBox[BMIN][1] = bmin[1];
00485     mBox[BMIN][2] = bmin[2];
00486     mBox[BMAX][0] = bmax[0];
00487     mBox[BMAX][1] = bmax[1];
00488     mBox[BMAX][2] = bmax[2];
00489     mStack.push_back( StackObj( root, 0 ) );
00490     return step_to_first_leaf( direction );
00491 }
00492 
00493 ErrorCode AdaptiveKDTreeIter::step_to_first_leaf( Direction direction )
00494 {
00495     ErrorCode rval;
00496     AdaptiveKDTree::Plane plane;
00497     const Direction opposite = static_cast< Direction >( 1 - direction );
00498 
00499     for( ;; )
00500     {
00501         childVect.clear();
00502         treeTool->treeStats.nodesVisited++;  // not sure whether this is the visit or the push_back below
00503         rval = treeTool->moab()->get_child_meshsets( mStack.back().entity, childVect );
00504         if( MB_SUCCESS != rval ) return rval;
00505         if( childVect.empty() )
00506         {  // leaf
00507             treeTool->treeStats.leavesVisited++;
00508             break;
00509         }
00510 
00511         rval = treeTool->get_split_plane( mStack.back().entity, plane );
00512         if( MB_SUCCESS != rval ) return rval;
00513 
00514         mStack.push_back( StackObj( childVect[direction], mBox[opposite][plane.norm] ) );
00515         mBox[opposite][plane.norm] = plane.coord;
00516     }
00517     return MB_SUCCESS;
00518 }
00519 
00520 ErrorCode AdaptiveKDTreeIter::step( Direction direction )
00521 {
00522     StackObj node, parent;
00523     ErrorCode rval;
00524     AdaptiveKDTree::Plane plane;
00525     const Direction opposite = static_cast< Direction >( 1 - direction );
00526 
00527     // If stack is empty, then either this iterator is uninitialized
00528     // or we reached the end of the iteration (and return
00529     // MB_ENTITY_NOT_FOUND) already.
00530     if( mStack.empty() ) return MB_FAILURE;
00531 
00532     // Pop the current node from the stack.
00533     // The stack should then contain the parent of the current node.
00534     // If the stack is empty after this pop, then we've reached the end.
00535     node = mStack.back();
00536     mStack.pop_back();
00537     treeTool->treeStats.nodesVisited++;
00538     if( mStack.empty() ) treeTool->treeStats.leavesVisited++;
00539 
00540     while( !mStack.empty() )
00541     {
00542         // Get data for parent entity
00543         parent = mStack.back();
00544         childVect.clear();
00545         rval = treeTool->moab()->get_child_meshsets( parent.entity, childVect );
00546         if( MB_SUCCESS != rval ) return rval;
00547         rval = treeTool->get_split_plane( parent.entity, plane );
00548         if( MB_SUCCESS != rval ) return rval;
00549 
00550         // If we're at the left child
00551         if( childVect[opposite] == node.entity )
00552         {
00553             // change from box of left child to box of parent
00554             mBox[direction][plane.norm] = node.coord;
00555             // push right child on stack
00556             node.entity = childVect[direction];
00557             treeTool->treeStats.nodesVisited++;  // changed node
00558             node.coord = mBox[opposite][plane.norm];
00559             mStack.push_back( node );
00560             // change from box of parent to box of right child
00561             mBox[opposite][plane.norm] = plane.coord;
00562             // descend to left-most leaf of the right child
00563             return step_to_first_leaf( opposite );
00564         }
00565 
00566         // The current node is the right child of the parent,
00567         // continue up the tree.
00568         assert( childVect[direction] == node.entity );
00569         mBox[opposite][plane.norm] = node.coord;
00570         node                       = parent;
00571         treeTool->treeStats.nodesVisited++;
00572         mStack.pop_back();
00573     }
00574 
00575     return MB_ENTITY_NOT_FOUND;
00576 }
00577 
00578 ErrorCode AdaptiveKDTreeIter::get_neighbors( AdaptiveKDTree::Axis norm,
00579                                              bool neg,
00580                                              std::vector< AdaptiveKDTreeIter >& results,
00581                                              double epsilon ) const
00582 {
00583     StackObj node, parent;
00584     ErrorCode rval;
00585     AdaptiveKDTree::Plane plane;
00586     int child_idx;
00587 
00588     // Find tree node at which the specified side of the box
00589     // for this node was created.
00590     AdaptiveKDTreeIter iter( *this );  // temporary iterator (don't modify *this)
00591     node = iter.mStack.back();
00592     iter.mStack.pop_back();
00593     for( ;; )
00594     {
00595         // reached the root - original node was on boundary (no neighbors)
00596         if( iter.mStack.empty() ) return MB_SUCCESS;
00597 
00598         // get parent node data
00599         parent = iter.mStack.back();
00600         iter.childVect.clear();
00601         rval = treeTool->moab()->get_child_meshsets( parent.entity, iter.childVect );
00602         if( MB_SUCCESS != rval ) return rval;
00603         rval = treeTool->get_split_plane( parent.entity, plane );
00604         if( MB_SUCCESS != rval ) return rval;
00605 
00606         child_idx = iter.childVect[0] == node.entity ? 0 : 1;
00607         assert( iter.childVect[child_idx] == node.entity );
00608 
00609         // if we found the split plane for the desired side
00610         // push neighbor on stack and stop
00611         if( plane.norm == norm && (int)neg == child_idx )
00612         {
00613             // change from box of previous child to box of parent
00614             iter.mBox[1 - child_idx][plane.norm] = node.coord;
00615             // push other child of parent onto stack
00616             node.entity = iter.childVect[1 - child_idx];
00617             node.coord  = iter.mBox[child_idx][plane.norm];
00618             iter.mStack.push_back( node );
00619             // change from parent box to box of new child
00620             iter.mBox[child_idx][plane.norm] = plane.coord;
00621             break;
00622         }
00623 
00624         // continue up the tree
00625         iter.mBox[1 - child_idx][plane.norm] = node.coord;
00626         node                                 = parent;
00627         iter.mStack.pop_back();
00628     }
00629 
00630     // now move down tree, searching for adjacent boxes
00631     std::vector< AdaptiveKDTreeIter > list;
00632     // loop over all potential paths to neighbors (until list is empty)
00633     for( ;; )
00634     {
00635         // follow a single path to a leaf, append any other potential
00636         // paths to neighbors to 'list'
00637         node = iter.mStack.back();
00638         for( ;; )
00639         {
00640             iter.childVect.clear();
00641             rval = treeTool->moab()->get_child_meshsets( node.entity, iter.childVect );
00642             if( MB_SUCCESS != rval ) return rval;
00643 
00644             // if leaf
00645             if( iter.childVect.empty() )
00646             {
00647                 results.push_back( iter );
00648                 break;
00649             }
00650 
00651             rval = treeTool->get_split_plane( node.entity, plane );
00652             if( MB_SUCCESS != rval ) return rval;
00653 
00654             // if split parallel to side
00655             if( plane.norm == norm )
00656             {
00657                 // continue with whichever child is on the correct side of the split
00658                 node.entity = iter.childVect[neg];
00659                 node.coord  = iter.mBox[1 - neg][plane.norm];
00660                 iter.mStack.push_back( node );
00661                 iter.mBox[1 - neg][plane.norm] = plane.coord;
00662             }
00663             // if left child is adjacent
00664             else if( this->mBox[BMIN][plane.norm] - plane.coord <= epsilon )
00665             {
00666                 // if right child is also adjacent, add to list
00667                 if( plane.coord - this->mBox[BMAX][plane.norm] <= epsilon )
00668                 {
00669                     list.push_back( iter );
00670                     list.back().mStack.push_back( StackObj( iter.childVect[1], iter.mBox[BMIN][plane.norm] ) );
00671                     list.back().mBox[BMIN][plane.norm] = plane.coord;
00672                 }
00673                 // continue with left child
00674                 node.entity = iter.childVect[0];
00675                 node.coord  = iter.mBox[BMAX][plane.norm];
00676                 iter.mStack.push_back( node );
00677                 iter.mBox[BMAX][plane.norm] = plane.coord;
00678             }
00679             // right child is adjacent
00680             else
00681             {
00682                 // if left child is not adjacent, right must be or something
00683                 // is really messed up.
00684                 assert( plane.coord - this->mBox[BMAX][plane.norm] <= epsilon );
00685                 // continue with left child
00686                 node.entity = iter.childVect[1];
00687                 node.coord  = iter.mBox[BMIN][plane.norm];
00688                 iter.mStack.push_back( node );
00689                 iter.mBox[BMIN][plane.norm] = plane.coord;
00690             }
00691         }
00692 
00693         if( list.empty() ) break;
00694 
00695         iter = list.back();
00696         list.pop_back();
00697     }
00698 
00699     return MB_SUCCESS;
00700 }
00701 
00702 ErrorCode AdaptiveKDTreeIter::sibling_side( AdaptiveKDTree::Axis& axis_out, bool& neg_out ) const
00703 {
00704     if( mStack.size() < 2 )  // at tree root
00705         return MB_ENTITY_NOT_FOUND;
00706 
00707     EntityHandle parent = mStack[mStack.size() - 2].entity;
00708     AdaptiveKDTree::Plane plane;
00709     ErrorCode rval = tool()->get_split_plane( parent, plane );
00710     if( MB_SUCCESS != rval ) return MB_FAILURE;
00711 
00712     childVect.clear();
00713     rval = tool()->moab()->get_child_meshsets( parent, childVect );
00714     if( MB_SUCCESS != rval || childVect.size() != 2 ) return MB_FAILURE;
00715 
00716     axis_out = static_cast< AdaptiveKDTree::Axis >( plane.norm );
00717     neg_out  = ( childVect[1] == handle() );
00718     assert( childVect[neg_out] == handle() );
00719     return MB_SUCCESS;
00720 }
00721 
00722 ErrorCode AdaptiveKDTreeIter::get_parent_split_plane( AdaptiveKDTree::Plane& plane ) const
00723 {
00724     if( mStack.size() < 2 )  // at tree root
00725         return MB_ENTITY_NOT_FOUND;
00726 
00727     EntityHandle parent = mStack[mStack.size() - 2].entity;
00728     return tool()->get_split_plane( parent, plane );
00729 }
00730 
00731 bool AdaptiveKDTreeIter::is_sibling( const AdaptiveKDTreeIter& other_leaf ) const
00732 {
00733     const size_t s = mStack.size();
00734     return ( s > 1 ) && ( s == other_leaf.mStack.size() ) &&
00735            ( other_leaf.mStack[s - 2].entity == mStack[s - 2].entity ) && other_leaf.handle() != handle();
00736 }
00737 
00738 bool AdaptiveKDTreeIter::is_sibling( EntityHandle other_leaf ) const
00739 {
00740     if( mStack.size() < 2 || other_leaf == handle() ) return false;
00741     EntityHandle parent = mStack[mStack.size() - 2].entity;
00742     childVect.clear();
00743     ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
00744     if( MB_SUCCESS != rval || childVect.size() != 2 )
00745     {
00746         assert( false );
00747         return false;
00748     }
00749     return childVect[0] == other_leaf || childVect[1] == other_leaf;
00750 }
00751 
00752 bool AdaptiveKDTreeIter::sibling_is_forward() const
00753 {
00754     if( mStack.size() < 2 )  // if root
00755         return false;
00756     EntityHandle parent = mStack[mStack.size() - 2].entity;
00757     childVect.clear();
00758     ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
00759     if( MB_SUCCESS != rval || childVect.size() != 2 )
00760     {
00761         assert( false );
00762         return false;
00763     }
00764     return childVect[0] == handle();
00765 }
00766 
00767 bool AdaptiveKDTreeIter::intersect_ray( const double ray_point[3],
00768                                         const double ray_vect[3],
00769                                         double& t_enter,
00770                                         double& t_exit ) const
00771 {
00772     treeTool->treeStats.traversalLeafObjectTests++;
00773     return GeomUtil::ray_box_intersect( CartVect( box_min() ), CartVect( box_max() ), CartVect( ray_point ),
00774                                         CartVect( ray_vect ), t_enter, t_exit );
00775 }
00776 
00777 ErrorCode AdaptiveKDTree::intersect_children_with_elems( const Range& elems,
00778                                                          AdaptiveKDTree::Plane plane,
00779                                                          double eps,
00780                                                          CartVect box_min,
00781                                                          CartVect box_max,
00782                                                          Range& left_tris,
00783                                                          Range& right_tris,
00784                                                          Range& both_tris,
00785                                                          double& metric_value )
00786 {
00787     left_tris.clear();
00788     right_tris.clear();
00789     both_tris.clear();
00790     CartVect coords[16];
00791 
00792     // get extents of boxes for left and right sides
00793     BoundBox left_box( box_min, box_max ), right_box( box_min, box_max );
00794     right_box.bMin             = box_min;
00795     left_box.bMax              = box_max;
00796     right_box.bMin[plane.norm] = left_box.bMax[plane.norm] = plane.coord;
00797     const CartVect left_cen                                = 0.5 * ( left_box.bMax + box_min );
00798     const CartVect left_dim                                = 0.5 * ( left_box.bMax - box_min );
00799     const CartVect right_cen                               = 0.5 * ( box_max + right_box.bMin );
00800     const CartVect right_dim                               = 0.5 * ( box_max - right_box.bMin );
00801     const CartVect dim                                     = box_max - box_min;
00802     const double max_tol                                   = std::max( dim[0], std::max( dim[1], dim[2] ) ) / 10;
00803 
00804     // test each entity
00805     ErrorCode rval;
00806     int count, count2;
00807     const EntityHandle *conn, *conn2;
00808 
00809     const Range::const_iterator elem_begin = elems.lower_bound( MBEDGE );
00810     const Range::const_iterator poly_begin = elems.lower_bound( MBPOLYHEDRON, elem_begin );
00811     const Range::const_iterator set_begin  = elems.lower_bound( MBENTITYSET, poly_begin );
00812     Range::iterator left_ins               = left_tris.begin();
00813     Range::iterator right_ins              = right_tris.begin();
00814     Range::iterator both_ins               = both_tris.begin();
00815     Range::const_iterator i;
00816 
00817     // vertices
00818     for( i = elems.begin(); i != elem_begin; ++i )
00819     {
00820         tree_stats().constructLeafObjectTests++;
00821         rval = moab()->get_coords( &*i, 1, coords[0].array() );
00822         if( MB_SUCCESS != rval ) return rval;
00823 
00824         bool lo = false, ro = false;
00825         if( coords[0][plane.norm] <= plane.coord ) lo = true;
00826         if( coords[0][plane.norm] >= plane.coord ) ro = true;
00827 
00828         if( lo && ro )
00829             both_ins = both_tris.insert( both_ins, *i, *i );
00830         else if( lo )
00831             left_ins = left_tris.insert( left_ins, *i, *i );
00832         else  // if (ro)
00833             right_ins = right_tris.insert( right_ins, *i, *i );
00834     }
00835 
00836     // non-polyhedron elements
00837     std::vector< EntityHandle > dum_vector;
00838     for( i = elem_begin; i != poly_begin; ++i )
00839     {
00840         tree_stats().constructLeafObjectTests++;
00841         rval = moab()->get_connectivity( *i, conn, count, true, &dum_vector );
00842         if( MB_SUCCESS != rval ) return rval;
00843         if( count > (int)( sizeof( coords ) / sizeof( coords[0] ) ) ) return MB_FAILURE;
00844         rval = moab()->get_coords( &conn[0], count, coords[0].array() );
00845         if( MB_SUCCESS != rval ) return rval;
00846 
00847         bool lo = false, ro = false;
00848         for( int j = 0; j < count; ++j )
00849         {
00850             if( coords[j][plane.norm] <= plane.coord ) lo = true;
00851             if( coords[j][plane.norm] >= plane.coord ) ro = true;
00852         }
00853 
00854         // Triangle must be in at least one leaf.  If test against plane
00855         // identified that leaf, then we're done.  If triangle is on both
00856         // sides of plane, do more precise test to ensure that it is really
00857         // in both.
00858         //        BoundBox box;
00859         //        box.update(*moab(), *i);
00860         if( lo && ro )
00861         {
00862             double tol = eps;
00863             lo = ro = false;
00864             while( !lo && !ro && tol <= max_tol )
00865             {
00866                 tree_stats().boxElemTests += 2;
00867                 lo = GeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE( *i ), left_cen, left_dim + CartVect( tol ),
00868                                                  count );
00869                 ro = GeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE( *i ), right_cen, right_dim + CartVect( tol ),
00870                                                  count );
00871 
00872                 tol *= 10.0;
00873             }
00874         }
00875         if( lo && ro )
00876             both_ins = both_tris.insert( both_ins, *i, *i );
00877         else if( lo )
00878             left_ins = left_tris.insert( left_ins, *i, *i );
00879         else if( ro )
00880             right_ins = right_tris.insert( right_ins, *i, *i );
00881     }
00882 
00883     // polyhedra
00884     for( i = poly_begin; i != set_begin; ++i )
00885     {
00886         tree_stats().constructLeafObjectTests++;
00887         rval = moab()->get_connectivity( *i, conn, count, true );
00888         if( MB_SUCCESS != rval ) return rval;
00889 
00890         // just check the bounding box of the polyhedron
00891         bool lo = false, ro = false;
00892         for( int j = 0; j < count; ++j )
00893         {
00894             rval = moab()->get_connectivity( conn[j], conn2, count2, true );
00895             if( MB_SUCCESS != rval ) return rval;
00896 
00897             for( int k = 0; k < count2; ++k )
00898             {
00899                 rval = moab()->get_coords( conn2 + k, 1, coords[0].array() );
00900                 if( MB_SUCCESS != rval ) return rval;
00901                 if( coords[0][plane.norm] <= plane.coord ) lo = true;
00902                 if( coords[0][plane.norm] >= plane.coord ) ro = true;
00903             }
00904         }
00905 
00906         if( lo && ro )
00907             both_ins = both_tris.insert( both_ins, *i, *i );
00908         else if( lo )
00909             left_ins = left_tris.insert( left_ins, *i, *i );
00910         else if( ro )
00911             right_ins = right_tris.insert( right_ins, *i, *i );
00912     }
00913 
00914     // sets
00915     BoundBox tbox;
00916     for( i = set_begin; i != elems.end(); ++i )
00917     {
00918         tree_stats().constructLeafObjectTests++;
00919         rval = tbox.update( *moab(), *i, spherical, radius );
00920         if( MB_SUCCESS != rval ) return rval;
00921 
00922         bool lo = false, ro = false;
00923         if( tbox.bMin[plane.norm] <= plane.coord ) lo = true;
00924         if( tbox.bMax[plane.norm] >= plane.coord ) ro = true;
00925 
00926         if( lo && ro )
00927             both_ins = both_tris.insert( both_ins, *i, *i );
00928         else if( lo )
00929             left_ins = left_tris.insert( left_ins, *i, *i );
00930         else  // if (ro)
00931             right_ins = right_tris.insert( right_ins, *i, *i );
00932     }
00933 
00934     CartVect box_dim  = box_max - box_min;
00935     double area_left  = left_dim[0] * left_dim[1] + left_dim[1] * left_dim[2] + left_dim[2] * left_dim[0];
00936     double area_right = right_dim[0] * right_dim[1] + right_dim[1] * right_dim[2] + right_dim[2] * right_dim[0];
00937     double area_both  = box_dim[0] * box_dim[1] + box_dim[1] * box_dim[2] + box_dim[2] * box_dim[0];
00938     metric_value = ( area_left * left_tris.size() + area_right * right_tris.size() ) / area_both + both_tris.size();
00939     return MB_SUCCESS;
00940 }
00941 
00942 ErrorCode AdaptiveKDTree::best_subdivision_plane( int num_planes,
00943                                                   const AdaptiveKDTreeIter& iter,
00944                                                   Range& best_left,
00945                                                   Range& best_right,
00946                                                   Range& best_both,
00947                                                   AdaptiveKDTree::Plane& best_plane,
00948                                                   double eps )
00949 {
00950     double metric_val = std::numeric_limits< unsigned >::max();
00951 
00952     ErrorCode r;
00953     const CartVect box_min( iter.box_min() );
00954     const CartVect box_max( iter.box_max() );
00955     const CartVect diff( box_max - box_min );
00956 
00957     Range entities;
00958     r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
00959     if( MB_SUCCESS != r ) return r;
00960     const size_t p_count = entities.size();
00961 
00962     for( int axis = 0; axis < 3; ++axis )
00963     {
00964         int plane_count = num_planes;
00965         if( ( num_planes + 1 ) * eps >= diff[axis] ) plane_count = (int)( diff[axis] / eps ) - 1;
00966 
00967         for( int p = 1; p <= plane_count; ++p )
00968         {
00969             AdaptiveKDTree::Plane plane = { box_min[axis] + ( p / ( 1.0 + plane_count ) ) * diff[axis], axis };
00970             Range left, right, both;
00971             double val;
00972             r = intersect_children_with_elems( entities, plane, eps, box_min, box_max, left, right, both, val );
00973             if( MB_SUCCESS != r ) return r;
00974             const size_t sdiff = p_count - both.size();
00975             if( left.size() == sdiff || right.size() == sdiff ) continue;
00976 
00977             if( val >= metric_val ) continue;
00978 
00979             metric_val = val;
00980             best_plane = plane;
00981             best_left.swap( left );
00982             best_right.swap( right );
00983             best_both.swap( both );
00984         }
00985     }
00986 
00987     return MB_SUCCESS;
00988 }
00989 
00990 ErrorCode AdaptiveKDTree::best_subdivision_snap_plane( int num_planes,
00991                                                        const AdaptiveKDTreeIter& iter,
00992                                                        Range& best_left,
00993                                                        Range& best_right,
00994                                                        Range& best_both,
00995                                                        AdaptiveKDTree::Plane& best_plane,
00996                                                        std::vector< double >& tmp_data,
00997                                                        double eps )
00998 {
00999     double metric_val = std::numeric_limits< unsigned >::max();
01000 
01001     ErrorCode r;
01002     // const CartVect tol(eps*diff);
01003 
01004     Range entities, vertices;
01005     r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
01006     if( MB_SUCCESS != r ) return r;
01007     const size_t p_count = entities.size();
01008     r                    = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, Interface::UNION );
01009     if( MB_SUCCESS != r ) return r;
01010 
01011     unsigned int nverts = vertices.size();
01012     tmp_data.resize( 3 * nverts );
01013     r = iter.tool()->moab()->get_coords( vertices, &tmp_data[0], &tmp_data[nverts], &tmp_data[2 * nverts] );
01014     if( MB_SUCCESS != r ) return r;
01015 
01016     // calculate bounding box of vertices
01017     // decide based on the actual box the splitting plane
01018     // do not decide based on iterator box.
01019     // it could be too big
01020     // BoundBox box;
01021     // r = box.update(*moab(), vertices);
01022     CartVect box_min;
01023     CartVect box_max;
01024     for( int dir = 0; dir < 3; dir++ )
01025     {
01026         double amin = tmp_data[dir * nverts];
01027         double amax = amin;
01028         double* p   = &tmp_data[dir * nverts + 1];
01029         for( unsigned int i = 1; i < nverts; i++ )
01030         {
01031             if( *p < amin ) amin = *p;
01032             if( *p > amax ) amax = *p;
01033             p++;
01034         }
01035         box_min[dir] = amin;
01036         box_max[dir] = amax;
01037     }
01038     CartVect diff( box_max - box_min );
01039 
01040     for( int axis = 0; axis < 3; ++axis )
01041     {
01042         int plane_count = num_planes;
01043 
01044         // if num_planes results in width < eps, reset the plane count
01045         if( ( num_planes + 1 ) * eps >= diff[axis] ) plane_count = (int)( diff[axis] / eps ) - 1;
01046 
01047         for( int p = 1; p <= plane_count; ++p )
01048         {
01049 
01050             // coord of this plane on axis
01051             double coord = box_min[axis] + ( p / ( 1.0 + plane_count ) ) * diff[axis];
01052 
01053             // find closest vertex coordinate to this plane position
01054             unsigned int istrt   = axis * nverts;
01055             double closest_coord = tmp_data[istrt];
01056             for( unsigned i = 1; i < nverts; ++i )
01057                 if( fabs( coord - tmp_data[istrt + i] ) < fabs( coord - closest_coord ) )
01058                     closest_coord = tmp_data[istrt + i];
01059             if( closest_coord - box_min[axis] <= eps || box_max[axis] - closest_coord <= eps ) continue;
01060 
01061             // seprate elems into left/right/both, and compute separating metric
01062             AdaptiveKDTree::Plane plane = { closest_coord, axis };
01063             Range left, right, both;
01064             double val;
01065             r = intersect_children_with_elems( entities, plane, eps, box_min, box_max, left, right, both, val );
01066             if( MB_SUCCESS != r ) return r;
01067             const size_t d = p_count - both.size();
01068             if( left.size() == d || right.size() == d ) continue;
01069 
01070             if( val >= metric_val ) continue;
01071 
01072             metric_val = val;
01073             best_plane = plane;
01074             best_left.swap( left );
01075             best_right.swap( right );
01076             best_both.swap( both );
01077         }
01078     }
01079 
01080     return MB_SUCCESS;
01081 }
01082 
01083 ErrorCode AdaptiveKDTree::best_vertex_median_plane( int num_planes,
01084                                                     const AdaptiveKDTreeIter& iter,
01085                                                     Range& best_left,
01086                                                     Range& best_right,
01087                                                     Range& best_both,
01088                                                     AdaptiveKDTree::Plane& best_plane,
01089                                                     std::vector< double >& coords,
01090                                                     double eps )
01091 {
01092     double metric_val = std::numeric_limits< unsigned >::max();
01093 
01094     ErrorCode r;
01095     const CartVect box_min( iter.box_min() );
01096     const CartVect box_max( iter.box_max() );
01097 
01098     Range entities, vertices;
01099     r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
01100     if( MB_SUCCESS != r ) return r;
01101     const size_t p_count = entities.size();
01102     r                    = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, Interface::UNION );
01103     if( MB_SUCCESS != r ) return r;
01104 
01105     coords.resize( vertices.size() );
01106     for( int axis = 0; axis < 3; ++axis )
01107     {
01108         if( box_max[axis] - box_min[axis] <= 2 * eps ) continue;
01109 
01110         double* ptrs[] = { 0, 0, 0 };
01111         ptrs[axis]     = &coords[0];
01112         r              = iter.tool()->moab()->get_coords( vertices, ptrs[0], ptrs[1], ptrs[2] );
01113         if( MB_SUCCESS != r ) return r;
01114 
01115         std::sort( coords.begin(), coords.end() );
01116         std::vector< double >::iterator citer;
01117         citer              = std::upper_bound( coords.begin(), coords.end(), box_min[axis] + eps );
01118         const size_t count = std::upper_bound( citer, coords.end(), box_max[axis] - eps ) - citer;
01119         size_t step;
01120         int np = num_planes;
01121         if( count < 2 * (size_t)num_planes )
01122         {
01123             step = 1;
01124             np   = count - 1;
01125         }
01126         else
01127         {
01128             step = count / ( num_planes + 1 );
01129         }
01130 
01131         for( int p = 1; p <= np; ++p )
01132         {
01133 
01134             citer += step;
01135             AdaptiveKDTree::Plane plane = { *citer, axis };
01136             Range left, right, both;
01137             double val;
01138             r = intersect_children_with_elems( entities, plane, eps, box_min, box_max, left, right, both, val );
01139             if( MB_SUCCESS != r ) return r;
01140             const size_t diff = p_count - both.size();
01141             if( left.size() == diff || right.size() == diff ) continue;
01142 
01143             if( val >= metric_val ) continue;
01144 
01145             metric_val = val;
01146             best_plane = plane;
01147             best_left.swap( left );
01148             best_right.swap( right );
01149             best_both.swap( both );
01150         }
01151     }
01152 
01153     return MB_SUCCESS;
01154 }
01155 
01156 ErrorCode AdaptiveKDTree::best_vertex_sample_plane( int num_planes,
01157                                                     const AdaptiveKDTreeIter& iter,
01158                                                     Range& best_left,
01159                                                     Range& best_right,
01160                                                     Range& best_both,
01161                                                     AdaptiveKDTree::Plane& best_plane,
01162                                                     std::vector< double >& coords,
01163                                                     std::vector< EntityHandle >& indices,
01164                                                     double eps )
01165 {
01166     const size_t random_elem_threshold = 20 * num_planes;
01167     double metric_val                  = std::numeric_limits< unsigned >::max();
01168 
01169     ErrorCode r;
01170     const CartVect box_min( iter.box_min() );
01171     const CartVect box_max( iter.box_max() );
01172 
01173     Range entities, vertices;
01174     r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
01175     if( MB_SUCCESS != r ) return r;
01176 
01177     // We are selecting random vertex coordinates to use for candidate split
01178     // planes.  So if element list is large, begin by selecting random elements.
01179     const size_t p_count = entities.size();
01180     coords.resize( 3 * num_planes );
01181     if( p_count < random_elem_threshold )
01182     {
01183         r = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, Interface::UNION );
01184         if( MB_SUCCESS != r ) return r;
01185     }
01186     else
01187     {
01188         indices.resize( random_elem_threshold );
01189         const int num_rand = p_count / RAND_MAX + 1;
01190         for( size_t j = 0; j < random_elem_threshold; ++j )
01191         {
01192             size_t rnd = rand();
01193             for( int i = num_rand; i > 1; --i )
01194                 rnd *= rand();
01195             rnd %= p_count;
01196             indices[j] = entities[rnd];
01197         }
01198         r = iter.tool()->moab()->get_adjacencies( &indices[0], random_elem_threshold, 0, false, vertices,
01199                                                   Interface::UNION );
01200         if( MB_SUCCESS != r ) return r;
01201     }
01202 
01203     coords.resize( vertices.size() );
01204     for( int axis = 0; axis < 3; ++axis )
01205     {
01206         if( box_max[axis] - box_min[axis] <= 2 * eps ) continue;
01207 
01208         double* ptrs[] = { 0, 0, 0 };
01209         ptrs[axis]     = &coords[0];
01210         r              = iter.tool()->moab()->get_coords( vertices, ptrs[0], ptrs[1], ptrs[2] );
01211         if( MB_SUCCESS != r ) return r;
01212 
01213         size_t num_valid_coords = 0;
01214         for( size_t i = 0; i < coords.size(); ++i )
01215             if( coords[i] > box_min[axis] + eps && coords[i] < box_max[axis] - eps ) ++num_valid_coords;
01216 
01217         if( 2 * (size_t)num_planes > num_valid_coords )
01218         {
01219             indices.clear();
01220             for( size_t i = 0; i < coords.size(); ++i )
01221                 if( coords[i] > box_min[axis] + eps && coords[i] < box_max[axis] - eps ) indices.push_back( i );
01222         }
01223         else
01224         {
01225             indices.resize( num_planes );
01226             // make sure random indices are sufficient to cover entire range
01227             const int num_rand = coords.size() / RAND_MAX + 1;
01228             for( int j = 0; j < num_planes; ++j )
01229             {
01230                 size_t rnd;
01231                 do
01232                 {
01233                     rnd = rand();
01234                     for( int i = num_rand; i > 1; --i )
01235                         rnd *= rand();
01236                     rnd %= coords.size();
01237                 } while( coords[rnd] <= box_min[axis] + eps || coords[rnd] >= box_max[axis] - eps );
01238                 indices[j] = rnd;
01239             }
01240         }
01241 
01242         for( unsigned p = 0; p < indices.size(); ++p )
01243         {
01244 
01245             AdaptiveKDTree::Plane plane = { coords[indices[p]], axis };
01246             Range left, right, both;
01247             double val;
01248             r = intersect_children_with_elems( entities, plane, eps, box_min, box_max, left, right, both, val );
01249             if( MB_SUCCESS != r ) return r;
01250             const size_t diff = p_count - both.size();
01251             if( left.size() == diff || right.size() == diff ) continue;
01252 
01253             if( val >= metric_val ) continue;
01254 
01255             metric_val = val;
01256             best_plane = plane;
01257             best_left.swap( left );
01258             best_right.swap( right );
01259             best_both.swap( both );
01260         }
01261     }
01262 
01263     return MB_SUCCESS;
01264 }
01265 
01266 ErrorCode AdaptiveKDTree::point_search( const double* point,
01267                                         EntityHandle& leaf_out,
01268                                         const double iter_tol,
01269                                         const double inside_tol,
01270                                         bool* multiple_leaves,
01271                                         EntityHandle* start_node,
01272                                         CartVect* params )
01273 {
01274     std::vector< EntityHandle > children;
01275     Plane plane;
01276 
01277     treeStats.numTraversals++;
01278     leaf_out = 0;
01279     BoundBox box;
01280     // kdtrees never have multiple leaves containing a pt
01281     if( multiple_leaves ) *multiple_leaves = false;
01282 
01283     EntityHandle node = ( start_node ? *start_node : myRoot );
01284 
01285     treeStats.nodesVisited++;
01286     ErrorCode rval = get_bounding_box( box, &node );
01287     if( MB_SUCCESS != rval ) return rval;
01288     if( !box.contains_point( point, iter_tol ) ) return MB_SUCCESS;
01289 
01290     rval = moab()->get_child_meshsets( node, children );
01291     if( MB_SUCCESS != rval ) return rval;
01292 
01293     while( !children.empty() )
01294     {
01295         treeStats.nodesVisited++;
01296 
01297         rval = get_split_plane( node, plane );
01298         if( MB_SUCCESS != rval ) return rval;
01299 
01300         const double d = point[plane.norm] - plane.coord;
01301         node           = children[( d > 0.0 )];
01302 
01303         children.clear();
01304         rval = moab()->get_child_meshsets( node, children );
01305         if( MB_SUCCESS != rval ) return rval;
01306     }
01307 
01308     treeStats.leavesVisited++;
01309     if( myEval && params )
01310     {
01311         rval = myEval->find_containing_entity( node, point, iter_tol, inside_tol, leaf_out, params->array(),
01312                                                &treeStats.traversalLeafObjectTests );
01313         if( MB_SUCCESS != rval ) return rval;
01314     }
01315     else
01316         leaf_out = node;
01317 
01318     return MB_SUCCESS;
01319 }
01320 
01321 ErrorCode AdaptiveKDTree::point_search( const double* point,
01322                                         AdaptiveKDTreeIter& leaf_it,
01323                                         const double iter_tol,
01324                                         const double /*inside_tol*/,
01325                                         bool* multiple_leaves,
01326                                         EntityHandle* start_node )
01327 {
01328     ErrorCode rval;
01329     treeStats.numTraversals++;
01330 
01331     // kdtrees never have multiple leaves containing a pt
01332     if( multiple_leaves ) *multiple_leaves = false;
01333 
01334     leaf_it.mBox[0] = boundBox.bMin;
01335     leaf_it.mBox[1] = boundBox.bMax;
01336 
01337     // test that point is inside tree
01338     if( !boundBox.contains_point( point, iter_tol ) )
01339     {
01340         treeStats.nodesVisited++;
01341         return MB_ENTITY_NOT_FOUND;
01342     }
01343 
01344     // initialize iterator at tree root
01345     leaf_it.treeTool = this;
01346     leaf_it.mStack.clear();
01347     leaf_it.mStack.push_back( AdaptiveKDTreeIter::StackObj( ( start_node ? *start_node : myRoot ), 0 ) );
01348 
01349     // loop until we reach a leaf
01350     AdaptiveKDTree::Plane plane;
01351     for( ;; )
01352     {
01353         treeStats.nodesVisited++;
01354 
01355         // get children
01356         leaf_it.childVect.clear();
01357         rval = moab()->get_child_meshsets( leaf_it.handle(), leaf_it.childVect );
01358         if( MB_SUCCESS != rval ) return rval;
01359 
01360         // if no children, then at leaf (done)
01361         if( leaf_it.childVect.empty() )
01362         {
01363             treeStats.leavesVisited++;
01364             break;
01365         }
01366 
01367         // get split plane
01368         rval = get_split_plane( leaf_it.handle(), plane );
01369         if( MB_SUCCESS != rval ) return rval;
01370 
01371         // step iterator to appropriate child
01372         // idx: 0->left, 1->right
01373         const int idx = ( point[plane.norm] > plane.coord );
01374         leaf_it.mStack.push_back(
01375             AdaptiveKDTreeIter::StackObj( leaf_it.childVect[idx], leaf_it.mBox[1 - idx][plane.norm] ) );
01376         leaf_it.mBox[1 - idx][plane.norm] = plane.coord;
01377     }
01378 
01379     return MB_SUCCESS;
01380 }
01381 
01382 struct NodeDistance
01383 {
01384     EntityHandle handle;
01385     CartVect dist;  // from_point - closest_point_on_box
01386 };
01387 
01388 ErrorCode AdaptiveKDTree::distance_search( const double from_point[3],
01389                                            const double distance,
01390                                            std::vector< EntityHandle >& result_list,
01391                                            const double iter_tol,
01392                                            const double inside_tol,
01393                                            std::vector< double >* result_dists,
01394                                            std::vector< CartVect >* result_params,
01395                                            EntityHandle* tree_root )
01396 {
01397     treeStats.numTraversals++;
01398     const double dist_sqr = distance * distance;
01399     const CartVect from( from_point );
01400     std::vector< NodeDistance > list,
01401         result_list_nodes;  // list of subtrees to traverse, and results
01402                             // pre-allocate space for default max tree depth
01403     list.reserve( maxDepth );
01404 
01405     // misc temporary values
01406     Plane plane;
01407     NodeDistance node;
01408     ErrorCode rval;
01409     std::vector< EntityHandle > children;
01410 
01411     // Get distance from input position to bounding box of tree
01412     // (zero if inside box)
01413     BoundBox box;
01414     rval = get_bounding_box( box );
01415     if( MB_SUCCESS == rval && !box.contains_point( from_point, iter_tol ) )
01416     {
01417         treeStats.nodesVisited++;
01418         return MB_SUCCESS;
01419     }
01420 
01421     // if bounding box is not available (e.g. not starting from true root)
01422     // just start with zero.  Less efficient, but will work.
01423     node.dist = CartVect( 0.0 );
01424     if( MB_SUCCESS == rval )
01425     {
01426         for( int i = 0; i < 3; ++i )
01427         {
01428             if( from_point[i] < box.bMin[i] )
01429                 node.dist[i] = box.bMin[i] - from_point[i];
01430             else if( from_point[i] > box.bMax[i] )
01431                 node.dist[i] = from_point[i] - box.bMax[i];
01432         }
01433         if( node.dist % node.dist > dist_sqr )
01434         {
01435             treeStats.nodesVisited++;
01436             return MB_SUCCESS;
01437         }
01438     }
01439 
01440     // begin with root in list
01441     node.handle = ( tree_root ? *tree_root : myRoot );
01442     list.push_back( node );
01443 
01444     while( !list.empty() )
01445     {
01446 
01447         node = list.back();
01448         list.pop_back();
01449         treeStats.nodesVisited++;
01450 
01451         // If leaf node, test contained triangles
01452         children.clear();
01453         rval = moab()->get_child_meshsets( node.handle, children );
01454         if( children.empty() )
01455         {
01456             treeStats.leavesVisited++;
01457             if( myEval && result_params )
01458             {
01459                 EntityHandle ent;
01460                 CartVect params;
01461                 rval = myEval->find_containing_entity( node.handle, from_point, iter_tol, inside_tol, ent,
01462                                                        params.array(), &treeStats.traversalLeafObjectTests );
01463                 if( MB_SUCCESS != rval )
01464                     return rval;
01465                 else if( ent )
01466                 {
01467                     result_list.push_back( ent );
01468                     result_params->push_back( params );
01469                     if( result_dists ) result_dists->push_back( 0.0 );
01470                 }
01471             }
01472             else
01473             {
01474                 result_list_nodes.push_back( node );
01475                 continue;
01476             }
01477         }
01478 
01479         // If not leaf node, add children to working list
01480         rval = get_split_plane( node.handle, plane );
01481         if( MB_SUCCESS != rval ) return rval;
01482 
01483         const double d = from[plane.norm] - plane.coord;
01484 
01485         // right of plane?
01486         if( d > 0 )
01487         {
01488             node.handle = children[1];
01489             list.push_back( node );
01490             // if the split plane is close to the input point, add
01491             // the left child also (we'll check the exact distance
01492             /// when we pop it from the list.)
01493             if( d <= distance )
01494             {
01495                 node.dist[plane.norm] = d;
01496                 if( node.dist % node.dist <= dist_sqr )
01497                 {
01498                     node.handle = children[0];
01499                     list.push_back( node );
01500                 }
01501             }
01502         }
01503         // left of plane
01504         else
01505         {
01506             node.handle = children[0];
01507             list.push_back( node );
01508             // if the split plane is close to the input point, add
01509             // the right child also (we'll check the exact distance
01510             /// when we pop it from the list.)
01511             if( -d <= distance )
01512             {
01513                 node.dist[plane.norm] = -d;
01514                 if( node.dist % node.dist <= dist_sqr )
01515                 {
01516                     node.handle = children[1];
01517                     list.push_back( node );
01518                 }
01519             }
01520         }
01521     }
01522 
01523     if( myEval && result_params ) return MB_SUCCESS;
01524 
01525     // separate loops to avoid if test inside loop
01526 
01527     result_list.reserve( result_list_nodes.size() );
01528     for( std::vector< NodeDistance >::iterator vit = result_list_nodes.begin(); vit != result_list_nodes.end(); ++vit )
01529         result_list.push_back( ( *vit ).handle );
01530 
01531     if( result_dists && distance > 0.0 )
01532     {
01533         result_dists->reserve( result_list_nodes.size() );
01534         for( std::vector< NodeDistance >::iterator vit = result_list_nodes.begin(); vit != result_list_nodes.end();
01535              ++vit )
01536             result_dists->push_back( ( *vit ).dist.length() );
01537     }
01538 
01539     return MB_SUCCESS;
01540 }
01541 
01542 static ErrorCode closest_to_triangles( Interface* moab,
01543                                        const Range& tris,
01544                                        const CartVect& from,
01545                                        double& shortest_dist_sqr,
01546                                        CartVect& closest_pt,
01547                                        EntityHandle& closest_tri )
01548 {
01549     ErrorCode rval;
01550     CartVect pos, diff, verts[3];
01551     const EntityHandle* conn = NULL;
01552     int len                  = 0;
01553 
01554     for( Range::iterator i = tris.begin(); i != tris.end(); ++i )
01555     {
01556         rval = moab->get_connectivity( *i, conn, len );
01557         if( MB_SUCCESS != rval ) return rval;
01558 
01559         rval = moab->get_coords( conn, 3, verts[0].array() );
01560         if( MB_SUCCESS != rval ) return rval;
01561 
01562         GeomUtil::closest_location_on_tri( from, verts, pos );
01563         diff            = pos - from;
01564         double dist_sqr = diff % diff;
01565         if( dist_sqr < shortest_dist_sqr )
01566         {
01567             // new closest location
01568             shortest_dist_sqr = dist_sqr;
01569             closest_pt        = pos;
01570             closest_tri       = *i;
01571         }
01572     }
01573 
01574     return MB_SUCCESS;
01575 }
01576 
01577 static ErrorCode closest_to_triangles( Interface* moab,
01578                                        EntityHandle set_handle,
01579                                        const CartVect& from,
01580                                        double& shortest_dist_sqr,
01581                                        CartVect& closest_pt,
01582                                        EntityHandle& closest_tri )
01583 {
01584     ErrorCode rval;
01585     Range tris;
01586 
01587     rval = moab->get_entities_by_type( set_handle, MBTRI, tris );
01588     if( MB_SUCCESS != rval ) return rval;
01589 
01590     return closest_to_triangles( moab, tris, from, shortest_dist_sqr, closest_pt, closest_tri );
01591 }
01592 
01593 ErrorCode AdaptiveKDTree::find_close_triangle( EntityHandle root,
01594                                                const double from[3],
01595                                                double pt[3],
01596                                                EntityHandle& triangle )
01597 {
01598     ErrorCode rval;
01599     Range tris;
01600     Plane split;
01601     std::vector< EntityHandle > stack;
01602     std::vector< EntityHandle > children( 2 );
01603     stack.reserve( 30 );
01604     assert( root );
01605     stack.push_back( root );
01606 
01607     while( !stack.empty() )
01608     {
01609         EntityHandle node = stack.back();
01610         stack.pop_back();
01611 
01612         for( ;; )
01613         {  // loop until we find a leaf
01614 
01615             children.clear();
01616             rval = moab()->get_child_meshsets( node, children );
01617             if( MB_SUCCESS != rval ) return rval;
01618 
01619             // loop termination criterion
01620             if( children.empty() ) break;
01621 
01622             // if not a leaf, get split plane
01623             rval = get_split_plane( node, split );
01624             if( MB_SUCCESS != rval ) return rval;
01625 
01626             // continue down the side that contains the point,
01627             // and push the other side onto the stack in case
01628             // we need to check it later.
01629             int rs = split.right_side( from );
01630             node   = children[rs];
01631             stack.push_back( children[1 - rs] );
01632         }
01633 
01634         // We should now be at a leaf.
01635         // If it has some triangles, we're done.
01636         // If not, continue searching for another leaf.
01637         tris.clear();
01638         rval = moab()->get_entities_by_type( node, MBTRI, tris );
01639         if( !tris.empty() )
01640         {
01641             double dist_sqr = HUGE_VAL;
01642             CartVect point( pt );
01643             rval = closest_to_triangles( moab(), tris, CartVect( from ), dist_sqr, point, triangle );
01644             point.get( pt );
01645             return rval;
01646         }
01647     }
01648 
01649     // If we got here, then we traversed the entire tree
01650     // and all the leaves were empty.
01651     return MB_ENTITY_NOT_FOUND;
01652 }
01653 
01654 /** Find the triangles in a set that are closer to the input
01655  *  position than any triangles in the 'closest_tris' list.
01656  *
01657  *  closest_tris is assumed to contain a list of triangles for
01658  *  which the first is the closest known triangle to the input
01659  *  position and the first entry in 'closest_pts' is the closest
01660  *  location on that triangle.  Any other values in the lists must
01661  *  be other triangles for which the closest point is within the
01662  *  input tolerance of the closest closest point.  This function
01663  *  will update the lists as appropriate if any closer triangles
01664  *  or triangles within the tolerance of the current closest location
01665  *  are found.  The first entry is maintained as the closest of the
01666  *  list of triangles.
01667  */
01668 /*
01669   static ErrorCode closest_to_triangles( Interface* moab,
01670   EntityHandle set_handle,
01671   double tolerance,
01672   const CartVect& from,
01673   std::vector<EntityHandle>& closest_tris,
01674   std::vector<CartVect>& closest_pts )
01675   {
01676   ErrorCode rval;
01677   Range tris;
01678   CartVect pos, diff, verts[3];
01679   const EntityHandle* conn;
01680   int len;
01681   double shortest_dist_sqr = HUGE_VAL;
01682   if (!closest_pts.empty()) {
01683   diff = from - closest_pts.front();
01684   shortest_dist_sqr = diff % diff;
01685   }
01686 
01687   rval = moab->get_entities_by_type( set_handle, MBTRI, tris );
01688   if (MB_SUCCESS != rval)
01689   return rval;
01690 
01691   for (Range::iterator i = tris.begin(); i != tris.end(); ++i) {
01692   rval = moab->get_connectivity( *i, conn, len );
01693   if (MB_SUCCESS != rval)
01694   return rval;
01695 
01696   rval = moab->get_coords( conn, 3, verts[0].array() );
01697   if (MB_SUCCESS != rval)
01698   return rval;
01699 
01700   GeomUtil::closest_location_on_tri( from, verts, pos );
01701   diff = pos - from;
01702   double dist_sqr = diff % diff;
01703   if (dist_sqr < shortest_dist_sqr) {
01704     // new closest location
01705     shortest_dist_sqr = dist_sqr;
01706 
01707     if (closest_pts.empty()) {
01708     closest_tris.push_back( *i );
01709     closest_pts.push_back( pos );
01710     }
01711       // if have a previous closest location
01712       else {
01713         // if previous closest is more than 2*tolerance away
01714           // from new closest, then nothing in the list can
01715           // be within tolerance of new closest point.
01716           diff = pos - closest_pts.front();
01717           dist_sqr = diff % diff;
01718           if (dist_sqr > 4.0 * tolerance * tolerance) {
01719           closest_tris.clear();
01720           closest_pts.clear();
01721           closest_tris.push_back( *i );
01722           closest_pts.push_back( pos );
01723           }
01724             // otherwise need to remove any triangles that are
01725               // not within tolerance of the new closest point.
01726               else {
01727               unsigned r = 0, w = 0;
01728               for (r = 0; r < closest_pts.size(); ++r) {
01729               diff = pos - closest_pts[r];
01730               if (diff % diff <= tolerance*tolerance) {
01731               closest_pts[w] = closest_pts[r];
01732               closest_tris[w] = closest_tris[r];
01733               ++w;
01734               }
01735               }
01736               closest_pts.resize( w + 1 );
01737               closest_tris.resize( w + 1 );
01738                 // always put the closest one in the front
01739                 if (w > 0) {
01740                 closest_pts.back() = closest_pts.front();
01741                 closest_tris.back() = closest_tris.front();
01742                 }
01743                 closest_pts.front() = pos;
01744                 closest_tris.front() = *i;
01745                 }
01746                 }
01747                 }
01748                 else {
01749                   // If within tolerance of old closest triangle,
01750                     // add this one to the list.
01751                     diff = closest_pts.front() - pos;
01752                     if (diff % diff <= tolerance*tolerance) {
01753                     closest_pts.push_back( pos );
01754                     closest_tris.push_back( *i );
01755                     }
01756                     }
01757                     }
01758 
01759                     return MB_SUCCESS;
01760                     }
01761 */
01762 
01763 ErrorCode AdaptiveKDTree::closest_triangle( EntityHandle tree_root,
01764                                             const double from_coords[3],
01765                                             double closest_point_out[3],
01766                                             EntityHandle& triangle_out )
01767 {
01768     ErrorCode rval;
01769     double shortest_dist_sqr = HUGE_VAL;
01770     std::vector< EntityHandle > leaves;
01771     const CartVect from( from_coords );
01772     CartVect closest_pt;
01773 
01774     // Find the leaf containing the input point
01775     // This search does not take into account any bounding box for the
01776     // tree, so it always returns one leaf.
01777     assert( tree_root );
01778     rval = find_close_triangle( tree_root, from_coords, closest_pt.array(), triangle_out );
01779     if( MB_SUCCESS != rval ) return rval;
01780 
01781     // Find any other leaves for which the bounding box is within
01782     // the same distance from the input point as the current closest
01783     // point is.
01784     CartVect diff = closest_pt - from;
01785     rval = distance_search( from_coords, sqrt( diff % diff ), leaves, 1.0e-10, 1.0e-6, NULL, NULL, &tree_root );
01786     if( MB_SUCCESS != rval ) return rval;
01787 
01788     // Check any close leaves to see if they contain triangles that
01789     // are as close to or closer than the current closest triangle(s).
01790     for( unsigned i = 0; i < leaves.size(); ++i )
01791     {
01792         rval = closest_to_triangles( moab(), leaves[i], from, shortest_dist_sqr, closest_pt, triangle_out );
01793         if( MB_SUCCESS != rval ) return rval;
01794     }
01795 
01796     // pass back resulting position
01797     closest_pt.get( closest_point_out );
01798     return MB_SUCCESS;
01799 }
01800 
01801 ErrorCode AdaptiveKDTree::sphere_intersect_triangles( EntityHandle tree_root,
01802                                                       const double center[3],
01803                                                       double rad,
01804                                                       std::vector< EntityHandle >& triangles )
01805 {
01806     ErrorCode rval;
01807     std::vector< EntityHandle > leaves;
01808     const CartVect from( center );
01809     CartVect closest_pt;
01810     const EntityHandle* conn;
01811     CartVect coords[3];
01812     int conn_len;
01813 
01814     // get leaves of tree that intersect sphere
01815     assert( tree_root );
01816     rval = distance_search( center, rad, leaves, 1.0e-10, 1.0e-6, NULL, NULL, &tree_root );
01817     if( MB_SUCCESS != rval ) return rval;
01818 
01819     // search each leaf for triangles intersecting sphere
01820     for( unsigned i = 0; i < leaves.size(); ++i )
01821     {
01822         Range tris;
01823         rval = moab()->get_entities_by_type( leaves[i], MBTRI, tris );
01824         if( MB_SUCCESS != rval ) return rval;
01825 
01826         for( Range::iterator j = tris.begin(); j != tris.end(); ++j )
01827         {
01828             rval = moab()->get_connectivity( *j, conn, conn_len );
01829             if( MB_SUCCESS != rval ) return rval;
01830             rval = moab()->get_coords( conn, 3, coords[0].array() );
01831             if( MB_SUCCESS != rval ) return rval;
01832             GeomUtil::closest_location_on_tri( from, coords, closest_pt );
01833             closest_pt -= from;
01834             if( ( closest_pt % closest_pt ) <= ( rad * rad ) ) triangles.push_back( *j );
01835         }
01836     }
01837 
01838     // remove duplicates from triangle list
01839     std::sort( triangles.begin(), triangles.end() );
01840     triangles.erase( std::unique( triangles.begin(), triangles.end() ), triangles.end() );
01841     return MB_SUCCESS;
01842 }
01843 
01844 struct NodeSeg
01845 {
01846     NodeSeg( EntityHandle h, double b, double e ) : handle( h ), beg( b ), end( e ) {}
01847     EntityHandle handle;
01848     double beg, end;
01849 };
01850 
01851 ErrorCode AdaptiveKDTree::ray_intersect_triangles( EntityHandle root,
01852                                                    const double tol,
01853                                                    const double ray_dir_in[3],
01854                                                    const double ray_pt_in[3],
01855                                                    std::vector< EntityHandle >& tris_out,
01856                                                    std::vector< double >& dists_out,
01857                                                    int max_ints,
01858                                                    double ray_end )
01859 {
01860     ErrorCode rval;
01861     double ray_beg = 0.0;
01862     if( ray_end < 0.0 ) ray_end = HUGE_VAL;
01863 
01864     // if root has bounding box, trim ray to that box
01865     CartVect tvec( tol );
01866     BoundBox box;
01867     const CartVect ray_pt( ray_pt_in ), ray_dir( ray_dir_in );
01868     rval = get_bounding_box( box );
01869     if( MB_SUCCESS == rval )
01870     {
01871         if( !GeomUtil::segment_box_intersect( box.bMin - tvec, box.bMax + tvec, ray_pt, ray_dir, ray_beg, ray_end ) )
01872             return MB_SUCCESS;  // ray misses entire tree.
01873     }
01874 
01875     Range tris;
01876     Range::iterator iter;
01877     CartVect tri_coords[3];
01878     const EntityHandle* tri_conn;
01879     int conn_len;
01880     double tri_t;
01881 
01882     Plane plane;
01883     std::vector< EntityHandle > children;
01884     std::vector< NodeSeg > list;
01885     NodeSeg seg( root, ray_beg, ray_end );
01886     list.push_back( seg );
01887 
01888     while( !list.empty() )
01889     {
01890         seg = list.back();
01891         list.pop_back();
01892 
01893         // If we are limited to a certain number of intersections
01894         // (max_ints != 0), then ray_end will contain the distance
01895         // to the furthest intersection we have so far.  If the
01896         // tree node is further than that, skip it.
01897         if( seg.beg > ray_end ) continue;
01898 
01899         // Check if at a leaf
01900         children.clear();
01901         rval = moab()->get_child_meshsets( seg.handle, children );
01902         if( MB_SUCCESS != rval ) return rval;
01903         if( children.empty() )
01904         {  // leaf
01905 
01906             tris.clear();
01907             rval = moab()->get_entities_by_type( seg.handle, MBTRI, tris );
01908             if( MB_SUCCESS != rval ) return rval;
01909 
01910             for( iter = tris.begin(); iter != tris.end(); ++iter )
01911             {
01912                 rval = moab()->get_connectivity( *iter, tri_conn, conn_len );
01913                 if( MB_SUCCESS != rval ) return rval;
01914                 rval = moab()->get_coords( tri_conn, 3, tri_coords[0].array() );
01915                 if( MB_SUCCESS != rval ) return rval;
01916 
01917                 if( GeomUtil::ray_tri_intersect( tri_coords, ray_pt, ray_dir, tri_t, &ray_end ) )
01918                 {
01919                     if( !max_ints )
01920                     {
01921                         if( std::find( tris_out.begin(), tris_out.end(), *iter ) == tris_out.end() )
01922                         {
01923                             tris_out.push_back( *iter );
01924                             dists_out.push_back( tri_t );
01925                         }
01926                     }
01927                     else if( tri_t < ray_end )
01928                     {
01929                         if( std::find( tris_out.begin(), tris_out.end(), *iter ) == tris_out.end() )
01930                         {
01931                             if( tris_out.size() < (unsigned)max_ints )
01932                             {
01933                                 tris_out.resize( tris_out.size() + 1 );
01934                                 dists_out.resize( dists_out.size() + 1 );
01935                             }
01936                             int w = tris_out.size() - 1;
01937                             for( ; w > 0 && tri_t < dists_out[w - 1]; --w )
01938                             {
01939                                 tris_out[w]  = tris_out[w - 1];
01940                                 dists_out[w] = dists_out[w - 1];
01941                             }
01942                             tris_out[w]  = *iter;
01943                             dists_out[w] = tri_t;
01944                             if( tris_out.size() >= (unsigned)max_ints )
01945                                 // when we have already reached the max intx points, we cans safely
01946                                 // reset ray_end, because we will accept new points only "closer"
01947                                 // than the last one
01948                                 ray_end = dists_out.back();
01949                         }
01950                     }
01951                 }
01952             }
01953 
01954             continue;
01955         }
01956 
01957         rval = get_split_plane( seg.handle, plane );
01958         if( MB_SUCCESS != rval ) return rval;
01959 
01960         // Consider two planes that are the split plane +/- the tolerance.
01961         // Calculate the segment parameter at which the line segment intersects
01962         // the true plane, and also the difference between that value and the
01963         // intersection with either of the +/- tol planes.
01964         const double inv_dir = 1.0 / ray_dir[plane.norm];                       // only do division once
01965         const double t       = ( plane.coord - ray_pt[plane.norm] ) * inv_dir;  // intersection with plane
01966         const double diff    = tol * inv_dir;                                   // t adjustment for +tol plane
01967             // const double t0 = t - diff; // intersection with -tol plane
01968             // const double t1 = t + diff; // intersection with +tol plane
01969 
01970         // The index of the child tree node (0 or 1) that is on the
01971         // side of the plane to which the ray direction points.  That is,
01972         // if the ray direction is opposite the plane normal, the index
01973         // of the child corresponding to the side beneath the plane.  If
01974         // the ray direction is the same as the plane normal, the index
01975         // of the child corresponding to the side above the plane.
01976         const int fwd_child = ( ray_dir[plane.norm] > 0.0 );
01977 
01978         // Note: we maintain seg.beg <= seg.end at all times, so assume that here.
01979 
01980         // If segment is parallel to plane
01981         if( !Util::is_finite( t ) )
01982         {
01983             if( ray_pt[plane.norm] - tol <= plane.coord ) list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
01984             if( ray_pt[plane.norm] + tol >= plane.coord ) list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
01985         }
01986         // If segment is entirely to one side of plane such that the
01987         // intersection with the split plane is past the end of the segment
01988         else if( seg.end + diff < t )
01989         {
01990             // If segment direction is opposite that of plane normal, then
01991             // being past the end of the segment means that we are to the
01992             // right (or above) the plane and what the right child (index == 1).
01993             // Otherwise we want the left child (index == 0);
01994             list.push_back( NodeSeg( children[1 - fwd_child], seg.beg, seg.end ) );
01995         }
01996         // If the segment is entirely to one side of the plane such that
01997         // the intersection with the split plane is before the start of the
01998         // segment
01999         else if( seg.beg - diff > t )
02000         {
02001             // If segment direction is opposite that of plane normal, then
02002             // being before the start of the segment means that we are to the
02003             // left (or below) the plane and what the left child (index == 0).
02004             // Otherwise we want the right child (index == 1);
02005             list.push_back( NodeSeg( children[fwd_child], seg.beg, seg.end ) );
02006         }
02007         // Otherwise we must intersect the plane.
02008         // Note: be careful not to grow the segment if t is slightly
02009         // outside the current segment, as doing so would effectively
02010         // increase the tolerance as we descend the tree.
02011         else if( t <= seg.beg )
02012         {
02013             list.push_back( NodeSeg( children[1 - fwd_child], seg.beg, seg.beg ) );
02014             list.push_back( NodeSeg( children[fwd_child], seg.beg, seg.end ) );
02015         }
02016         else if( t >= seg.end )
02017         {
02018             list.push_back( NodeSeg( children[1 - fwd_child], seg.beg, seg.end ) );
02019             list.push_back( NodeSeg( children[fwd_child], seg.end, seg.end ) );
02020         }
02021         else
02022         {
02023             list.push_back( NodeSeg( children[1 - fwd_child], seg.beg, t ) );
02024             list.push_back( NodeSeg( children[fwd_child], t, seg.end ) );
02025         }
02026     }
02027 
02028     return MB_SUCCESS;
02029 }
02030 
02031 ErrorCode AdaptiveKDTree::compute_depth( EntityHandle root, unsigned int& min_depth, unsigned int& max_depth )
02032 {
02033     AdaptiveKDTreeIter iter;
02034     get_tree_iterator( root, iter );
02035     iter.step_to_first_leaf( AdaptiveKDTreeIter::LEFT );
02036     min_depth = max_depth = iter.depth();
02037 
02038     int num_of_elements = 0, max, min;
02039     moab()->get_number_entities_by_handle( iter.handle(), num_of_elements );
02040     max = min = num_of_elements;
02041     int k     = 0;
02042     while( MB_SUCCESS == iter.step() )
02043     {
02044         int temp = 0;
02045         moab()->get_number_entities_by_handle( iter.handle(), temp );
02046         max = std::max( max, temp );
02047         min = std::min( min, temp );
02048         if( iter.depth() > max_depth )
02049             max_depth = iter.depth();
02050         else if( iter.depth() < min_depth )
02051             min_depth = iter.depth();
02052         ++k;
02053     }
02054     return MB_SUCCESS;
02055 }
02056 
02057 ErrorCode AdaptiveKDTree::get_info( EntityHandle root, double bmin[3], double bmax[3], unsigned int& dep )
02058 {
02059     BoundBox box;
02060     ErrorCode result = get_bounding_box( box, &root );
02061     if( MB_SUCCESS != result ) return result;
02062     box.bMin.get( bmin );
02063     box.bMax.get( bmax );
02064 
02065     unsigned min_depth;
02066     return compute_depth( root, min_depth, dep );
02067 }
02068 
02069 static std::string mem_to_string( unsigned long mem )
02070 {
02071     char unit[3] = "B";
02072     if( mem > 9 * 1024 )
02073     {
02074         mem = ( mem + 512 ) / 1024;
02075         strcpy( unit, "kB" );
02076     }
02077     if( mem > 9 * 1024 )
02078     {
02079         mem = ( mem + 512 ) / 1024;
02080         strcpy( unit, "MB" );
02081     }
02082     if( mem > 9 * 1024 )
02083     {
02084         mem = ( mem + 512 ) / 1024;
02085         strcpy( unit, "GB" );
02086     }
02087     char buffer[256];
02088     sprintf( buffer, "%lu %s", mem, unit );
02089     return buffer;
02090 }
02091 
02092 template < typename T >
02093 struct SimpleStat
02094 {
02095     T min, max, sum, sqr;
02096     size_t count;
02097     SimpleStat();
02098     void add( T value );
02099     double avg() const
02100     {
02101         return (double)sum / count;
02102     }
02103     double rms() const
02104     {
02105         return sqrt( (double)sqr / count );
02106     }
02107     double dev() const
02108     {
02109         return ( count > 1
02110                      ? sqrt( ( count * (double)sqr - (double)sum * (double)sum ) / ( (double)count * ( count - 1 ) ) )
02111                      : 0.0 );
02112     }
02113 };
02114 
02115 template < typename T >
02116 SimpleStat< T >::SimpleStat()
02117     : min( std::numeric_limits< T >::max() ), max( std::numeric_limits< T >::min() ), sum( 0 ), sqr( 0 ), count( 0 )
02118 {
02119 }
02120 
02121 template < typename T >
02122 void SimpleStat< T >::add( T value )
02123 {
02124     if( value < min ) min = value;
02125     if( value > max ) max = value;
02126     sum += value;
02127     sqr += value * value;
02128     ++count;
02129 }
02130 
02131 ErrorCode AdaptiveKDTree::print()
02132 {
02133     Range range;
02134 
02135     Range tree_sets, elem2d, elem3d, verts, all;
02136     moab()->get_child_meshsets( myRoot, tree_sets, 0 );
02137     for( Range::iterator rit = tree_sets.begin(); rit != tree_sets.end(); ++rit )
02138     {
02139         moab()->get_entities_by_dimension( *rit, 2, elem2d );
02140         moab()->get_entities_by_dimension( *rit, 3, elem3d );
02141         moab()->get_entities_by_type( *rit, MBVERTEX, verts );
02142     }
02143     all.merge( verts );
02144     all.merge( elem2d );
02145     all.merge( elem3d );
02146     tree_sets.insert( myRoot );
02147     unsigned long long set_used, set_amortized, set_store_used, set_store_amortized, set_tag_used, set_tag_amortized,
02148         elem_used, elem_amortized;
02149     moab()->estimated_memory_use( tree_sets, &set_used, &set_amortized, &set_store_used, &set_store_amortized, 0, 0, 0,
02150                                   0, &set_tag_used, &set_tag_amortized );
02151     moab()->estimated_memory_use( all, &elem_used, &elem_amortized );
02152 
02153     int num_2d = 0, num_3d = 0;
02154     ;
02155     moab()->get_number_entities_by_dimension( 0, 2, num_2d );
02156     moab()->get_number_entities_by_dimension( 0, 3, num_3d );
02157 
02158     BoundBox box;
02159     ErrorCode rval = get_bounding_box( box, &myRoot );
02160     if( MB_SUCCESS != rval || box == BoundBox() ) throw rval;
02161     double diff[3]        = { box.bMax[0] - box.bMin[0], box.bMax[1] - box.bMin[1], box.bMax[2] - box.bMin[2] };
02162     double tree_vol       = diff[0] * diff[1] * diff[2];
02163     double tree_surf_area = 2 * ( diff[0] * diff[1] + diff[1] * diff[2] + diff[2] * diff[0] );
02164 
02165     SimpleStat< unsigned > depth, size;
02166     SimpleStat< double > vol, surf;
02167 
02168     AdaptiveKDTreeIter iter;
02169     get_tree_iterator( myRoot, iter );
02170     do
02171     {
02172         depth.add( iter.depth() );
02173 
02174         int num_leaf_elem;
02175         moab()->get_number_entities_by_handle( iter.handle(), num_leaf_elem );
02176         size.add( num_leaf_elem );
02177 
02178         const double* n = iter.box_min();
02179         const double* x = iter.box_max();
02180         double dims[3]  = { x[0] - n[0], x[1] - n[1], x[2] - n[2] };
02181 
02182         double leaf_vol = dims[0] * dims[1] * dims[2];
02183         vol.add( leaf_vol );
02184 
02185         double area = 2.0 * ( dims[0] * dims[1] + dims[1] * dims[2] + dims[2] * dims[0] );
02186         surf.add( area );
02187 
02188     } while( MB_SUCCESS == iter.step() );
02189 
02190     printf( "------------------------------------------------------------------\n" );
02191     printf( "tree volume:      %f\n", tree_vol );
02192     printf( "total elements:   %d\n", num_2d + num_3d );
02193     printf( "number of leaves: %lu\n", (unsigned long)depth.count );
02194     printf( "number of nodes:  %lu\n", (unsigned long)tree_sets.size() );
02195     printf( "volume ratio:     %0.2f%%\n", 100 * ( vol.sum / tree_vol ) );
02196     printf( "surface ratio:    %0.2f%%\n", 100 * ( surf.sum / tree_surf_area ) );
02197     printf( "\nmemory:           used  amortized\n" );
02198     printf( "            ---------- ----------\n" );
02199     printf( "elements    %10s %10s\n", mem_to_string( elem_used ).c_str(), mem_to_string( elem_amortized ).c_str() );
02200     printf( "sets (total)%10s %10s\n", mem_to_string( set_used ).c_str(), mem_to_string( set_amortized ).c_str() );
02201     printf( "sets        %10s %10s\n", mem_to_string( set_store_used ).c_str(),
02202             mem_to_string( set_store_amortized ).c_str() );
02203     printf( "set tags    %10s %10s\n", mem_to_string( set_tag_used ).c_str(),
02204             mem_to_string( set_tag_amortized ).c_str() );
02205     printf( "\nleaf stats:        min        avg        rms        max    std.dev\n" );
02206     printf( "            ---------- ---------- ---------- ---------- ----------\n" );
02207     printf( "depth       %10u %10.1f %10.1f %10u %10.2f\n", depth.min, depth.avg(), depth.rms(), depth.max,
02208             depth.dev() );
02209     printf( "triangles   %10u %10.1f %10.1f %10u %10.2f\n", size.min, size.avg(), size.rms(), size.max, size.dev() );
02210     printf( "volume      %10.2g %10.2g %10.2g %10.2g %10.2g\n", vol.min, vol.avg(), vol.rms(), vol.max, vol.dev() );
02211     printf( "surf. area  %10.2g %10.2g %10.2g %10.2g %10.2g\n", surf.min, surf.avg(), surf.rms(), surf.max,
02212             surf.dev() );
02213     printf( "------------------------------------------------------------------\n" );
02214 
02215     return MB_SUCCESS;
02216 }
02217 
02218 }  // namespace moab
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines