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