MOAB: Mesh Oriented datABase
(version 5.4.1)
|
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 ([email protected]) 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