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