MOAB: Mesh Oriented datABase  (version 5.4.0)
BSPTree.cpp
Go to the documentation of this file.
00001 /*
00002  * MOAB, a Mesh-Oriented datABase, is a software component for creating,
00003  * storing and accessing finite element mesh data.
00004  *
00005  * Copyright 2008 Sandia Corporation.  Under the terms of Contract
00006  * DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government
00007  * retains certain rights in this software.
00008  *
00009  * This library is free software; you can redistribute it and/or
00010  * modify it under the terms of the GNU Lesser General Public
00011  * License as published by the Free Software Foundation; either
00012  * version 2.1 of the License, or (at your option) any later version.
00013  *
00014  */
00015 
00016 /**\file BSPTree.cpp
00017  *\author Jason Kraftcheck (kraftche@cae.wisc.edu)
00018  *\date 2008-05-13
00019  */
00020 
00021 #include "moab/BSPTree.hpp"
00022 #include "moab/GeomUtil.hpp"
00023 #include "moab/Range.hpp"
00024 #include "Internals.hpp"
00025 #include "moab/BSPTreePoly.hpp"
00026 #include "moab/Util.hpp"
00027 
00028 #include <cassert>
00029 #include <cstring>
00030 #include <algorithm>
00031 #include <limits>
00032 
00033 #if defined( MOAB_HAVE_IEEEFP_H )
00034 #include <ieeefp.h>
00035 #endif
00036 
00037 #define MB_BSP_TREE_DEFAULT_TAG_NAME "BSPTree"
00038 
00039 namespace moab
00040 {
00041 
00042 static void corners_from_box( const double box_min[3], const double box_max[3], double corners[8][3] )
00043 {
00044     const double* ranges[] = { box_min, box_max };
00045     for( int z = 0; z < 2; ++z )
00046     {
00047         corners[4 * z][0] = box_min[0];
00048         corners[4 * z][1] = box_min[1];
00049         corners[4 * z][2] = ranges[z][2];
00050 
00051         corners[4 * z + 1][0] = box_max[0];
00052         corners[4 * z + 1][1] = box_min[1];
00053         corners[4 * z + 1][2] = ranges[z][2];
00054 
00055         corners[4 * z + 2][0] = box_max[0];
00056         corners[4 * z + 2][1] = box_max[1];
00057         corners[4 * z + 2][2] = ranges[z][2];
00058 
00059         corners[4 * z + 3][0] = box_min[0];
00060         corners[4 * z + 3][1] = box_max[1];
00061         corners[4 * z + 3][2] = ranges[z][2];
00062     }
00063 }
00064 
00065 // assume box has planar sides
00066 // test if point is contained in box
00067 static bool point_in_box( const double corners[8][3], const double point[3] )
00068 {
00069     const unsigned side_verts[6][3] = { { 0, 3, 1 }, { 4, 5, 7 }, { 0, 1, 4 }, { 1, 2, 5 }, { 2, 3, 6 }, { 3, 0, 7 } };
00070     // If we assume planar sides, then the box is the intersection
00071     // of 6 half-spaces defined by the planes of the sides.
00072     const CartVect pt( point );
00073     for( unsigned s = 0; s < 6; ++s )
00074     {
00075         CartVect v0( corners[side_verts[s][0]] );
00076         CartVect v1( corners[side_verts[s][1]] );
00077         CartVect v2( corners[side_verts[s][2]] );
00078         CartVect N = ( v1 - v0 ) * ( v2 - v0 );
00079         if( ( v0 - pt ) % N < 0 ) return false;
00080     }
00081     return true;
00082 }
00083 
00084 void BSPTree::Plane::set( const double pt1[3], const double pt2[3], const double pt3[3] )
00085 {
00086     const double v1[]  = { pt2[0] - pt1[0], pt2[1] - pt1[1], pt2[2] - pt1[2] };
00087     const double v2[]  = { pt3[0] - pt1[0], pt3[1] - pt1[1], pt3[2] - pt1[2] };
00088     const double nrm[] = { v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2],
00089                            v1[0] * v2[1] - v1[1] * v2[0] };
00090     set( nrm, pt1 );
00091 }
00092 
00093 ErrorCode BSPTree::init_tags( const char* tagname )
00094 {
00095     if( !tagname ) tagname = MB_BSP_TREE_DEFAULT_TAG_NAME;
00096 
00097     std::string rootname( tagname );
00098     rootname += "_box";
00099 
00100     ErrorCode rval = moab()->tag_get_handle( tagname, 4, MB_TYPE_DOUBLE, planeTag, MB_TAG_CREAT | MB_TAG_DENSE );
00101     if( MB_SUCCESS != rval )
00102         planeTag = 0;
00103     else
00104         rval = moab()->tag_get_handle( rootname.c_str(), 24, MB_TYPE_DOUBLE, rootTag, MB_TAG_CREAT | MB_TAG_SPARSE );
00105     if( MB_SUCCESS != rval ) rootTag = 0;
00106     return rval;
00107 }
00108 
00109 BSPTree::BSPTree( Interface* mb, const char* tagname, unsigned set_flags )
00110     : mbInstance( mb ), meshSetFlags( set_flags ), cleanUpTrees( false )
00111 {
00112     init_tags( tagname );
00113 }
00114 
00115 BSPTree::BSPTree( Interface* mb, bool destroy_created_trees, const char* tagname, unsigned set_flags )
00116     : mbInstance( mb ), meshSetFlags( set_flags ), cleanUpTrees( destroy_created_trees )
00117 {
00118     init_tags( tagname );
00119 }
00120 
00121 BSPTree::~BSPTree()
00122 {
00123     if( !cleanUpTrees ) return;
00124 
00125     while( !createdTrees.empty() )
00126     {
00127         EntityHandle tree = createdTrees.back();
00128         // make sure this is a tree (rather than some other, stale handle)
00129         const void* data_ptr = 0;
00130         ErrorCode rval       = moab()->tag_get_by_ptr( rootTag, &tree, 1, &data_ptr );
00131         if( MB_SUCCESS == rval ) rval = delete_tree( tree );
00132         if( MB_SUCCESS != rval ) createdTrees.pop_back();
00133     }
00134 }
00135 
00136 ErrorCode BSPTree::set_split_plane( EntityHandle node, const Plane& p )
00137 {
00138     // check for unit-length normal
00139     const double lensqr = p.norm[0] * p.norm[0] + p.norm[1] * p.norm[1] + p.norm[2] * p.norm[2];
00140     if( fabs( lensqr - 1.0 ) < std::numeric_limits< double >::epsilon() )
00141         return moab()->tag_set_data( planeTag, &node, 1, &p );
00142 
00143     const double inv_len = 1.0 / sqrt( lensqr );
00144     Plane p2( p );
00145     p2.norm[0] *= inv_len;
00146     p2.norm[1] *= inv_len;
00147     p2.norm[2] *= inv_len;
00148     p2.coeff *= inv_len;
00149 
00150     // check for zero-length normal
00151     if( !Util::is_finite( p2.norm[0] + p2.norm[1] + p2.norm[2] + p2.coeff ) ) return MB_FAILURE;
00152 
00153     // store plane
00154     return moab()->tag_set_data( planeTag, &node, 1, &p2 );
00155 }
00156 
00157 ErrorCode BSPTree::set_tree_box( EntityHandle root_handle, const double box_min[3], const double box_max[3] )
00158 {
00159     double corners[8][3];
00160     corners_from_box( box_min, box_max, corners );
00161     return set_tree_box( root_handle, corners );
00162 }
00163 
00164 ErrorCode BSPTree::set_tree_box( EntityHandle root_handle, const double corners[8][3] )
00165 {
00166     return moab()->tag_set_data( rootTag, &root_handle, 1, corners );
00167 }
00168 
00169 ErrorCode BSPTree::get_tree_box( EntityHandle root_handle, double corners[8][3] )
00170 {
00171     return moab()->tag_get_data( rootTag, &root_handle, 1, corners );
00172 }
00173 
00174 ErrorCode BSPTree::get_tree_box( EntityHandle root_handle, double corners[24] )
00175 {
00176     return moab()->tag_get_data( rootTag, &root_handle, 1, corners );
00177 }
00178 
00179 ErrorCode BSPTree::create_tree( EntityHandle& root_handle )
00180 {
00181     const double min[3] = { -HUGE_VAL, -HUGE_VAL, -HUGE_VAL };
00182     const double max[3] = { HUGE_VAL, HUGE_VAL, HUGE_VAL };
00183     return create_tree( min, max, root_handle );
00184 }
00185 
00186 ErrorCode BSPTree::create_tree( const double corners[8][3], EntityHandle& root_handle )
00187 {
00188     ErrorCode rval = moab()->create_meshset( meshSetFlags, root_handle );
00189     if( MB_SUCCESS != rval ) return rval;
00190 
00191     rval = set_tree_box( root_handle, corners );
00192     if( MB_SUCCESS != rval )
00193     {
00194         moab()->delete_entities( &root_handle, 1 );
00195         root_handle = 0;
00196         return rval;
00197     }
00198 
00199     createdTrees.push_back( root_handle );
00200     return MB_SUCCESS;
00201 }
00202 
00203 ErrorCode BSPTree::create_tree( const double box_min[3], const double box_max[3], EntityHandle& root_handle )
00204 {
00205     double corners[8][3];
00206     corners_from_box( box_min, box_max, corners );
00207     return create_tree( corners, root_handle );
00208 }
00209 
00210 ErrorCode BSPTree::delete_tree( EntityHandle root_handle )
00211 {
00212     ErrorCode rval;
00213 
00214     std::vector< EntityHandle > children, dead_sets, current_sets;
00215     current_sets.push_back( root_handle );
00216     while( !current_sets.empty() )
00217     {
00218         EntityHandle set = current_sets.back();
00219         current_sets.pop_back();
00220         dead_sets.push_back( set );
00221         rval = moab()->get_child_meshsets( set, children );
00222         if( MB_SUCCESS != rval ) return rval;
00223         std::copy( children.begin(), children.end(), std::back_inserter( current_sets ) );
00224         children.clear();
00225     }
00226 
00227     rval = moab()->tag_delete_data( rootTag, &root_handle, 1 );
00228     if( MB_SUCCESS != rval ) return rval;
00229 
00230     createdTrees.erase( std::remove( createdTrees.begin(), createdTrees.end(), root_handle ), createdTrees.end() );
00231     return moab()->delete_entities( &dead_sets[0], dead_sets.size() );
00232 }
00233 
00234 ErrorCode BSPTree::find_all_trees( Range& results )
00235 {
00236     return moab()->get_entities_by_type_and_tag( 0, MBENTITYSET, &rootTag, 0, 1, results );
00237 }
00238 
00239 ErrorCode BSPTree::get_tree_iterator( EntityHandle root, BSPTreeIter& iter )
00240 {
00241     ErrorCode rval = iter.initialize( this, root );
00242     if( MB_SUCCESS != rval ) return rval;
00243     return iter.step_to_first_leaf( BSPTreeIter::LEFT );
00244 }
00245 
00246 ErrorCode BSPTree::get_tree_end_iterator( EntityHandle root, BSPTreeIter& iter )
00247 {
00248     ErrorCode rval = iter.initialize( this, root );
00249     if( MB_SUCCESS != rval ) return rval;
00250     return iter.step_to_first_leaf( BSPTreeIter::RIGHT );
00251 }
00252 
00253 ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, Plane plane, EntityHandle& left, EntityHandle& right )
00254 {
00255     ErrorCode rval;
00256 
00257     rval = moab()->create_meshset( meshSetFlags, left );
00258     if( MB_SUCCESS != rval ) return rval;
00259 
00260     rval = moab()->create_meshset( meshSetFlags, right );
00261     if( MB_SUCCESS != rval )
00262     {
00263         moab()->delete_entities( &left, 1 );
00264         return rval;
00265     }
00266 
00267     if( MB_SUCCESS != set_split_plane( leaf.handle(), plane ) ||
00268         MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), left ) ||
00269         MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), right ) ||
00270         MB_SUCCESS != leaf.step_to_first_leaf( BSPTreeIter::LEFT ) )
00271     {
00272         EntityHandle children[] = { left, right };
00273         moab()->delete_entities( children, 2 );
00274         return MB_FAILURE;
00275     }
00276 
00277     return MB_SUCCESS;
00278 }
00279 
00280 ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, Plane plane )
00281 {
00282     EntityHandle left, right;
00283     return split_leaf( leaf, plane, left, right );
00284 }
00285 
00286 ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, Plane plane, const Range& left_entities, const Range& right_entities )
00287 {
00288     EntityHandle left, right, parent = leaf.handle();
00289     ErrorCode rval = split_leaf( leaf, plane, left, right );
00290     if( MB_SUCCESS != rval ) return rval;
00291 
00292     if( MB_SUCCESS == moab()->add_entities( left, left_entities ) &&
00293         MB_SUCCESS == moab()->add_entities( right, right_entities ) &&
00294         MB_SUCCESS == moab()->clear_meshset( &parent, 1 ) )
00295         return MB_SUCCESS;
00296 
00297     moab()->remove_child_meshset( parent, left );
00298     moab()->remove_child_meshset( parent, right );
00299     EntityHandle children[] = { left, right };
00300     moab()->delete_entities( children, 2 );
00301     return MB_FAILURE;
00302 }
00303 
00304 ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf,
00305                                Plane plane,
00306                                const std::vector< EntityHandle >& left_entities,
00307                                const std::vector< EntityHandle >& right_entities )
00308 {
00309     EntityHandle left, right, parent = leaf.handle();
00310     ErrorCode rval = split_leaf( leaf, plane, left, right );
00311     if( MB_SUCCESS != rval ) return rval;
00312 
00313     if( MB_SUCCESS == moab()->add_entities( left, &left_entities[0], left_entities.size() ) &&
00314         MB_SUCCESS == moab()->add_entities( right, &right_entities[0], right_entities.size() ) &&
00315         MB_SUCCESS == moab()->clear_meshset( &parent, 1 ) )
00316         return MB_SUCCESS;
00317 
00318     moab()->remove_child_meshset( parent, left );
00319     moab()->remove_child_meshset( parent, right );
00320     EntityHandle children[] = { left, right };
00321     moab()->delete_entities( children, 2 );
00322     return MB_FAILURE;
00323 }
00324 
00325 ErrorCode BSPTree::merge_leaf( BSPTreeIter& iter )
00326 {
00327     ErrorCode rval;
00328     if( iter.depth() == 1 )  // at root
00329         return MB_FAILURE;
00330 
00331     // Move iter to parent
00332     iter.up();
00333 
00334     // Get sets to merge
00335     EntityHandle parent = iter.handle();
00336     iter.childVect.clear();
00337     rval = moab()->get_child_meshsets( parent, iter.childVect );
00338     if( MB_SUCCESS != rval ) return rval;
00339 
00340     // Remove child links
00341     moab()->remove_child_meshset( parent, iter.childVect[0] );
00342     moab()->remove_child_meshset( parent, iter.childVect[1] );
00343     std::vector< EntityHandle > stack( iter.childVect );
00344 
00345     // Get all entities from children and put them in parent
00346     Range range;
00347     while( !stack.empty() )
00348     {
00349         EntityHandle h = stack.back();
00350         stack.pop_back();
00351         range.clear();
00352         rval = moab()->get_entities_by_handle( h, range );
00353         if( MB_SUCCESS != rval ) return rval;
00354         rval = moab()->add_entities( parent, range );
00355         if( MB_SUCCESS != rval ) return rval;
00356 
00357         iter.childVect.clear();
00358         rval = moab()->get_child_meshsets( h, iter.childVect );MB_CHK_ERR( rval );
00359         if( !iter.childVect.empty() )
00360         {
00361             moab()->remove_child_meshset( h, iter.childVect[0] );
00362             moab()->remove_child_meshset( h, iter.childVect[1] );
00363             stack.push_back( iter.childVect[0] );
00364             stack.push_back( iter.childVect[1] );
00365         }
00366 
00367         rval = moab()->delete_entities( &h, 1 );
00368         if( MB_SUCCESS != rval ) return rval;
00369     }
00370 
00371     return MB_SUCCESS;
00372 }
00373 
00374 ErrorCode BSPTreeIter::initialize( BSPTree* btool, EntityHandle root, const double* /*point*/ )
00375 {
00376     treeTool = btool;
00377     mStack.clear();
00378     mStack.push_back( root );
00379     return MB_SUCCESS;
00380 }
00381 
00382 ErrorCode BSPTreeIter::step_to_first_leaf( Direction direction )
00383 {
00384     ErrorCode rval;
00385     for( ;; )
00386     {
00387         childVect.clear();
00388         rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
00389         if( MB_SUCCESS != rval ) return rval;
00390         if( childVect.empty() )  // leaf
00391             break;
00392 
00393         mStack.push_back( childVect[direction] );
00394     }
00395     return MB_SUCCESS;
00396 }
00397 
00398 ErrorCode BSPTreeIter::step( Direction direction )
00399 {
00400     EntityHandle node, parent;
00401     ErrorCode rval;
00402     const Direction opposite = static_cast< Direction >( 1 - direction );
00403 
00404     // If stack is empty, then either this iterator is uninitialized
00405     // or we reached the end of the iteration (and return
00406     // MB_ENTITY_NOT_FOUND) already.
00407     if( mStack.empty() ) return MB_FAILURE;
00408 
00409     // Pop the current node from the stack.
00410     // The stack should then contain the parent of the current node.
00411     // If the stack is empty after this pop, then we've reached the end.
00412     node = mStack.back();
00413     mStack.pop_back();
00414 
00415     while( !mStack.empty() )
00416     {
00417         // Get data for parent entity
00418         parent = mStack.back();
00419         childVect.clear();
00420         rval = tool()->moab()->get_child_meshsets( parent, childVect );
00421         if( MB_SUCCESS != rval ) return rval;
00422 
00423         // If we're at the left child
00424         if( childVect[opposite] == node )
00425         {
00426             // push right child on stack
00427             mStack.push_back( childVect[direction] );
00428             // descend to left-most leaf of the right child
00429             return step_to_first_leaf( opposite );
00430         }
00431 
00432         // The current node is the right child of the parent,
00433         // continue up the tree.
00434         assert( childVect[direction] == node );
00435         node = parent;
00436         mStack.pop_back();
00437     }
00438 
00439     return MB_ENTITY_NOT_FOUND;
00440 }
00441 
00442 ErrorCode BSPTreeIter::up()
00443 {
00444     if( mStack.size() < 2 ) return MB_ENTITY_NOT_FOUND;
00445     mStack.pop_back();
00446     return MB_SUCCESS;
00447 }
00448 
00449 ErrorCode BSPTreeIter::down( const BSPTree::Plane& /*plane*/, Direction dir )
00450 {
00451     childVect.clear();
00452     ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
00453     if( MB_SUCCESS != rval ) return rval;
00454     if( childVect.empty() ) return MB_ENTITY_NOT_FOUND;
00455 
00456     mStack.push_back( childVect[dir] );
00457     return MB_SUCCESS;
00458 }
00459 
00460 ErrorCode BSPTreeIter::get_parent_split_plane( BSPTree::Plane& plane ) const
00461 {
00462     if( mStack.size() < 2 )  // at tree root
00463         return MB_ENTITY_NOT_FOUND;
00464 
00465     EntityHandle parent = mStack[mStack.size() - 2];
00466     return tool()->get_split_plane( parent, plane );
00467 }
00468 
00469 double BSPTreeIter::volume() const
00470 {
00471     BSPTreePoly polyhedron;
00472     ErrorCode rval = calculate_polyhedron( polyhedron );
00473     return MB_SUCCESS == rval ? polyhedron.volume() : -1.0;
00474 }
00475 
00476 bool BSPTreeIter::is_sibling( const BSPTreeIter& other_leaf ) const
00477 {
00478     const size_t s = mStack.size();
00479     return ( s > 1 ) && ( s == other_leaf.mStack.size() ) && ( other_leaf.mStack[s - 2] == mStack[s - 2] ) &&
00480            other_leaf.handle() != handle();
00481 }
00482 
00483 bool BSPTreeIter::is_sibling( EntityHandle other_leaf ) const
00484 {
00485     if( mStack.size() < 2 || other_leaf == handle() ) return false;
00486     EntityHandle parent = mStack[mStack.size() - 2];
00487     childVect.clear();
00488     ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
00489     if( MB_SUCCESS != rval || childVect.size() != 2 )
00490     {
00491         assert( false );
00492         return false;
00493     }
00494     return childVect[0] == other_leaf || childVect[1] == other_leaf;
00495 }
00496 
00497 bool BSPTreeIter::sibling_is_forward() const
00498 {
00499     if( mStack.size() < 2 )  // if root
00500         return false;
00501     EntityHandle parent = mStack[mStack.size() - 2];
00502     childVect.clear();
00503     ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
00504     if( MB_SUCCESS != rval || childVect.size() != 2 )
00505     {
00506         assert( false );
00507         return false;
00508     }
00509     return childVect[0] == handle();
00510 }
00511 
00512 ErrorCode BSPTreeIter::calculate_polyhedron( BSPTreePoly& poly_out ) const
00513 {
00514     ErrorCode rval;
00515 
00516     assert( sizeof( CartVect ) == 3 * sizeof( double ) );
00517     CartVect corners[8];
00518     rval = treeTool->get_tree_box( mStack.front(), corners[0].array() );
00519     if( MB_SUCCESS != rval ) return rval;
00520 
00521     rval = poly_out.set( corners );
00522     if( MB_SUCCESS != rval ) return rval;
00523 
00524     BSPTree::Plane plane;
00525     std::vector< EntityHandle >::const_iterator i    = mStack.begin();
00526     std::vector< EntityHandle >::const_iterator here = mStack.end() - 1;
00527     while( i != here )
00528     {
00529         rval = treeTool->get_split_plane( *i, plane );
00530         if( MB_SUCCESS != rval ) return rval;
00531 
00532         childVect.clear();
00533         rval = treeTool->moab()->get_child_meshsets( *i, childVect );
00534         if( MB_SUCCESS != rval ) return rval;
00535         if( childVect.size() != 2 ) return MB_FAILURE;
00536 
00537         ++i;
00538         if( childVect[1] == *i ) plane.flip();
00539 
00540         CartVect norm( plane.norm );
00541         poly_out.cut_polyhedron( norm, plane.coeff );
00542     }
00543 
00544     return MB_SUCCESS;
00545 }
00546 
00547 ErrorCode BSPTreeBoxIter::initialize( BSPTree* tool_ptr, EntityHandle root, const double* point )
00548 {
00549     ErrorCode rval = BSPTreeIter::initialize( tool_ptr, root );
00550     if( MB_SUCCESS != rval ) return rval;
00551 
00552     rval = tool()->get_tree_box( root, leafCoords );
00553     if( MB_SUCCESS != rval ) return rval;
00554 
00555     if( point && !point_in_box( leafCoords, point ) ) return MB_ENTITY_NOT_FOUND;
00556 
00557     stackData.resize( 1 );
00558     return MB_SUCCESS;
00559 }
00560 
00561 BSPTreeBoxIter::SideBits BSPTreeBoxIter::side_above_plane( const double hex_coords[8][3], const BSPTree::Plane& plane )
00562 {
00563     unsigned result = 0;
00564     for( unsigned i = 0; i < 8u; ++i )
00565         result |= plane.above( hex_coords[i] ) << i;
00566     return (BSPTreeBoxIter::SideBits)result;
00567 }
00568 
00569 BSPTreeBoxIter::SideBits BSPTreeBoxIter::side_on_plane( const double hex_coords[8][3], const BSPTree::Plane& plane )
00570 {
00571     unsigned result = 0;
00572     for( unsigned i = 0; i < 8u; ++i )
00573     {
00574         bool on = plane.distance( hex_coords[i] ) <= BSPTree::epsilon();
00575         result |= on << i;
00576     }
00577     return (BSPTreeBoxIter::SideBits)result;
00578 }
00579 
00580 static inline void copy_coords( const double src[3], double dest[3] )
00581 {
00582     dest[0] = src[0];
00583     dest[1] = src[1];
00584     dest[2] = src[2];
00585 }
00586 
00587 ErrorCode BSPTreeBoxIter::face_corners( const SideBits face, const double hex_corners[8][3], double face_corners[4][3] )
00588 {
00589     switch( face )
00590     {
00591         case BSPTreeBoxIter::B0154:
00592             copy_coords( hex_corners[0], face_corners[0] );
00593             copy_coords( hex_corners[1], face_corners[1] );
00594             copy_coords( hex_corners[5], face_corners[2] );
00595             copy_coords( hex_corners[4], face_corners[3] );
00596             break;
00597         case BSPTreeBoxIter::B1265:
00598             copy_coords( hex_corners[1], face_corners[0] );
00599             copy_coords( hex_corners[2], face_corners[1] );
00600             copy_coords( hex_corners[6], face_corners[2] );
00601             copy_coords( hex_corners[5], face_corners[3] );
00602             break;
00603         case BSPTreeBoxIter::B2376:
00604             copy_coords( hex_corners[2], face_corners[0] );
00605             copy_coords( hex_corners[3], face_corners[1] );
00606             copy_coords( hex_corners[7], face_corners[2] );
00607             copy_coords( hex_corners[6], face_corners[3] );
00608             break;
00609         case BSPTreeBoxIter::B3047:
00610             copy_coords( hex_corners[3], face_corners[0] );
00611             copy_coords( hex_corners[0], face_corners[1] );
00612             copy_coords( hex_corners[4], face_corners[2] );
00613             copy_coords( hex_corners[7], face_corners[3] );
00614             break;
00615         case BSPTreeBoxIter::B3210:
00616             copy_coords( hex_corners[3], face_corners[0] );
00617             copy_coords( hex_corners[2], face_corners[1] );
00618             copy_coords( hex_corners[1], face_corners[2] );
00619             copy_coords( hex_corners[0], face_corners[3] );
00620             break;
00621         case BSPTreeBoxIter::B4567:
00622             copy_coords( hex_corners[4], face_corners[0] );
00623             copy_coords( hex_corners[5], face_corners[1] );
00624             copy_coords( hex_corners[6], face_corners[2] );
00625             copy_coords( hex_corners[7], face_corners[3] );
00626             break;
00627         default:
00628             return MB_FAILURE;  // child is not a box
00629     }
00630 
00631     return MB_SUCCESS;
00632 }
00633 
00634 /** \brief Clip an edge using a plane
00635  *
00636  * Given an edge from keep_end_coords to cut_end_coords,
00637  * cut the edge using the passed plane, such that cut_end_coords
00638  * is updated with a new location on the plane, and old_coords_out
00639  * contains the original value of cut_end_coords.
00640  */
00641 static inline void plane_cut_edge( double old_coords_out[3],
00642                                    const double keep_end_coords[3],
00643                                    double cut_end_coords[3],
00644                                    const BSPTree::Plane& plane )
00645 {
00646     const CartVect start( keep_end_coords ), end( cut_end_coords );
00647     const CartVect norm( plane.norm );
00648     CartVect xsect_point;
00649 
00650     const CartVect m = end - start;
00651     const double t   = -( norm % start + plane.coeff ) / ( norm % m );
00652     assert( t > 0.0 && t < 1.0 );
00653     xsect_point = start + t * m;
00654 
00655     end.get( old_coords_out );
00656     xsect_point.get( cut_end_coords );
00657 }
00658 
00659 /** Given the corners of a hexahedron in corners_input and a
00660  *  plane, cut the hex with the plane, updating corners_input
00661  *  and storing the original,cut-off side of the hex in cut_face_out.
00662  *
00663  *  The portion of the hex below the plane is retained.  cut_face_out
00664  *  will contain the side of the hex that is entirely above the plane.
00665  *\return MB_FAILURE if plane/hex intersection is not a quadrilateral.
00666  */
00667 static ErrorCode plane_cut_box( double cut_face_out[4][3], double corners_inout[8][3], const BSPTree::Plane& plane )
00668 {
00669     switch( BSPTreeBoxIter::side_above_plane( corners_inout, plane ) )
00670     {
00671         case BSPTreeBoxIter::B0154:
00672             plane_cut_edge( cut_face_out[0], corners_inout[3], corners_inout[0], plane );
00673             plane_cut_edge( cut_face_out[1], corners_inout[2], corners_inout[1], plane );
00674             plane_cut_edge( cut_face_out[2], corners_inout[6], corners_inout[5], plane );
00675             plane_cut_edge( cut_face_out[3], corners_inout[7], corners_inout[4], plane );
00676             break;
00677         case BSPTreeBoxIter::B1265:
00678             plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[1], plane );
00679             plane_cut_edge( cut_face_out[1], corners_inout[3], corners_inout[2], plane );
00680             plane_cut_edge( cut_face_out[2], corners_inout[7], corners_inout[6], plane );
00681             plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[5], plane );
00682             break;
00683         case BSPTreeBoxIter::B2376:
00684             plane_cut_edge( cut_face_out[0], corners_inout[1], corners_inout[2], plane );
00685             plane_cut_edge( cut_face_out[1], corners_inout[0], corners_inout[3], plane );
00686             plane_cut_edge( cut_face_out[2], corners_inout[4], corners_inout[7], plane );
00687             plane_cut_edge( cut_face_out[3], corners_inout[5], corners_inout[6], plane );
00688             break;
00689         case BSPTreeBoxIter::B3047:
00690             plane_cut_edge( cut_face_out[0], corners_inout[2], corners_inout[3], plane );
00691             plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[0], plane );
00692             plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[4], plane );
00693             plane_cut_edge( cut_face_out[3], corners_inout[6], corners_inout[7], plane );
00694             break;
00695         case BSPTreeBoxIter::B3210:
00696             plane_cut_edge( cut_face_out[0], corners_inout[7], corners_inout[3], plane );
00697             plane_cut_edge( cut_face_out[1], corners_inout[6], corners_inout[2], plane );
00698             plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[1], plane );
00699             plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[0], plane );
00700             break;
00701         case BSPTreeBoxIter::B4567:
00702             plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[4], plane );
00703             plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[5], plane );
00704             plane_cut_edge( cut_face_out[2], corners_inout[2], corners_inout[6], plane );
00705             plane_cut_edge( cut_face_out[3], corners_inout[3], corners_inout[7], plane );
00706             break;
00707         default:
00708             return MB_FAILURE;  // child is not a box
00709     }
00710 
00711     return MB_SUCCESS;
00712 }
00713 
00714 static inline void copy_coords( double dest[3], const double source[3] )
00715 {
00716     dest[0] = source[0];
00717     dest[1] = source[1];
00718     dest[2] = source[2];
00719 }
00720 
00721 /** reverse of plane_cut_box */
00722 static inline ErrorCode plane_uncut_box( const double cut_face_in[4][3],
00723                                          double corners_inout[8][3],
00724                                          const BSPTree::Plane& plane )
00725 {
00726     switch( BSPTreeBoxIter::side_on_plane( corners_inout, plane ) )
00727     {
00728         case BSPTreeBoxIter::B0154:
00729             copy_coords( corners_inout[0], cut_face_in[0] );
00730             copy_coords( corners_inout[1], cut_face_in[1] );
00731             copy_coords( corners_inout[5], cut_face_in[2] );
00732             copy_coords( corners_inout[4], cut_face_in[3] );
00733             break;
00734         case BSPTreeBoxIter::B1265:
00735             copy_coords( corners_inout[1], cut_face_in[0] );
00736             copy_coords( corners_inout[2], cut_face_in[1] );
00737             copy_coords( corners_inout[6], cut_face_in[2] );
00738             copy_coords( corners_inout[5], cut_face_in[3] );
00739             break;
00740         case BSPTreeBoxIter::B2376:
00741             copy_coords( corners_inout[2], cut_face_in[0] );
00742             copy_coords( corners_inout[3], cut_face_in[1] );
00743             copy_coords( corners_inout[7], cut_face_in[2] );
00744             copy_coords( corners_inout[6], cut_face_in[3] );
00745             break;
00746         case BSPTreeBoxIter::B3047:
00747             copy_coords( corners_inout[3], cut_face_in[0] );
00748             copy_coords( corners_inout[0], cut_face_in[1] );
00749             copy_coords( corners_inout[4], cut_face_in[2] );
00750             copy_coords( corners_inout[7], cut_face_in[3] );
00751             break;
00752         case BSPTreeBoxIter::B3210:
00753             copy_coords( corners_inout[3], cut_face_in[0] );
00754             copy_coords( corners_inout[2], cut_face_in[1] );
00755             copy_coords( corners_inout[1], cut_face_in[2] );
00756             copy_coords( corners_inout[0], cut_face_in[3] );
00757             break;
00758         case BSPTreeBoxIter::B4567:
00759             copy_coords( corners_inout[4], cut_face_in[0] );
00760             copy_coords( corners_inout[5], cut_face_in[1] );
00761             copy_coords( corners_inout[6], cut_face_in[2] );
00762             copy_coords( corners_inout[7], cut_face_in[3] );
00763             break;
00764         default:
00765             return MB_FAILURE;  // child is not a box
00766     }
00767 
00768     return MB_SUCCESS;
00769 }
00770 
00771 ErrorCode BSPTreeBoxIter::step_to_first_leaf( Direction direction )
00772 {
00773     ErrorCode rval;
00774     BSPTree::Plane plane;
00775     Corners clipped_corners;
00776 
00777     for( ;; )
00778     {
00779         childVect.clear();
00780         rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
00781         if( MB_SUCCESS != rval ) return rval;
00782         if( childVect.empty() )  // leaf
00783             break;
00784 
00785         rval = tool()->get_split_plane( mStack.back(), plane );
00786         if( MB_SUCCESS != rval ) return rval;
00787 
00788         if( direction == RIGHT ) plane.flip();
00789         rval = plane_cut_box( clipped_corners.coords, leafCoords, plane );
00790         if( MB_SUCCESS != rval ) return rval;
00791         mStack.push_back( childVect[direction] );
00792         stackData.push_back( clipped_corners );
00793     }
00794     return MB_SUCCESS;
00795 }
00796 
00797 ErrorCode BSPTreeBoxIter::up()
00798 {
00799     ErrorCode rval;
00800     if( mStack.size() == 1 ) return MB_ENTITY_NOT_FOUND;
00801 
00802     EntityHandle node    = mStack.back();
00803     Corners clipped_face = stackData.back();
00804     mStack.pop_back();
00805     stackData.pop_back();
00806 
00807     BSPTree::Plane plane;
00808     rval = tool()->get_split_plane( mStack.back(), plane );
00809     if( MB_SUCCESS != rval )
00810     {
00811         mStack.push_back( node );
00812         stackData.push_back( clipped_face );
00813         return rval;
00814     }
00815 
00816     rval = plane_uncut_box( clipped_face.coords, leafCoords, plane );
00817     if( MB_SUCCESS != rval )
00818     {
00819         mStack.push_back( node );
00820         stackData.push_back( clipped_face );
00821         return rval;
00822     }
00823 
00824     return MB_SUCCESS;
00825 }
00826 
00827 ErrorCode BSPTreeBoxIter::down( const BSPTree::Plane& plane_ref, Direction direction )
00828 {
00829     childVect.clear();
00830     ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
00831     if( MB_SUCCESS != rval ) return rval;
00832     if( childVect.empty() ) return MB_ENTITY_NOT_FOUND;
00833 
00834     BSPTree::Plane plane( plane_ref );
00835     if( direction == RIGHT ) plane.flip();
00836 
00837     Corners clipped_face;
00838     rval = plane_cut_box( clipped_face.coords, leafCoords, plane );
00839     if( MB_SUCCESS != rval ) return rval;
00840 
00841     mStack.push_back( childVect[direction] );
00842     stackData.push_back( clipped_face );
00843     return MB_SUCCESS;
00844 }
00845 
00846 ErrorCode BSPTreeBoxIter::step( Direction direction )
00847 {
00848     EntityHandle node, parent;
00849     Corners clipped_face;
00850     ErrorCode rval;
00851     BSPTree::Plane plane;
00852     const Direction opposite = static_cast< Direction >( 1 - direction );
00853 
00854     // If stack is empty, then either this iterator is uninitialized
00855     // or we reached the end of the iteration (and return
00856     // MB_ENTITY_NOT_FOUND) already.
00857     if( mStack.empty() ) return MB_FAILURE;
00858 
00859     // Pop the current node from the stack.
00860     // The stack should then contain the parent of the current node.
00861     // If the stack is empty after this pop, then we've reached the end.
00862     node = mStack.back();
00863     mStack.pop_back();
00864     clipped_face = stackData.back();
00865     stackData.pop_back();
00866 
00867     while( !mStack.empty() )
00868     {
00869         // Get data for parent entity
00870         parent = mStack.back();
00871         childVect.clear();
00872         rval = tool()->moab()->get_child_meshsets( parent, childVect );
00873         if( MB_SUCCESS != rval ) return rval;
00874         rval = tool()->get_split_plane( parent, plane );
00875         if( MB_SUCCESS != rval ) return rval;
00876         if( direction == LEFT ) plane.flip();
00877 
00878         // If we're at the left child
00879         if( childVect[opposite] == node )
00880         {
00881             // change from box of left child to box of parent
00882             plane_uncut_box( clipped_face.coords, leafCoords, plane );
00883             // change from box of parent to box of right child
00884             plane.flip();
00885             plane_cut_box( clipped_face.coords, leafCoords, plane );
00886             // push right child on stack
00887             mStack.push_back( childVect[direction] );
00888             stackData.push_back( clipped_face );
00889             // descend to left-most leaf of the right child
00890             return step_to_first_leaf( opposite );
00891         }
00892 
00893         // The current node is the right child of the parent,
00894         // continue up the tree.
00895         assert( childVect[direction] == node );
00896         plane.flip();
00897         plane_uncut_box( clipped_face.coords, leafCoords, plane );
00898         node         = parent;
00899         clipped_face = stackData.back();
00900         mStack.pop_back();
00901         stackData.pop_back();
00902     }
00903 
00904     return MB_ENTITY_NOT_FOUND;
00905 }
00906 
00907 ErrorCode BSPTreeBoxIter::get_box_corners( double coords[8][3] ) const
00908 {
00909     memcpy( coords, leafCoords, 24 * sizeof( double ) );
00910     return MB_SUCCESS;
00911 }
00912 
00913 // result = a - b
00914 static void subtr( double result[3], const double a[3], const double b[3] )
00915 {
00916     result[0] = a[0] - b[0];
00917     result[1] = a[1] - b[1];
00918     result[2] = a[2] - b[2];
00919 }
00920 
00921 // result = a + b + c + d
00922 static void sum( double result[3], const double a[3], const double b[3], const double c[3], const double d[3] )
00923 {
00924     result[0] = a[0] + b[0] + c[0] + d[0];
00925     result[1] = a[1] + b[1] + c[1] + d[1];
00926     result[2] = a[2] + b[2] + c[2] + d[2];
00927 }
00928 
00929 // result = a cross b
00930 static void cross( double result[3], const double a[3], const double b[3] )
00931 {
00932     result[0] = a[1] * b[2] - a[2] * b[1];
00933     result[1] = a[2] * b[0] - a[0] * b[2];
00934     result[2] = a[0] * b[1] - a[1] * b[0];
00935 }
00936 
00937 static double dot( const double a[3], const double b[3] )
00938 {
00939     return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
00940 }
00941 
00942 double BSPTreeBoxIter::volume() const
00943 {
00944     // have planar sides, so use mid-face tripple product
00945     double f1[3], f2[3], f3[3], f4[3], f5[3], f6[3];
00946     sum( f1, leafCoords[0], leafCoords[1], leafCoords[4], leafCoords[5] );
00947     sum( f2, leafCoords[1], leafCoords[2], leafCoords[5], leafCoords[6] );
00948     sum( f3, leafCoords[2], leafCoords[3], leafCoords[6], leafCoords[7] );
00949     sum( f4, leafCoords[0], leafCoords[3], leafCoords[4], leafCoords[7] );
00950     sum( f5, leafCoords[0], leafCoords[1], leafCoords[2], leafCoords[3] );
00951     sum( f6, leafCoords[4], leafCoords[5], leafCoords[6], leafCoords[7] );
00952     double v13[3], v24[3], v65[3];
00953     subtr( v13, f1, f3 );
00954     subtr( v24, f2, f4 );
00955     subtr( v65, f6, f5 );
00956     double cr[3];
00957     cross( cr, v13, v24 );
00958     return ( 1. / 64 ) * dot( cr, v65 );
00959 }
00960 
00961 BSPTreeBoxIter::XSect BSPTreeBoxIter::splits( const BSPTree::Plane& plane ) const
00962 {
00963     // test each corner relative to the plane
00964     unsigned result = 0;
00965     for( unsigned i = 0; i < 8u; ++i )
00966     {
00967         double d = plane.signed_distance( leafCoords[i] );
00968         // if corner is on plane, than intersection
00969         // will result in a degenerate hex
00970         if( fabs( d ) < BSPTree::epsilon() ) return NONHEX;
00971         // if mark vertices above plane
00972         if( d > 0.0 ) result |= 1 << i;
00973     }
00974 
00975     switch( result )
00976     {
00977             // if all vertices or no vertices above plane,
00978             // then plane doesn't intersect
00979         case 0:
00980         case 0xFF:
00981             return MISS;
00982 
00983             // if there are four vertices above the plane
00984             // and they compose a single face of the hex,
00985             // then the cut will result in two hexes
00986         case B0154:
00987         case B1265:
00988         case B2376:
00989         case B3047:
00990         case B3210:
00991         case B4567:
00992             return SPLIT;
00993 
00994             // otherwise intersects, but split would not result
00995             // in two hexahedrons
00996         default:
00997             return NONHEX;
00998     }
00999 }
01000 
01001 bool BSPTreeBoxIter::intersects( const BSPTree::Plane& plane ) const
01002 {
01003     // test each corner relative to the plane
01004     unsigned count = 0;
01005     for( unsigned i = 0; i < 8u; ++i )
01006         count += plane.above( leafCoords[i] );
01007     return count > 0 && count < 8u;
01008 }
01009 
01010 ErrorCode BSPTreeBoxIter::sibling_side( SideBits& side_out ) const
01011 {
01012     if( mStack.size() < 2 )  // at tree root
01013         return MB_ENTITY_NOT_FOUND;
01014 
01015     EntityHandle parent = mStack[mStack.size() - 2];
01016     BSPTree::Plane plane;
01017     ErrorCode rval = tool()->get_split_plane( parent, plane );
01018     if( MB_SUCCESS != rval ) return MB_FAILURE;
01019 
01020     side_out = side_on_plane( leafCoords, plane );
01021     return MB_SUCCESS;
01022 }
01023 
01024 ErrorCode BSPTreeBoxIter::get_neighbors( SideBits side, std::vector< BSPTreeBoxIter >& results, double epsilon ) const
01025 {
01026     EntityHandle tmp_handle;
01027     BSPTree::Plane plane;
01028     ErrorCode rval;
01029     int n;
01030 
01031     Corners face;
01032     rval = face_corners( side, leafCoords, face.coords );
01033     if( MB_SUCCESS != rval ) return rval;
01034 
01035     // Move up tree until we find the split that created the specified side.
01036     // Push the sibling at that level onto the iterator stack as
01037     // all neighbors will be rooted at that node.
01038     BSPTreeBoxIter iter( *this );  // temporary iterator (don't modifiy *this)
01039     for( ;; )
01040     {
01041         tmp_handle = iter.handle();
01042 
01043         rval = iter.up();
01044         if( MB_SUCCESS != rval )  // reached root - no neighbors on that side
01045             return ( rval == MB_ENTITY_NOT_FOUND ) ? MB_SUCCESS : rval;
01046 
01047         iter.childVect.clear();
01048         rval = tool()->moab()->get_child_meshsets( iter.handle(), iter.childVect );
01049         if( MB_SUCCESS != rval ) return rval;
01050 
01051         rval = tool()->get_split_plane( iter.handle(), plane );
01052         if( MB_SUCCESS != rval ) return rval;
01053         SideBits s = side_above_plane( iter.leafCoords, plane );
01054 
01055         if( tmp_handle == iter.childVect[0] && s == side )
01056         {
01057             rval = iter.down( plane, RIGHT );
01058             if( MB_SUCCESS != rval ) return rval;
01059             break;
01060         }
01061         else if( tmp_handle == iter.childVect[1] && opposite_face( s ) == side )
01062         {
01063             rval = iter.down( plane, LEFT );
01064             if( MB_SUCCESS != rval ) return rval;
01065             break;
01066         }
01067     }
01068 
01069     // now move down tree, searching for adjacent boxes
01070     std::vector< BSPTreeBoxIter > list;
01071     // loop over all potential paths to neighbors (until list is empty)
01072     for( ;; )
01073     {
01074         // follow a single path to a leaf, append any other potential
01075         // paths to neighbors to 'list'
01076         for( ;; )
01077         {
01078             rval = tool()->moab()->num_child_meshsets( iter.handle(), &n );
01079             if( MB_SUCCESS != rval ) return rval;
01080 
01081             // if leaf
01082             if( !n )
01083             {
01084                 results.push_back( iter );
01085                 break;
01086             }
01087 
01088             rval = tool()->get_split_plane( iter.handle(), plane );
01089             if( MB_SUCCESS != rval ) return rval;
01090 
01091             bool some_above = false, some_below = false;
01092             for( int i = 0; i < 4; ++i )
01093             {
01094                 double signed_d = plane.signed_distance( face.coords[i] );
01095                 if( signed_d > -epsilon ) some_above = true;
01096                 if( signed_d < epsilon ) some_below = true;
01097             }
01098 
01099             if( some_above && some_below )
01100             {
01101                 list.push_back( iter );
01102                 list.back().down( plane, RIGHT );
01103                 iter.down( plane, LEFT );
01104             }
01105             else if( some_above )
01106             {
01107                 iter.down( plane, RIGHT );
01108             }
01109             else if( some_below )
01110             {
01111                 iter.down( plane, LEFT );
01112             }
01113             else
01114             {
01115                 // tolerance issue -- epsilon to small? 2D box?
01116                 return MB_FAILURE;
01117             }
01118         }
01119 
01120         if( list.empty() ) break;
01121 
01122         iter = list.back();
01123         list.pop_back();
01124     }
01125 
01126     return MB_SUCCESS;
01127 }
01128 
01129 ErrorCode BSPTreeBoxIter::calculate_polyhedron( BSPTreePoly& poly_out ) const
01130 {
01131     const CartVect* ptr = reinterpret_cast< const CartVect* >( leafCoords );
01132     return poly_out.set( ptr );
01133 }
01134 
01135 ErrorCode BSPTree::leaf_containing_point( EntityHandle tree_root, const double point[3], EntityHandle& leaf_out )
01136 {
01137     std::vector< EntityHandle > children;
01138     Plane plane;
01139     EntityHandle node = tree_root;
01140     ErrorCode rval    = moab()->get_child_meshsets( node, children );
01141     if( MB_SUCCESS != rval ) return rval;
01142     while( !children.empty() )
01143     {
01144         rval = get_split_plane( node, plane );
01145         if( MB_SUCCESS != rval ) return rval;
01146 
01147         node = children[plane.above( point )];
01148         children.clear();
01149         rval = moab()->get_child_meshsets( node, children );
01150         if( MB_SUCCESS != rval ) return rval;
01151     }
01152     leaf_out = node;
01153     return MB_SUCCESS;
01154 }
01155 
01156 ErrorCode BSPTree::leaf_containing_point( EntityHandle root, const double point[3], BSPTreeIter& iter )
01157 {
01158     ErrorCode rval;
01159 
01160     rval = iter.initialize( this, root, point );
01161     if( MB_SUCCESS != rval ) return rval;
01162 
01163     for( ;; )
01164     {
01165         iter.childVect.clear();
01166         rval = moab()->get_child_meshsets( iter.handle(), iter.childVect );
01167         if( MB_SUCCESS != rval || iter.childVect.empty() ) return rval;
01168 
01169         Plane plane;
01170         rval = get_split_plane( iter.handle(), plane );
01171         if( MB_SUCCESS != rval ) return rval;
01172 
01173         rval = iter.down( plane, ( BSPTreeIter::Direction )( plane.above( point ) ) );
01174         if( MB_SUCCESS != rval ) return rval;
01175     }
01176 }
01177 
01178 template < typename PlaneIter >
01179 static inline bool ray_intersect_halfspaces( const CartVect& ray_pt,
01180                                              const CartVect& ray_dir,
01181                                              const PlaneIter& begin,
01182                                              const PlaneIter& end,
01183                                              double& t_enter,
01184                                              double& t_exit )
01185 {
01186     const double epsilon = 1e-12;
01187 
01188     // begin with inifinite ray
01189     t_enter = 0.0;
01190     t_exit  = std::numeric_limits< double >::infinity();
01191 
01192     // cut ray such that we keep only the portion contained
01193     // in each halfspace
01194     for( PlaneIter i = begin; i != end; ++i )
01195     {
01196         CartVect norm( i->norm );
01197         double coeff = i->coeff;
01198         double den   = norm % ray_dir;
01199         if( fabs( den ) < epsilon )
01200         {                                                   // ray is parallel to plane
01201             if( i->above( ray_pt.array() ) ) return false;  // ray entirely outside half-space
01202         }
01203         else
01204         {
01205             double t_xsect = ( -coeff - ( norm % ray_pt ) ) / den;
01206             // keep portion of ray/segment below plane
01207             if( den > 0 )
01208             {
01209                 if( t_xsect < t_exit ) t_exit = t_xsect;
01210             }
01211             else
01212             {
01213                 if( t_xsect > t_enter ) t_enter = t_xsect;
01214             }
01215         }
01216     }
01217 
01218     return t_exit >= t_enter;
01219 }
01220 
01221 class BoxPlaneIter
01222 {
01223     int faceNum;
01224     BSPTree::Plane facePlanes[6];
01225 
01226   public:
01227     BoxPlaneIter( const double coords[8][3] );
01228     BoxPlaneIter() : faceNum( 6 ) {}  // initialize to 'end'
01229     const BSPTree::Plane* operator->() const
01230     {
01231         return facePlanes + faceNum;
01232     }
01233     bool operator==( const BoxPlaneIter& other ) const
01234     {
01235         return faceNum == other.faceNum;
01236     }
01237     bool operator!=( const BoxPlaneIter& other ) const
01238     {
01239         return faceNum != other.faceNum;
01240     }
01241     BoxPlaneIter& operator++()
01242     {
01243         ++faceNum;
01244         return *this;
01245     }
01246 };
01247 
01248 static const int box_face_corners[6][4] = { { 0, 1, 5, 4 }, { 1, 2, 6, 5 }, { 2, 3, 7, 6 },
01249                                             { 3, 0, 4, 7 }, { 3, 2, 1, 0 }, { 4, 5, 6, 7 } };
01250 
01251 BoxPlaneIter::BoxPlaneIter( const double coords[8][3] ) : faceNum( 0 )
01252 {
01253     // NOTE:  In the case of a BSP tree, all sides of the
01254     //        leaf will planar.
01255     assert( sizeof( CartVect ) == sizeof( coords[0] ) );
01256     const CartVect* corners = reinterpret_cast< const CartVect* >( coords );
01257     for( int i = 0; i < 6; ++i )
01258     {
01259         const int* indices = box_face_corners[i];
01260         CartVect v1        = corners[indices[1]] - corners[indices[0]];
01261         CartVect v2        = corners[indices[3]] - corners[indices[0]];
01262         CartVect n         = v1 * v2;
01263         facePlanes[i]      = BSPTree::Plane( n.array(), -( n % corners[indices[2]] ) );
01264     }
01265 }
01266 
01267 bool BSPTreeBoxIter::intersect_ray( const double ray_point[3],
01268                                     const double ray_vect[3],
01269                                     double& t_enter,
01270                                     double& t_exit ) const
01271 {
01272     BoxPlaneIter iter( this->leafCoords ), end;
01273     return ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter, end, t_enter, t_exit );
01274 }
01275 
01276 class BSPTreePlaneIter
01277 {
01278     BSPTree* toolPtr;
01279     const EntityHandle* const pathToRoot;
01280     int pathPos;
01281     BSPTree::Plane tmpPlane;
01282     std::vector< EntityHandle > tmpChildren;
01283 
01284   public:
01285     BSPTreePlaneIter( BSPTree* tool, const EntityHandle* path, int path_len )
01286         : toolPtr( tool ), pathToRoot( path ), pathPos( path_len - 1 )
01287     {
01288         operator++();
01289     }
01290     BSPTreePlaneIter()  // initialize to 'end'
01291         : toolPtr( 0 ), pathToRoot( 0 ), pathPos( -1 )
01292     {
01293     }
01294 
01295     const BSPTree::Plane* operator->() const
01296     {
01297         return &tmpPlane;
01298     }
01299     bool operator==( const BSPTreePlaneIter& other ) const
01300     {
01301         return pathPos == other.pathPos;
01302     }
01303     bool operator!=( const BSPTreePlaneIter& other ) const
01304     {
01305         return pathPos != other.pathPos;
01306     }
01307     BSPTreePlaneIter& operator++();
01308 };
01309 
01310 BSPTreePlaneIter& BSPTreePlaneIter::operator++()
01311 {
01312     if( --pathPos < 0 ) return *this;
01313 
01314     EntityHandle prev = pathToRoot[pathPos + 1];
01315     EntityHandle curr = pathToRoot[pathPos];
01316 
01317     ErrorCode rval = toolPtr->get_split_plane( curr, tmpPlane );
01318     if( MB_SUCCESS != rval )
01319     {
01320         assert( false );
01321         pathPos = 0;
01322         return *this;
01323     }
01324 
01325     tmpChildren.clear();
01326     rval = toolPtr->moab()->get_child_meshsets( curr, tmpChildren );
01327     if( MB_SUCCESS != rval || tmpChildren.size() != 2 )
01328     {
01329         assert( false );
01330         pathPos = 0;
01331         return *this;
01332     }
01333 
01334     if( tmpChildren[1] == prev ) tmpPlane.flip();
01335     return *this;
01336 }
01337 
01338 bool BSPTreeIter::intersect_ray( const double ray_point[3],
01339                                  const double ray_vect[3],
01340                                  double& t_enter,
01341                                  double& t_exit ) const
01342 {
01343     // intersect with half-spaces defining tree
01344     BSPTreePlaneIter iter1( tool(), &mStack[0], mStack.size() ), end1;
01345     if( !ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter1, end1, t_enter, t_exit ) )
01346         return false;
01347 
01348     // itersect with box bounding entire tree
01349     double corners[8][3];
01350     ErrorCode rval = tool()->get_tree_box( mStack.front(), corners );
01351     if( MB_SUCCESS != rval )
01352     {
01353         assert( false );
01354         return false;
01355     }
01356 
01357     BoxPlaneIter iter2( corners ), end2;
01358     double t2_enter, t2_exit;
01359     if( !ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter2, end2, t2_enter, t2_exit ) )
01360         return false;
01361 
01362     // if intersect both box and halfspaces, check that
01363     // two intersections overlap
01364     if( t_enter < t2_enter ) t_enter = t2_enter;
01365     if( t_exit > t2_exit ) t_exit = t2_exit;
01366     return t_enter <= t_exit;
01367 }
01368 
01369 }  // namespace moab
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines