MOAB: Mesh Oriented datABase
(version 5.2.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 (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 <assert.h> 00029 #include <string.h> 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, Plane plane, const std::vector< EntityHandle >& left_entities, 00305 const std::vector< EntityHandle >& right_entities ) 00306 { 00307 EntityHandle left, right, parent = leaf.handle(); 00308 ErrorCode rval = split_leaf( leaf, plane, left, right ); 00309 if( MB_SUCCESS != rval ) return rval; 00310 00311 if( MB_SUCCESS == moab()->add_entities( left, &left_entities[0], left_entities.size() ) && 00312 MB_SUCCESS == moab()->add_entities( right, &right_entities[0], right_entities.size() ) && 00313 MB_SUCCESS == moab()->clear_meshset( &parent, 1 ) ) 00314 return MB_SUCCESS; 00315 00316 moab()->remove_child_meshset( parent, left ); 00317 moab()->remove_child_meshset( parent, right ); 00318 EntityHandle children[] = { left, right }; 00319 moab()->delete_entities( children, 2 ); 00320 return MB_FAILURE; 00321 } 00322 00323 ErrorCode BSPTree::merge_leaf( BSPTreeIter& iter ) 00324 { 00325 ErrorCode rval; 00326 if( iter.depth() == 1 ) // at root 00327 return MB_FAILURE; 00328 00329 // Move iter to parent 00330 iter.up(); 00331 00332 // Get sets to merge 00333 EntityHandle parent = iter.handle(); 00334 iter.childVect.clear(); 00335 rval = moab()->get_child_meshsets( parent, iter.childVect ); 00336 if( MB_SUCCESS != rval ) return rval; 00337 00338 // Remove child links 00339 moab()->remove_child_meshset( parent, iter.childVect[0] ); 00340 moab()->remove_child_meshset( parent, iter.childVect[1] ); 00341 std::vector< EntityHandle > stack( iter.childVect ); 00342 00343 // Get all entities from children and put them in parent 00344 Range range; 00345 while( !stack.empty() ) 00346 { 00347 EntityHandle h = stack.back(); 00348 stack.pop_back(); 00349 range.clear(); 00350 rval = moab()->get_entities_by_handle( h, range ); 00351 if( MB_SUCCESS != rval ) return rval; 00352 rval = moab()->add_entities( parent, range ); 00353 if( MB_SUCCESS != rval ) return rval; 00354 00355 iter.childVect.clear(); 00356 rval = moab()->get_child_meshsets( h, iter.childVect );MB_CHK_ERR( rval ); 00357 if( !iter.childVect.empty() ) 00358 { 00359 moab()->remove_child_meshset( h, iter.childVect[0] ); 00360 moab()->remove_child_meshset( h, iter.childVect[1] ); 00361 stack.push_back( iter.childVect[0] ); 00362 stack.push_back( iter.childVect[1] ); 00363 } 00364 00365 rval = moab()->delete_entities( &h, 1 ); 00366 if( MB_SUCCESS != rval ) return rval; 00367 } 00368 00369 return MB_SUCCESS; 00370 } 00371 00372 ErrorCode BSPTreeIter::initialize( BSPTree* btool, EntityHandle root, const double* /*point*/ ) 00373 { 00374 treeTool = btool; 00375 mStack.clear(); 00376 mStack.push_back( root ); 00377 return MB_SUCCESS; 00378 } 00379 00380 ErrorCode BSPTreeIter::step_to_first_leaf( Direction direction ) 00381 { 00382 ErrorCode rval; 00383 for( ;; ) 00384 { 00385 childVect.clear(); 00386 rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect ); 00387 if( MB_SUCCESS != rval ) return rval; 00388 if( childVect.empty() ) // leaf 00389 break; 00390 00391 mStack.push_back( childVect[direction] ); 00392 } 00393 return MB_SUCCESS; 00394 } 00395 00396 ErrorCode BSPTreeIter::step( Direction direction ) 00397 { 00398 EntityHandle node, parent; 00399 ErrorCode rval; 00400 const Direction opposite = static_cast< Direction >( 1 - direction ); 00401 00402 // If stack is empty, then either this iterator is uninitialized 00403 // or we reached the end of the iteration (and return 00404 // MB_ENTITY_NOT_FOUND) already. 00405 if( mStack.empty() ) return MB_FAILURE; 00406 00407 // Pop the current node from the stack. 00408 // The stack should then contain the parent of the current node. 00409 // If the stack is empty after this pop, then we've reached the end. 00410 node = mStack.back(); 00411 mStack.pop_back(); 00412 00413 while( !mStack.empty() ) 00414 { 00415 // Get data for parent entity 00416 parent = mStack.back(); 00417 childVect.clear(); 00418 rval = tool()->moab()->get_child_meshsets( parent, childVect ); 00419 if( MB_SUCCESS != rval ) return rval; 00420 00421 // If we're at the left child 00422 if( childVect[opposite] == node ) 00423 { 00424 // push right child on stack 00425 mStack.push_back( childVect[direction] ); 00426 // descend to left-most leaf of the right child 00427 return step_to_first_leaf( opposite ); 00428 } 00429 00430 // The current node is the right child of the parent, 00431 // continue up the tree. 00432 assert( childVect[direction] == node ); 00433 node = parent; 00434 mStack.pop_back(); 00435 } 00436 00437 return MB_ENTITY_NOT_FOUND; 00438 } 00439 00440 ErrorCode BSPTreeIter::up() 00441 { 00442 if( mStack.size() < 2 ) return MB_ENTITY_NOT_FOUND; 00443 mStack.pop_back(); 00444 return MB_SUCCESS; 00445 } 00446 00447 ErrorCode BSPTreeIter::down( const BSPTree::Plane& /*plane*/, Direction dir ) 00448 { 00449 childVect.clear(); 00450 ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect ); 00451 if( MB_SUCCESS != rval ) return rval; 00452 if( childVect.empty() ) return MB_ENTITY_NOT_FOUND; 00453 00454 mStack.push_back( childVect[dir] ); 00455 return MB_SUCCESS; 00456 } 00457 00458 ErrorCode BSPTreeIter::get_parent_split_plane( BSPTree::Plane& plane ) const 00459 { 00460 if( mStack.size() < 2 ) // at tree root 00461 return MB_ENTITY_NOT_FOUND; 00462 00463 EntityHandle parent = mStack[mStack.size() - 2]; 00464 return tool()->get_split_plane( parent, plane ); 00465 } 00466 00467 double BSPTreeIter::volume() const 00468 { 00469 BSPTreePoly polyhedron; 00470 ErrorCode rval = calculate_polyhedron( polyhedron ); 00471 return MB_SUCCESS == rval ? polyhedron.volume() : -1.0; 00472 } 00473 00474 bool BSPTreeIter::is_sibling( const BSPTreeIter& other_leaf ) const 00475 { 00476 const size_t s = mStack.size(); 00477 return ( s > 1 ) && ( s == other_leaf.mStack.size() ) && ( other_leaf.mStack[s - 2] == mStack[s - 2] ) && 00478 other_leaf.handle() != handle(); 00479 } 00480 00481 bool BSPTreeIter::is_sibling( EntityHandle other_leaf ) const 00482 { 00483 if( mStack.size() < 2 || other_leaf == handle() ) return false; 00484 EntityHandle parent = mStack[mStack.size() - 2]; 00485 childVect.clear(); 00486 ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect ); 00487 if( MB_SUCCESS != rval || childVect.size() != 2 ) 00488 { 00489 assert( false ); 00490 return false; 00491 } 00492 return childVect[0] == other_leaf || childVect[1] == other_leaf; 00493 } 00494 00495 bool BSPTreeIter::sibling_is_forward() const 00496 { 00497 if( mStack.size() < 2 ) // if root 00498 return false; 00499 EntityHandle parent = mStack[mStack.size() - 2]; 00500 childVect.clear(); 00501 ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect ); 00502 if( MB_SUCCESS != rval || childVect.size() != 2 ) 00503 { 00504 assert( false ); 00505 return false; 00506 } 00507 return childVect[0] == handle(); 00508 } 00509 00510 ErrorCode BSPTreeIter::calculate_polyhedron( BSPTreePoly& poly_out ) const 00511 { 00512 ErrorCode rval; 00513 00514 assert( sizeof( CartVect ) == 3 * sizeof( double ) ); 00515 CartVect corners[8]; 00516 rval = treeTool->get_tree_box( mStack.front(), corners[0].array() ); 00517 if( MB_SUCCESS != rval ) return rval; 00518 00519 rval = poly_out.set( corners ); 00520 if( MB_SUCCESS != rval ) return rval; 00521 00522 BSPTree::Plane plane; 00523 std::vector< EntityHandle >::const_iterator i = mStack.begin(); 00524 std::vector< EntityHandle >::const_iterator here = mStack.end() - 1; 00525 while( i != here ) 00526 { 00527 rval = treeTool->get_split_plane( *i, plane ); 00528 if( MB_SUCCESS != rval ) return rval; 00529 00530 childVect.clear(); 00531 rval = treeTool->moab()->get_child_meshsets( *i, childVect ); 00532 if( MB_SUCCESS != rval ) return rval; 00533 if( childVect.size() != 2 ) return MB_FAILURE; 00534 00535 ++i; 00536 if( childVect[1] == *i ) plane.flip(); 00537 00538 CartVect norm( plane.norm ); 00539 poly_out.cut_polyhedron( norm, plane.coeff ); 00540 } 00541 00542 return MB_SUCCESS; 00543 } 00544 00545 ErrorCode BSPTreeBoxIter::initialize( BSPTree* tool_ptr, EntityHandle root, const double* point ) 00546 { 00547 ErrorCode rval = BSPTreeIter::initialize( tool_ptr, root ); 00548 if( MB_SUCCESS != rval ) return rval; 00549 00550 rval = tool()->get_tree_box( root, leafCoords ); 00551 if( MB_SUCCESS != rval ) return rval; 00552 00553 if( point && !point_in_box( leafCoords, point ) ) return MB_ENTITY_NOT_FOUND; 00554 00555 stackData.resize( 1 ); 00556 return MB_SUCCESS; 00557 } 00558 00559 BSPTreeBoxIter::SideBits BSPTreeBoxIter::side_above_plane( const double hex_coords[8][3], const BSPTree::Plane& plane ) 00560 { 00561 unsigned result = 0; 00562 for( unsigned i = 0; i < 8u; ++i ) 00563 result |= plane.above( hex_coords[i] ) << i; 00564 return (BSPTreeBoxIter::SideBits)result; 00565 } 00566 00567 BSPTreeBoxIter::SideBits BSPTreeBoxIter::side_on_plane( const double hex_coords[8][3], const BSPTree::Plane& plane ) 00568 { 00569 unsigned result = 0; 00570 for( unsigned i = 0; i < 8u; ++i ) 00571 { 00572 bool on = plane.distance( hex_coords[i] ) <= BSPTree::epsilon(); 00573 result |= on << i; 00574 } 00575 return (BSPTreeBoxIter::SideBits)result; 00576 } 00577 00578 static inline void copy_coords( const double src[3], double dest[3] ) 00579 { 00580 dest[0] = src[0]; 00581 dest[1] = src[1]; 00582 dest[2] = src[2]; 00583 } 00584 00585 ErrorCode BSPTreeBoxIter::face_corners( const SideBits face, const double hex_corners[8][3], double face_corners[4][3] ) 00586 { 00587 switch( face ) 00588 { 00589 case BSPTreeBoxIter::B0154: 00590 copy_coords( hex_corners[0], face_corners[0] ); 00591 copy_coords( hex_corners[1], face_corners[1] ); 00592 copy_coords( hex_corners[5], face_corners[2] ); 00593 copy_coords( hex_corners[4], face_corners[3] ); 00594 break; 00595 case BSPTreeBoxIter::B1265: 00596 copy_coords( hex_corners[1], face_corners[0] ); 00597 copy_coords( hex_corners[2], face_corners[1] ); 00598 copy_coords( hex_corners[6], face_corners[2] ); 00599 copy_coords( hex_corners[5], face_corners[3] ); 00600 break; 00601 case BSPTreeBoxIter::B2376: 00602 copy_coords( hex_corners[2], face_corners[0] ); 00603 copy_coords( hex_corners[3], face_corners[1] ); 00604 copy_coords( hex_corners[7], face_corners[2] ); 00605 copy_coords( hex_corners[6], face_corners[3] ); 00606 break; 00607 case BSPTreeBoxIter::B3047: 00608 copy_coords( hex_corners[3], face_corners[0] ); 00609 copy_coords( hex_corners[0], face_corners[1] ); 00610 copy_coords( hex_corners[4], face_corners[2] ); 00611 copy_coords( hex_corners[7], face_corners[3] ); 00612 break; 00613 case BSPTreeBoxIter::B3210: 00614 copy_coords( hex_corners[3], face_corners[0] ); 00615 copy_coords( hex_corners[2], face_corners[1] ); 00616 copy_coords( hex_corners[1], face_corners[2] ); 00617 copy_coords( hex_corners[0], face_corners[3] ); 00618 break; 00619 case BSPTreeBoxIter::B4567: 00620 copy_coords( hex_corners[4], face_corners[0] ); 00621 copy_coords( hex_corners[5], face_corners[1] ); 00622 copy_coords( hex_corners[6], face_corners[2] ); 00623 copy_coords( hex_corners[7], face_corners[3] ); 00624 break; 00625 default: 00626 return MB_FAILURE; // child is not a box 00627 } 00628 00629 return MB_SUCCESS; 00630 } 00631 00632 /** \brief Clip an edge using a plane 00633 * 00634 * Given an edge from keep_end_coords to cut_end_coords, 00635 * cut the edge using the passed plane, such that cut_end_coords 00636 * is updated with a new location on the plane, and old_coords_out 00637 * contains the original value of cut_end_coords. 00638 */ 00639 static inline void plane_cut_edge( double old_coords_out[3], const double keep_end_coords[3], double cut_end_coords[3], 00640 const BSPTree::Plane& plane ) 00641 { 00642 const CartVect start( keep_end_coords ), end( cut_end_coords ); 00643 const CartVect norm( plane.norm ); 00644 CartVect xsect_point; 00645 00646 const CartVect m = end - start; 00647 const double t = -( norm % start + plane.coeff ) / ( norm % m ); 00648 assert( t > 0.0 && t < 1.0 ); 00649 xsect_point = start + t * m; 00650 00651 end.get( old_coords_out ); 00652 xsect_point.get( cut_end_coords ); 00653 } 00654 00655 /** Given the corners of a hexahedron in corners_input and a 00656 * plane, cut the hex with the plane, updating corners_input 00657 * and storing the original,cut-off side of the hex in cut_face_out. 00658 * 00659 * The portion of the hex below the plane is retained. cut_face_out 00660 * will contain the side of the hex that is entirely above the plane. 00661 *\return MB_FAILURE if plane/hex intersection is not a quadrilateral. 00662 */ 00663 static ErrorCode plane_cut_box( double cut_face_out[4][3], double corners_inout[8][3], const BSPTree::Plane& plane ) 00664 { 00665 switch( BSPTreeBoxIter::side_above_plane( corners_inout, plane ) ) 00666 { 00667 case BSPTreeBoxIter::B0154: 00668 plane_cut_edge( cut_face_out[0], corners_inout[3], corners_inout[0], plane ); 00669 plane_cut_edge( cut_face_out[1], corners_inout[2], corners_inout[1], plane ); 00670 plane_cut_edge( cut_face_out[2], corners_inout[6], corners_inout[5], plane ); 00671 plane_cut_edge( cut_face_out[3], corners_inout[7], corners_inout[4], plane ); 00672 break; 00673 case BSPTreeBoxIter::B1265: 00674 plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[1], plane ); 00675 plane_cut_edge( cut_face_out[1], corners_inout[3], corners_inout[2], plane ); 00676 plane_cut_edge( cut_face_out[2], corners_inout[7], corners_inout[6], plane ); 00677 plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[5], plane ); 00678 break; 00679 case BSPTreeBoxIter::B2376: 00680 plane_cut_edge( cut_face_out[0], corners_inout[1], corners_inout[2], plane ); 00681 plane_cut_edge( cut_face_out[1], corners_inout[0], corners_inout[3], plane ); 00682 plane_cut_edge( cut_face_out[2], corners_inout[4], corners_inout[7], plane ); 00683 plane_cut_edge( cut_face_out[3], corners_inout[5], corners_inout[6], plane ); 00684 break; 00685 case BSPTreeBoxIter::B3047: 00686 plane_cut_edge( cut_face_out[0], corners_inout[2], corners_inout[3], plane ); 00687 plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[0], plane ); 00688 plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[4], plane ); 00689 plane_cut_edge( cut_face_out[3], corners_inout[6], corners_inout[7], plane ); 00690 break; 00691 case BSPTreeBoxIter::B3210: 00692 plane_cut_edge( cut_face_out[0], corners_inout[7], corners_inout[3], plane ); 00693 plane_cut_edge( cut_face_out[1], corners_inout[6], corners_inout[2], plane ); 00694 plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[1], plane ); 00695 plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[0], plane ); 00696 break; 00697 case BSPTreeBoxIter::B4567: 00698 plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[4], plane ); 00699 plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[5], plane ); 00700 plane_cut_edge( cut_face_out[2], corners_inout[2], corners_inout[6], plane ); 00701 plane_cut_edge( cut_face_out[3], corners_inout[3], corners_inout[7], plane ); 00702 break; 00703 default: 00704 return MB_FAILURE; // child is not a box 00705 } 00706 00707 return MB_SUCCESS; 00708 } 00709 00710 static inline void copy_coords( double dest[3], const double source[3] ) 00711 { 00712 dest[0] = source[0]; 00713 dest[1] = source[1]; 00714 dest[2] = source[2]; 00715 } 00716 00717 /** reverse of plane_cut_box */ 00718 static inline ErrorCode plane_uncut_box( const double cut_face_in[4][3], double corners_inout[8][3], 00719 const BSPTree::Plane& plane ) 00720 { 00721 switch( BSPTreeBoxIter::side_on_plane( corners_inout, plane ) ) 00722 { 00723 case BSPTreeBoxIter::B0154: 00724 copy_coords( corners_inout[0], cut_face_in[0] ); 00725 copy_coords( corners_inout[1], cut_face_in[1] ); 00726 copy_coords( corners_inout[5], cut_face_in[2] ); 00727 copy_coords( corners_inout[4], cut_face_in[3] ); 00728 break; 00729 case BSPTreeBoxIter::B1265: 00730 copy_coords( corners_inout[1], cut_face_in[0] ); 00731 copy_coords( corners_inout[2], cut_face_in[1] ); 00732 copy_coords( corners_inout[6], cut_face_in[2] ); 00733 copy_coords( corners_inout[5], cut_face_in[3] ); 00734 break; 00735 case BSPTreeBoxIter::B2376: 00736 copy_coords( corners_inout[2], cut_face_in[0] ); 00737 copy_coords( corners_inout[3], cut_face_in[1] ); 00738 copy_coords( corners_inout[7], cut_face_in[2] ); 00739 copy_coords( corners_inout[6], cut_face_in[3] ); 00740 break; 00741 case BSPTreeBoxIter::B3047: 00742 copy_coords( corners_inout[3], cut_face_in[0] ); 00743 copy_coords( corners_inout[0], cut_face_in[1] ); 00744 copy_coords( corners_inout[4], cut_face_in[2] ); 00745 copy_coords( corners_inout[7], cut_face_in[3] ); 00746 break; 00747 case BSPTreeBoxIter::B3210: 00748 copy_coords( corners_inout[3], cut_face_in[0] ); 00749 copy_coords( corners_inout[2], cut_face_in[1] ); 00750 copy_coords( corners_inout[1], cut_face_in[2] ); 00751 copy_coords( corners_inout[0], cut_face_in[3] ); 00752 break; 00753 case BSPTreeBoxIter::B4567: 00754 copy_coords( corners_inout[4], cut_face_in[0] ); 00755 copy_coords( corners_inout[5], cut_face_in[1] ); 00756 copy_coords( corners_inout[6], cut_face_in[2] ); 00757 copy_coords( corners_inout[7], cut_face_in[3] ); 00758 break; 00759 default: 00760 return MB_FAILURE; // child is not a box 00761 } 00762 00763 return MB_SUCCESS; 00764 } 00765 00766 ErrorCode BSPTreeBoxIter::step_to_first_leaf( Direction direction ) 00767 { 00768 ErrorCode rval; 00769 BSPTree::Plane plane; 00770 Corners clipped_corners; 00771 00772 for( ;; ) 00773 { 00774 childVect.clear(); 00775 rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect ); 00776 if( MB_SUCCESS != rval ) return rval; 00777 if( childVect.empty() ) // leaf 00778 break; 00779 00780 rval = tool()->get_split_plane( mStack.back(), plane ); 00781 if( MB_SUCCESS != rval ) return rval; 00782 00783 if( direction == RIGHT ) plane.flip(); 00784 rval = plane_cut_box( clipped_corners.coords, leafCoords, plane ); 00785 if( MB_SUCCESS != rval ) return rval; 00786 mStack.push_back( childVect[direction] ); 00787 stackData.push_back( clipped_corners ); 00788 } 00789 return MB_SUCCESS; 00790 } 00791 00792 ErrorCode BSPTreeBoxIter::up() 00793 { 00794 ErrorCode rval; 00795 if( mStack.size() == 1 ) return MB_ENTITY_NOT_FOUND; 00796 00797 EntityHandle node = mStack.back(); 00798 Corners clipped_face = stackData.back(); 00799 mStack.pop_back(); 00800 stackData.pop_back(); 00801 00802 BSPTree::Plane plane; 00803 rval = tool()->get_split_plane( mStack.back(), plane ); 00804 if( MB_SUCCESS != rval ) 00805 { 00806 mStack.push_back( node ); 00807 stackData.push_back( clipped_face ); 00808 return rval; 00809 } 00810 00811 rval = plane_uncut_box( clipped_face.coords, leafCoords, plane ); 00812 if( MB_SUCCESS != rval ) 00813 { 00814 mStack.push_back( node ); 00815 stackData.push_back( clipped_face ); 00816 return rval; 00817 } 00818 00819 return MB_SUCCESS; 00820 } 00821 00822 ErrorCode BSPTreeBoxIter::down( const BSPTree::Plane& plane_ref, Direction direction ) 00823 { 00824 childVect.clear(); 00825 ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect ); 00826 if( MB_SUCCESS != rval ) return rval; 00827 if( childVect.empty() ) return MB_ENTITY_NOT_FOUND; 00828 00829 BSPTree::Plane plane( plane_ref ); 00830 if( direction == RIGHT ) plane.flip(); 00831 00832 Corners clipped_face; 00833 rval = plane_cut_box( clipped_face.coords, leafCoords, plane ); 00834 if( MB_SUCCESS != rval ) return rval; 00835 00836 mStack.push_back( childVect[direction] ); 00837 stackData.push_back( clipped_face ); 00838 return MB_SUCCESS; 00839 } 00840 00841 ErrorCode BSPTreeBoxIter::step( Direction direction ) 00842 { 00843 EntityHandle node, parent; 00844 Corners clipped_face; 00845 ErrorCode rval; 00846 BSPTree::Plane plane; 00847 const Direction opposite = static_cast< Direction >( 1 - direction ); 00848 00849 // If stack is empty, then either this iterator is uninitialized 00850 // or we reached the end of the iteration (and return 00851 // MB_ENTITY_NOT_FOUND) already. 00852 if( mStack.empty() ) return MB_FAILURE; 00853 00854 // Pop the current node from the stack. 00855 // The stack should then contain the parent of the current node. 00856 // If the stack is empty after this pop, then we've reached the end. 00857 node = mStack.back(); 00858 mStack.pop_back(); 00859 clipped_face = stackData.back(); 00860 stackData.pop_back(); 00861 00862 while( !mStack.empty() ) 00863 { 00864 // Get data for parent entity 00865 parent = mStack.back(); 00866 childVect.clear(); 00867 rval = tool()->moab()->get_child_meshsets( parent, childVect ); 00868 if( MB_SUCCESS != rval ) return rval; 00869 rval = tool()->get_split_plane( parent, plane ); 00870 if( MB_SUCCESS != rval ) return rval; 00871 if( direction == LEFT ) plane.flip(); 00872 00873 // If we're at the left child 00874 if( childVect[opposite] == node ) 00875 { 00876 // change from box of left child to box of parent 00877 plane_uncut_box( clipped_face.coords, leafCoords, plane ); 00878 // change from box of parent to box of right child 00879 plane.flip(); 00880 plane_cut_box( clipped_face.coords, leafCoords, plane ); 00881 // push right child on stack 00882 mStack.push_back( childVect[direction] ); 00883 stackData.push_back( clipped_face ); 00884 // descend to left-most leaf of the right child 00885 return step_to_first_leaf( opposite ); 00886 } 00887 00888 // The current node is the right child of the parent, 00889 // continue up the tree. 00890 assert( childVect[direction] == node ); 00891 plane.flip(); 00892 plane_uncut_box( clipped_face.coords, leafCoords, plane ); 00893 node = parent; 00894 clipped_face = stackData.back(); 00895 mStack.pop_back(); 00896 stackData.pop_back(); 00897 } 00898 00899 return MB_ENTITY_NOT_FOUND; 00900 } 00901 00902 ErrorCode BSPTreeBoxIter::get_box_corners( double coords[8][3] ) const 00903 { 00904 memcpy( coords, leafCoords, 24 * sizeof( double ) ); 00905 return MB_SUCCESS; 00906 } 00907 00908 // result = a - b 00909 static void subtr( double result[3], const double a[3], const double b[3] ) 00910 { 00911 result[0] = a[0] - b[0]; 00912 result[1] = a[1] - b[1]; 00913 result[2] = a[2] - b[2]; 00914 } 00915 00916 // result = a + b + c + d 00917 static void sum( double result[3], const double a[3], const double b[3], const double c[3], const double d[3] ) 00918 { 00919 result[0] = a[0] + b[0] + c[0] + d[0]; 00920 result[1] = a[1] + b[1] + c[1] + d[1]; 00921 result[2] = a[2] + b[2] + c[2] + d[2]; 00922 } 00923 00924 // result = a cross b 00925 static void cross( double result[3], const double a[3], const double b[3] ) 00926 { 00927 result[0] = a[1] * b[2] - a[2] * b[1]; 00928 result[1] = a[2] * b[0] - a[0] * b[2]; 00929 result[2] = a[0] * b[1] - a[1] * b[0]; 00930 } 00931 00932 static double dot( const double a[3], const double b[3] ) 00933 { 00934 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; 00935 } 00936 00937 double BSPTreeBoxIter::volume() const 00938 { 00939 // have planar sides, so use mid-face tripple product 00940 double f1[3], f2[3], f3[3], f4[3], f5[3], f6[3]; 00941 sum( f1, leafCoords[0], leafCoords[1], leafCoords[4], leafCoords[5] ); 00942 sum( f2, leafCoords[1], leafCoords[2], leafCoords[5], leafCoords[6] ); 00943 sum( f3, leafCoords[2], leafCoords[3], leafCoords[6], leafCoords[7] ); 00944 sum( f4, leafCoords[0], leafCoords[3], leafCoords[4], leafCoords[7] ); 00945 sum( f5, leafCoords[0], leafCoords[1], leafCoords[2], leafCoords[3] ); 00946 sum( f6, leafCoords[4], leafCoords[5], leafCoords[6], leafCoords[7] ); 00947 double v13[3], v24[3], v65[3]; 00948 subtr( v13, f1, f3 ); 00949 subtr( v24, f2, f4 ); 00950 subtr( v65, f6, f5 ); 00951 double cr[3]; 00952 cross( cr, v13, v24 ); 00953 return ( 1. / 64 ) * dot( cr, v65 ); 00954 } 00955 00956 BSPTreeBoxIter::XSect BSPTreeBoxIter::splits( const BSPTree::Plane& plane ) const 00957 { 00958 // test each corner relative to the plane 00959 unsigned result = 0; 00960 for( unsigned i = 0; i < 8u; ++i ) 00961 { 00962 double d = plane.signed_distance( leafCoords[i] ); 00963 // if corner is on plane, than intersection 00964 // will result in a degenerate hex 00965 if( fabs( d ) < BSPTree::epsilon() ) return NONHEX; 00966 // if mark vertices above plane 00967 if( d > 0.0 ) result |= 1 << i; 00968 } 00969 00970 switch( result ) 00971 { 00972 // if all vertices or no vertices above plane, 00973 // then plane doesn't intersect 00974 case 0: 00975 case 0xFF: 00976 return MISS; 00977 00978 // if there are four vertices above the plane 00979 // and they compose a single face of the hex, 00980 // then the cut will result in two hexes 00981 case B0154: 00982 case B1265: 00983 case B2376: 00984 case B3047: 00985 case B3210: 00986 case B4567: 00987 return SPLIT; 00988 00989 // otherwise intersects, but split would not result 00990 // in two hexahedrons 00991 default: 00992 return NONHEX; 00993 } 00994 } 00995 00996 bool BSPTreeBoxIter::intersects( const BSPTree::Plane& plane ) const 00997 { 00998 // test each corner relative to the plane 00999 unsigned count = 0; 01000 for( unsigned i = 0; i < 8u; ++i ) 01001 count += plane.above( leafCoords[i] ); 01002 return count > 0 && count < 8u; 01003 } 01004 01005 ErrorCode BSPTreeBoxIter::sibling_side( SideBits& side_out ) const 01006 { 01007 if( mStack.size() < 2 ) // at tree root 01008 return MB_ENTITY_NOT_FOUND; 01009 01010 EntityHandle parent = mStack[mStack.size() - 2]; 01011 BSPTree::Plane plane; 01012 ErrorCode rval = tool()->get_split_plane( parent, plane ); 01013 if( MB_SUCCESS != rval ) return MB_FAILURE; 01014 01015 side_out = side_on_plane( leafCoords, plane ); 01016 return MB_SUCCESS; 01017 } 01018 01019 ErrorCode BSPTreeBoxIter::get_neighbors( SideBits side, std::vector< BSPTreeBoxIter >& results, double epsilon ) const 01020 { 01021 EntityHandle tmp_handle; 01022 BSPTree::Plane plane; 01023 ErrorCode rval; 01024 int n; 01025 01026 Corners face; 01027 rval = face_corners( side, leafCoords, face.coords ); 01028 if( MB_SUCCESS != rval ) return rval; 01029 01030 // Move up tree until we find the split that created the specified side. 01031 // Push the sibling at that level onto the iterator stack as 01032 // all neighbors will be rooted at that node. 01033 BSPTreeBoxIter iter( *this ); // temporary iterator (don't modifiy *this) 01034 for( ;; ) 01035 { 01036 tmp_handle = iter.handle(); 01037 01038 rval = iter.up(); 01039 if( MB_SUCCESS != rval ) // reached root - no neighbors on that side 01040 return ( rval == MB_ENTITY_NOT_FOUND ) ? MB_SUCCESS : rval; 01041 01042 iter.childVect.clear(); 01043 rval = tool()->moab()->get_child_meshsets( iter.handle(), iter.childVect ); 01044 if( MB_SUCCESS != rval ) return rval; 01045 01046 rval = tool()->get_split_plane( iter.handle(), plane ); 01047 if( MB_SUCCESS != rval ) return rval; 01048 SideBits s = side_above_plane( iter.leafCoords, plane ); 01049 01050 if( tmp_handle == iter.childVect[0] && s == side ) 01051 { 01052 rval = iter.down( plane, RIGHT ); 01053 if( MB_SUCCESS != rval ) return rval; 01054 break; 01055 } 01056 else if( tmp_handle == iter.childVect[1] && opposite_face( s ) == side ) 01057 { 01058 rval = iter.down( plane, LEFT ); 01059 if( MB_SUCCESS != rval ) return rval; 01060 break; 01061 } 01062 } 01063 01064 // now move down tree, searching for adjacent boxes 01065 std::vector< BSPTreeBoxIter > list; 01066 // loop over all potential paths to neighbors (until list is empty) 01067 for( ;; ) 01068 { 01069 // follow a single path to a leaf, append any other potential 01070 // paths to neighbors to 'list' 01071 for( ;; ) 01072 { 01073 rval = tool()->moab()->num_child_meshsets( iter.handle(), &n ); 01074 if( MB_SUCCESS != rval ) return rval; 01075 01076 // if leaf 01077 if( !n ) 01078 { 01079 results.push_back( iter ); 01080 break; 01081 } 01082 01083 rval = tool()->get_split_plane( iter.handle(), plane ); 01084 if( MB_SUCCESS != rval ) return rval; 01085 01086 bool some_above = false, some_below = false; 01087 for( int i = 0; i < 4; ++i ) 01088 { 01089 double signed_d = plane.signed_distance( face.coords[i] ); 01090 if( signed_d > -epsilon ) some_above = true; 01091 if( signed_d < epsilon ) some_below = true; 01092 } 01093 01094 if( some_above && some_below ) 01095 { 01096 list.push_back( iter ); 01097 list.back().down( plane, RIGHT ); 01098 iter.down( plane, LEFT ); 01099 } 01100 else if( some_above ) 01101 { 01102 iter.down( plane, RIGHT ); 01103 } 01104 else if( some_below ) 01105 { 01106 iter.down( plane, LEFT ); 01107 } 01108 else 01109 { 01110 // tolerance issue -- epsilon to small? 2D box? 01111 return MB_FAILURE; 01112 } 01113 } 01114 01115 if( list.empty() ) break; 01116 01117 iter = list.back(); 01118 list.pop_back(); 01119 } 01120 01121 return MB_SUCCESS; 01122 } 01123 01124 ErrorCode BSPTreeBoxIter::calculate_polyhedron( BSPTreePoly& poly_out ) const 01125 { 01126 const CartVect* ptr = reinterpret_cast< const CartVect* >( leafCoords ); 01127 return poly_out.set( ptr ); 01128 } 01129 01130 ErrorCode BSPTree::leaf_containing_point( EntityHandle tree_root, const double point[3], EntityHandle& leaf_out ) 01131 { 01132 std::vector< EntityHandle > children; 01133 Plane plane; 01134 EntityHandle node = tree_root; 01135 ErrorCode rval = moab()->get_child_meshsets( node, children ); 01136 if( MB_SUCCESS != rval ) return rval; 01137 while( !children.empty() ) 01138 { 01139 rval = get_split_plane( node, plane ); 01140 if( MB_SUCCESS != rval ) return rval; 01141 01142 node = children[plane.above( point )]; 01143 children.clear(); 01144 rval = moab()->get_child_meshsets( node, children ); 01145 if( MB_SUCCESS != rval ) return rval; 01146 } 01147 leaf_out = node; 01148 return MB_SUCCESS; 01149 } 01150 01151 ErrorCode BSPTree::leaf_containing_point( EntityHandle root, const double point[3], BSPTreeIter& iter ) 01152 { 01153 ErrorCode rval; 01154 01155 rval = iter.initialize( this, root, point ); 01156 if( MB_SUCCESS != rval ) return rval; 01157 01158 for( ;; ) 01159 { 01160 iter.childVect.clear(); 01161 rval = moab()->get_child_meshsets( iter.handle(), iter.childVect ); 01162 if( MB_SUCCESS != rval || iter.childVect.empty() ) return rval; 01163 01164 Plane plane; 01165 rval = get_split_plane( iter.handle(), plane ); 01166 if( MB_SUCCESS != rval ) return rval; 01167 01168 rval = iter.down( plane, ( BSPTreeIter::Direction )( plane.above( point ) ) ); 01169 if( MB_SUCCESS != rval ) return rval; 01170 } 01171 } 01172 01173 template < typename PlaneIter > 01174 static inline bool ray_intersect_halfspaces( const CartVect& ray_pt, const CartVect& ray_dir, const PlaneIter& begin, 01175 const PlaneIter& end, double& t_enter, double& t_exit ) 01176 { 01177 const double epsilon = 1e-12; 01178 01179 // begin with inifinite ray 01180 t_enter = 0.0; 01181 t_exit = std::numeric_limits< double >::infinity(); 01182 01183 // cut ray such that we keep only the portion contained 01184 // in each halfspace 01185 for( PlaneIter i = begin; i != end; ++i ) 01186 { 01187 CartVect norm( i->norm ); 01188 double coeff = i->coeff; 01189 double den = norm % ray_dir; 01190 if( fabs( den ) < epsilon ) 01191 { // ray is parallel to plane 01192 if( i->above( ray_pt.array() ) ) return false; // ray entirely outside half-space 01193 } 01194 else 01195 { 01196 double t_xsect = ( -coeff - ( norm % ray_pt ) ) / den; 01197 // keep portion of ray/segment below plane 01198 if( den > 0 ) 01199 { 01200 if( t_xsect < t_exit ) t_exit = t_xsect; 01201 } 01202 else 01203 { 01204 if( t_xsect > t_enter ) t_enter = t_xsect; 01205 } 01206 } 01207 } 01208 01209 return t_exit >= t_enter; 01210 } 01211 01212 class BoxPlaneIter 01213 { 01214 int faceNum; 01215 BSPTree::Plane facePlanes[6]; 01216 01217 public: 01218 BoxPlaneIter( const double coords[8][3] ); 01219 BoxPlaneIter() : faceNum( 6 ) {} // initialize to 'end' 01220 const BSPTree::Plane* operator->() const 01221 { 01222 return facePlanes + faceNum; 01223 } 01224 bool operator==( const BoxPlaneIter& other ) const 01225 { 01226 return faceNum == other.faceNum; 01227 } 01228 bool operator!=( const BoxPlaneIter& other ) const 01229 { 01230 return faceNum != other.faceNum; 01231 } 01232 BoxPlaneIter& operator++() 01233 { 01234 ++faceNum; 01235 return *this; 01236 } 01237 }; 01238 01239 static const int box_face_corners[6][4] = { { 0, 1, 5, 4 }, { 1, 2, 6, 5 }, { 2, 3, 7, 6 }, 01240 { 3, 0, 4, 7 }, { 3, 2, 1, 0 }, { 4, 5, 6, 7 } }; 01241 01242 BoxPlaneIter::BoxPlaneIter( const double coords[8][3] ) : faceNum( 0 ) 01243 { 01244 // NOTE: In the case of a BSP tree, all sides of the 01245 // leaf will planar. 01246 assert( sizeof( CartVect ) == sizeof( coords[0] ) ); 01247 const CartVect* corners = reinterpret_cast< const CartVect* >( coords ); 01248 for( int i = 0; i < 6; ++i ) 01249 { 01250 const int* indices = box_face_corners[i]; 01251 CartVect v1 = corners[indices[1]] - corners[indices[0]]; 01252 CartVect v2 = corners[indices[3]] - corners[indices[0]]; 01253 CartVect n = v1 * v2; 01254 facePlanes[i] = BSPTree::Plane( n.array(), -( n % corners[indices[2]] ) ); 01255 } 01256 } 01257 01258 bool BSPTreeBoxIter::intersect_ray( const double ray_point[3], const double ray_vect[3], double& t_enter, 01259 double& t_exit ) const 01260 { 01261 BoxPlaneIter iter( this->leafCoords ), end; 01262 return ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter, end, t_enter, t_exit ); 01263 } 01264 01265 class BSPTreePlaneIter 01266 { 01267 BSPTree* toolPtr; 01268 const EntityHandle* const pathToRoot; 01269 int pathPos; 01270 BSPTree::Plane tmpPlane; 01271 std::vector< EntityHandle > tmpChildren; 01272 01273 public: 01274 BSPTreePlaneIter( BSPTree* tool, const EntityHandle* path, int path_len ) 01275 : toolPtr( tool ), pathToRoot( path ), pathPos( path_len - 1 ) 01276 { 01277 operator++(); 01278 } 01279 BSPTreePlaneIter() // initialize to 'end' 01280 : toolPtr( 0 ), pathToRoot( 0 ), pathPos( -1 ) 01281 { 01282 } 01283 01284 const BSPTree::Plane* operator->() const 01285 { 01286 return &tmpPlane; 01287 } 01288 bool operator==( const BSPTreePlaneIter& other ) const 01289 { 01290 return pathPos == other.pathPos; 01291 } 01292 bool operator!=( const BSPTreePlaneIter& other ) const 01293 { 01294 return pathPos != other.pathPos; 01295 } 01296 BSPTreePlaneIter& operator++(); 01297 }; 01298 01299 BSPTreePlaneIter& BSPTreePlaneIter::operator++() 01300 { 01301 if( --pathPos < 0 ) return *this; 01302 01303 EntityHandle prev = pathToRoot[pathPos + 1]; 01304 EntityHandle curr = pathToRoot[pathPos]; 01305 01306 ErrorCode rval = toolPtr->get_split_plane( curr, tmpPlane ); 01307 if( MB_SUCCESS != rval ) 01308 { 01309 assert( false ); 01310 pathPos = 0; 01311 return *this; 01312 } 01313 01314 tmpChildren.clear(); 01315 rval = toolPtr->moab()->get_child_meshsets( curr, tmpChildren ); 01316 if( MB_SUCCESS != rval || tmpChildren.size() != 2 ) 01317 { 01318 assert( false ); 01319 pathPos = 0; 01320 return *this; 01321 } 01322 01323 if( tmpChildren[1] == prev ) tmpPlane.flip(); 01324 return *this; 01325 } 01326 01327 bool BSPTreeIter::intersect_ray( const double ray_point[3], const double ray_vect[3], double& t_enter, 01328 double& t_exit ) const 01329 { 01330 // intersect with half-spaces defining tree 01331 BSPTreePlaneIter iter1( tool(), &mStack[0], mStack.size() ), end1; 01332 if( !ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter1, end1, t_enter, t_exit ) ) 01333 return false; 01334 01335 // itersect with box bounding entire tree 01336 double corners[8][3]; 01337 ErrorCode rval = tool()->get_tree_box( mStack.front(), corners ); 01338 if( MB_SUCCESS != rval ) 01339 { 01340 assert( false ); 01341 return false; 01342 } 01343 01344 BoxPlaneIter iter2( corners ), end2; 01345 double t2_enter, t2_exit; 01346 if( !ray_intersect_halfspaces( CartVect( ray_point ), CartVect( ray_vect ), iter2, end2, t2_enter, t2_exit ) ) 01347 return false; 01348 01349 // if intersect both box and halfspaces, check that 01350 // two intersections overlap 01351 if( t_enter < t2_enter ) t_enter = t2_enter; 01352 if( t_exit > t2_exit ) t_exit = t2_exit; 01353 return t_enter <= t_exit; 01354 } 01355 01356 } // namespace moab