root/MOAB/trunk/MBAdaptiveKDTree.cpp @ 1527

Revision 1527, 59.9 KB (checked in by kraftche, 23 months ago)

Add a few features to MBAdaptiveKDTree:

o Make MBAdaptiveKDTreeIter bidirectional.
o Add function to get iterator for leaf containing a point.
  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/*
2 * MOAB, a Mesh-Oriented datABase, is a software component for creating,
3 * storing and accessing finite element mesh data.
4 *
5 * Copyright 2004 Sandia Corporation.  Under the terms of Contract
6 * DE-AC04-94AL85000 with Sandia Coroporation, the U.S. Government
7 * retains certain rights in this software.
8 *
9 * This library is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 2.1 of the License, or (at your option) any later version.
13 *
14 */
15
16/**\file MBAdaptiveKDTree.cpp
17 *\author Jason Kraftcheck (kraftche@cae.wisc.edu)
18 *\date 2007-04-1
19 */
20
21#include "MBAdaptiveKDTree.hpp"
22#include "MBInterface.hpp"
23#include "MBGeomUtil.hpp"
24#include "MBRange.hpp"
25#include "MBInternals.hpp"
26
27#include <assert.h>
28#include <algorithm>
29#include <limits>
30
31#ifdef _MSC_VER
32#  include <float.h>
33#  define finite(A) _finite(A)
34#endif
35
36MBAdaptiveKDTree::Settings::Settings()
37  : maxEntPerLeaf(6), 
38    maxTreeDepth(30),
39    candidateSplitsPerDir(5),
40    candidatePlaneSet(SUBDIVISION_SNAP),
41    minBoxWidth( std::numeric_limits<double>::epsilon() )
42  {}
43
44
45#define MB_AD_KD_TREE_DEFAULT_TAG_NAME "AKDTree"
46
47// If defined, use single tag for both axis and location of split plane
48#define MB_AD_KD_TREE_USE_SINGLE_TAG
49
50// No effect if MB_AD_KD_TREE_USE_SINGLE_TAG is not defined.
51// If defined, store plane axis as double so tag has consistent
52// type (doubles for both location and axis).  If not defined,
53// store struct Plane as opaque.
54#define MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG
55
56#if defined(MB_AD_KD_TREE_USE_SINGLE_TAG) && defined(HDF5_FILE)
57# include <H5Tpublic.h>
58#endif
59
60#define MAKE_TAG( NAME, STORAGE, TYPE, COUNT, HANDLE, DEFAULT ) \
61  if (MB_SUCCESS != make_tag( moab(), \
62                              (NAME), \
63                              (STORAGE), \
64                              (TYPE), \
65                              (COUNT), \
66                              (DEFAULT), \
67                              (HANDLE), \
68                              ctl )) { \
69    planeTag = axisTag = rootTag = (MBTag)-1; \
70    return; \
71  }
72
73static MBErrorCode make_tag( MBInterface* iface,
74                             std::string name,
75                             MBTagType storage, 
76                             MBDataType type,
77                             int count,
78                             void* default_val,
79                             MBTag& tag_handle,
80                             std::vector<MBTag>& created_tags )
81{
82  int size;
83  switch (type) {
84    case MB_TYPE_DOUBLE:  size = sizeof(double);         break;
85    case MB_TYPE_INTEGER: size = sizeof(int);            break;
86    case MB_TYPE_OPAQUE:  size = 1;                      break;
87    case MB_TYPE_HANDLE:  size = sizeof(MBEntityHandle); break;
88    default: return MB_FAILURE;
89  }
90  size *= count;
91 
92  MBErrorCode rval = iface->tag_create( name.c_str(),
93                                        size,
94                                        storage,
95                                        type,
96                                        tag_handle,
97                                        default_val,
98                                        false );
99  if (MB_SUCCESS == rval) 
100    created_tags.push_back( tag_handle );
101  else if (MB_ALREADY_ALLOCATED == rval)
102    rval = iface->tag_create( name.c_str(),
103                              size,
104                              MB_TAG_DENSE,
105                              type,
106                              tag_handle,
107                              default_val,
108                              true );
109
110  if (MB_SUCCESS != rval)
111    while( !created_tags.empty() ) {
112      iface->tag_delete( created_tags.back() );
113      created_tags.pop_back();
114    }
115 
116  return rval;
117}
118
119MBAdaptiveKDTree::MBAdaptiveKDTree( MBInterface* mb, const char* tagname_in, unsigned set_flags )
120  : mbInstance(mb), meshSetFlags(set_flags)
121{
122  const char* tagname = tagname_in ? tagname_in : MB_AD_KD_TREE_DEFAULT_TAG_NAME;
123  std::vector<MBTag> ctl;
124
125#ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
126    // create two tags, one for axis direction and one for axis coordinate
127  std::string n1(tagname), n2(tagname);
128  n1 += "_coord";
129  n2 += "_norm";
130  MAKE_TAG( n1, MB_TAG_DENSE, MB_TYPE_DOUBLE, 1, planeTag, 0 )
131  MAKE_TAG( n2, MB_TAG_DENSE, MB_TYPE_INT,    1, axisTag,  0 )
132
133#elif defined(MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG)
134    // create tag to hold two doubles, one for location and one for axis
135  MAKE_TAG( tagname, MB_TAG_DENSE, MB_TYPE_DOUBLE, 2, planeTag, 0 )
136#else
137    // create opaque tag to hold struct Plane
138  MAKE_TAG( tagname, MB_TAG_DENSE, MB_TYPE_OPAQUE, sizeof(Plane), planeTag, 0 )
139
140#ifdef HDF5_FILE 
141    // create a mesh tag holding the HDF5 type for a struct Plane
142  MBTag type_tag;
143  std::string type_tag_name = "__hdf5_tag_type_";
144  type_tag_name += tagname;
145  MAKE_TAG( type_tag_name, MB_TAG_MESH), MB_TYPE_OPAQUE, sizeof(hid_t), type_tag, 0 )
146    // create HDF5 type object describing struct Plane
147  Plane p;
148  hid_t handle = H5Tcreate( H5T_COMPOUND, sizeof(Plane) );
149  H5Tinsert( handle, "coord", &(p.coord) - &p, H5T_NATIVE_DOUBLE );
150  H5Tinsert( handle, "norm", &(p.axis) - &p, H5T_NATIVE_INT );
151  mbInstance->tag_set_data( type_tag, 0, 0, &handle );
152#endif
153#endif
154
155  std::string root_name(tagname);
156  root_name += "_box";
157  MAKE_TAG( root_name, MB_TAG_SPARSE, MB_TYPE_DOUBLE, 6, rootTag, 0 )
158}
159
160inline MBErrorCode MBAdaptiveKDTree::get_split_plane( MBEntityHandle entity,
161                                                      Plane& plane )
162{
163#ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
164  MBErrorCode r1, r2;
165  r1 = moab()->tag_get_data( planeTag, &entity, 1, &plane.coord );
166  r2 = moab()->tag_get_data( axisTag , &entity, 1, &plane.norm  );
167  return MB_SUCCESS == r1 ? r2 : r1;
168#elif defined(MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG)
169  double values[2];
170  MBErrorCode rval = moab()->tag_get_data( planeTag, &entity, 1, values );
171  plane.coord = values[0];
172  plane.norm = (int)values[1];
173  return rval;
174#else
175  return moab()->tag_get_data( planeTag, &entity, 1, &plane );
176#endif
177}
178
179inline MBErrorCode MBAdaptiveKDTree::set_split_plane( MBEntityHandle entity, 
180                                                      const Plane& plane )
181{
182#ifndef MB_AD_KD_TREE_USE_SINGLE_TAG
183  MBErrorCode r1, r2;
184  r1 = moab()->tag_set_data( planeTag, &entity, 1, &plane.coord );
185  r2 = moab()->tag_set_data( axisTag , &entity, 1, &plane.norm  );
186  return MB_SUCCESS == r1 ? r2 : r1;
187#elif defined(MB_AD_KD_TREE_USE_TWO_DOUBLE_TAG)
188  double values[2] = { plane.coord, plane.norm };
189  return moab()->tag_set_data( planeTag, &entity, 1, values );
190#else
191  return moab()->tag_set_data( planeTag, &entity, 1, &plane );
192#endif
193}
194
195
196MBErrorCode MBAdaptiveKDTree::set_tree_box( MBEntityHandle root_handle,
197                                            const double box_min[3],
198                                            const double box_max[3] )
199{
200  const double box[6] = { box_min[0], box_min[1], box_min[2],
201                          box_max[0], box_max[1], box_max[2] };
202  return moab()->tag_set_data( rootTag, &root_handle, 1, box );
203}
204
205MBErrorCode MBAdaptiveKDTree::get_tree_box( MBEntityHandle root_handle,
206                                            double box_min_out[3],
207                                            double box_max_out[3] )
208{
209  double box[6];
210  MBErrorCode rval = moab()->tag_get_data( rootTag, &root_handle, 1, box );
211  box_min_out[0] = box[0]; box_min_out[1] = box[1]; box_min_out[2] = box[2];
212  box_max_out[0] = box[3]; box_max_out[1] = box[4]; box_max_out[2] = box[5];
213  return rval;
214}
215
216
217MBErrorCode MBAdaptiveKDTree::create_tree( const double box_min[3],
218                                           const double box_max[3],
219                                           MBEntityHandle& root_handle )
220{
221  MBErrorCode rval = moab()->create_meshset( meshSetFlags, root_handle );
222  if (MB_SUCCESS != rval)
223    return rval;
224 
225  rval = set_tree_box( root_handle, box_min, box_max );
226  if (MB_SUCCESS != rval) {
227    moab()->delete_entities( &root_handle, 1 );
228    root_handle = 0;
229    return rval;
230  }
231 
232  return MB_SUCCESS;
233}
234
235MBErrorCode MBAdaptiveKDTree::delete_tree( MBEntityHandle root_handle )
236{
237  MBErrorCode rval;
238 
239  std::vector<MBEntityHandle> children, dead_sets, current_sets;
240  current_sets.push_back( root_handle );
241  while (!current_sets.empty()) {
242    MBEntityHandle set = current_sets.back();
243    current_sets.pop_back();
244    dead_sets.push_back( set );
245    rval = moab()->get_child_meshsets( set, children );
246    if (MB_SUCCESS != rval)
247      return rval;
248    std::copy( children.begin(), children.end(), std::back_inserter(current_sets) );
249    children.clear();
250  }
251 
252  rval = moab()->tag_delete_data( rootTag, &root_handle, 1 );
253  if (MB_SUCCESS != rval)
254    return rval;
255 
256  return moab()->delete_entities( &dead_sets[0], dead_sets.size() );
257}
258
259MBErrorCode MBAdaptiveKDTree::find_all_trees( MBRange& results )
260{
261  return moab()->get_entities_by_type_and_tag( 0, MBENTITYSET, 
262                                               &rootTag, 0, 1,
263                                               results );
264}
265
266MBErrorCode MBAdaptiveKDTree::get_tree_iterator( MBEntityHandle root,
267                                                 MBAdaptiveKDTreeIter& iter )
268{
269  double box[6];
270  MBErrorCode rval = moab()->tag_get_data( rootTag, &root, 1, box );
271  if (MB_SUCCESS != rval)
272    return rval;
273 
274  return get_sub_tree_iterator( root, box, box+3, iter );
275}
276
277MBErrorCode MBAdaptiveKDTree::get_last_iterator( MBEntityHandle root,
278                                                 MBAdaptiveKDTreeIter& iter )
279{
280  double box[6];
281  MBErrorCode rval = moab()->tag_get_data( rootTag, &root, 1, box );
282  if (MB_SUCCESS != rval)
283    return rval;
284 
285  return iter.initialize( this, root, box, box+3, MBAdaptiveKDTreeIter::RIGHT );
286}
287
288MBErrorCode MBAdaptiveKDTree::get_sub_tree_iterator( MBEntityHandle root,
289                                                     const double min[3], 
290                                                     const double max[3],
291                                                     MBAdaptiveKDTreeIter& result ) 
292{
293  return result.initialize( this, root, min, max, MBAdaptiveKDTreeIter::LEFT );
294}
295
296MBErrorCode MBAdaptiveKDTree::split_leaf( MBAdaptiveKDTreeIter& leaf,
297                                          Plane plane,
298                                          MBEntityHandle& left,
299                                          MBEntityHandle& right )
300{
301  MBErrorCode rval;
302 
303  rval = moab()->create_meshset( meshSetFlags, left );
304  if (MB_SUCCESS != rval)
305    return rval;
306 
307  rval = moab()->create_meshset( meshSetFlags, right );
308  if (MB_SUCCESS != rval) {
309    moab()->delete_entities( &left, 1 );
310    return rval;
311  }
312 
313  if (MB_SUCCESS != set_split_plane( leaf.handle(), plane ) ||
314      MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), left ) ||
315      MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), right) ||
316      MB_SUCCESS != leaf.step_to_first_leaf(MBAdaptiveKDTreeIter::LEFT)) {
317    MBEntityHandle children[] = { left, right };
318    moab()->delete_entities( children, 2 );
319    return MB_FAILURE;
320  }
321 
322  return MB_SUCCESS;
323}
324
325MBErrorCode MBAdaptiveKDTree::split_leaf( MBAdaptiveKDTreeIter& leaf,
326                                          Plane plane )
327{
328  MBEntityHandle left, right;
329  return split_leaf( leaf, plane, left, right );
330}
331
332MBErrorCode MBAdaptiveKDTree::split_leaf( MBAdaptiveKDTreeIter& leaf, 
333                                          Plane plane,
334                                          const MBRange& left_entities,
335                                          const MBRange& right_entities )
336{
337  MBEntityHandle left, right, parent = leaf.handle();
338  MBErrorCode rval = split_leaf( leaf, plane, left, right );
339  if (MB_SUCCESS != rval)
340    return rval;
341 
342  if (MB_SUCCESS == moab()->add_entities( left, left_entities ) &&
343      MB_SUCCESS == moab()->add_entities(right,right_entities ) &&
344      MB_SUCCESS == moab()->clear_meshset( &parent, 1 ))
345    return MB_SUCCESS;
346 
347  moab()->remove_child_meshset( parent, left );
348  moab()->remove_child_meshset( parent, right );
349  MBEntityHandle children[] = { left, right };
350  moab()->delete_entities( children, 2 );
351  return MB_FAILURE;
352}
353
354MBErrorCode MBAdaptiveKDTree::split_leaf( MBAdaptiveKDTreeIter& leaf, 
355                                          Plane plane,
356                                          const std::vector<MBEntityHandle>& left_entities,
357                                          const std::vector<MBEntityHandle>& right_entities )
358{
359  MBEntityHandle left, right, parent = leaf.handle();
360  MBErrorCode rval = split_leaf( leaf, plane, left, right );
361  if (MB_SUCCESS != rval)
362    return rval;
363 
364  if (MB_SUCCESS == moab()->add_entities( left, &left_entities[0], left_entities.size() ) &&
365      MB_SUCCESS == moab()->add_entities(right,&right_entities[0],right_entities.size() ) &&
366      MB_SUCCESS == moab()->clear_meshset( &parent, 1 ))
367    return MB_SUCCESS;
368 
369  moab()->remove_child_meshset( parent, left );
370  moab()->remove_child_meshset( parent, right );
371  MBEntityHandle children[] = { left, right };
372  moab()->delete_entities( children, 2 );
373  return MB_FAILURE;
374}
375
376MBErrorCode MBAdaptiveKDTree::merge_leaf( MBAdaptiveKDTreeIter& iter )
377{
378  MBErrorCode rval;
379  if (iter.depth() == 1) // at root
380    return MB_FAILURE;
381 
382    // Move iter to parent
383 
384  MBAdaptiveKDTreeIter::StackObj node = iter.mStack.back();
385  iter.mStack.pop_back();
386 
387  iter.childVect.clear();
388  rval = moab()->get_child_meshsets( iter.mStack.back().entity, iter.childVect );
389  if (MB_SUCCESS != rval)
390    return rval;
391  Plane plane;
392  rval = get_split_plane( iter.mStack.back().entity, plane );
393  if (MB_SUCCESS != rval)
394    return rval;
395 
396  int child_idx = iter.childVect[0] == node.entity ? 0 : 1;
397  assert(iter.childVect[child_idx] == node.entity);
398  iter.mBox[1-child_idx][plane.norm] = node.coord;
399 
400
401    // Get all entities from children and put them in parent
402  MBEntityHandle parent = iter.handle();
403  moab()->remove_child_meshset( parent, iter.childVect[0] );
404  moab()->remove_child_meshset( parent, iter.childVect[1] );
405  std::vector<MBEntityHandle> stack( iter.childVect );
406 
407  MBRange range;
408  while (!stack.empty()) {
409    MBEntityHandle h = stack.back();
410    stack.pop_back();
411    range.clear();
412    rval = moab()->get_entities_by_handle( h, range );
413    if (MB_SUCCESS != rval)
414      return rval;
415    rval = moab()->add_entities( parent, range );
416    if (MB_SUCCESS != rval)
417      return rval;
418   
419    iter.childVect.clear();
420    moab()->get_child_meshsets( h, iter.childVect );
421    if (!iter.childVect.empty()) {
422     moab()->remove_child_meshset( h, iter.childVect[0] );
423     moab()->remove_child_meshset( h, iter.childVect[1] );
424     stack.push_back( iter.childVect[0] );
425     stack.push_back( iter.childVect[1] );
426    }
427 
428    rval = moab()->delete_entities( &h, 1 );
429    if (MB_SUCCESS != rval)
430      return rval;
431  }
432 
433  return MB_SUCCESS;
434}
435
436 
437
438MBErrorCode MBAdaptiveKDTreeIter::initialize( MBAdaptiveKDTree* tool,
439                                              MBEntityHandle root,
440                                              const double box_min[3],
441                                              const double box_max[3],
442                                              Direction direction )
443{
444  mStack.clear();
445  treeTool = tool;
446  mBox[BMIN][0] = box_min[0];
447  mBox[BMIN][1] = box_min[1];
448  mBox[BMIN][2] = box_min[2];
449  mBox[BMAX][0] = box_max[0];
450  mBox[BMAX][1] = box_max[1];
451  mBox[BMAX][2] = box_max[2];
452  mStack.push_back( StackObj(root,0) );
453  return step_to_first_leaf( direction );
454}
455
456MBErrorCode MBAdaptiveKDTreeIter::step_to_first_leaf( Direction direction )
457{
458  MBErrorCode rval;
459  MBAdaptiveKDTree::Plane plane;
460  const Direction opposite = static_cast<Direction>(1-direction);
461 
462  for (;;) {
463    childVect.clear();
464    rval = treeTool->moab()->get_child_meshsets( mStack.back().entity, childVect );
465    if (MB_SUCCESS != rval)
466      return rval;
467    if (childVect.empty()) // leaf
468      break;
469 
470    rval = treeTool->get_split_plane( mStack.back().entity, plane );
471    if (MB_SUCCESS != rval)
472      return rval;
473 
474    mStack.push_back( StackObj(childVect[direction],mBox[opposite][plane.norm]) );
475    mBox[opposite][plane.norm] = plane.coord;
476  }
477  return MB_SUCCESS;
478}
479
480MBErrorCode MBAdaptiveKDTreeIter::step( Direction direction )
481{
482  StackObj node, parent;
483  MBErrorCode rval;
484  MBAdaptiveKDTree::Plane plane;
485  const Direction opposite = static_cast<Direction>(1-direction);
486 
487    // If stack is empty, then either this iterator is uninitialized
488    // or we reached the end of the iteration (and return
489    // MB_ENTITY_NOT_FOUND) already.
490  if (mStack.empty())
491    return MB_FAILURE;
492   
493    // Pop the current node from the stack.
494    // The stack should then contain the parent of the current node.
495    // If the stack is empty after this pop, then we've reached the end.
496  node = mStack.back();
497  mStack.pop_back();
498 
499  while(!mStack.empty()) {
500      // Get data for parent entity
501    parent = mStack.back();
502    childVect.clear();
503    rval = treeTool->moab()->get_child_meshsets( parent.entity, childVect );
504    if (MB_SUCCESS != rval)
505      return rval;
506    rval = treeTool->get_split_plane( parent.entity, plane );
507    if (MB_SUCCESS != rval)
508      return rval;
509   
510      // If we're at the left child
511    if (childVect[opposite] == node.entity) {
512        // change from box of left child to box of parent
513      mBox[direction][plane.norm] = node.coord;
514        // push right child on stack
515      node.entity = childVect[direction];
516      node.coord = mBox[opposite][plane.norm];
517      mStack.push_back( node );
518        // change from box of parent to box of right child
519      mBox[opposite][plane.norm] = plane.coord;
520        // descend to left-most leaf of the right child
521      return step_to_first_leaf(opposite);
522    }
523   
524      // The current node is the right child of the parent,
525      // continue up the tree.
526    assert( childVect[direction] == node.entity );
527    mBox[opposite][plane.norm] = node.coord;
528    node = parent;
529    mStack.pop_back();
530  }
531 
532  return MB_ENTITY_NOT_FOUND;
533}
534
535MBErrorCode MBAdaptiveKDTreeIter::get_neighbors( 
536                      MBAdaptiveKDTree::Axis norm, bool neg,
537                      std::vector<MBAdaptiveKDTreeIter>& results ) const
538{
539  StackObj node, parent;
540  MBErrorCode rval;
541  MBAdaptiveKDTree::Plane plane;
542  int child_idx;
543 
544    // Find tree node at which the specified side of the box
545    // for this node was created.
546  MBAdaptiveKDTreeIter iter( *this ); // temporary iterator (don't modifiy *this)
547  node = iter.mStack.back();
548  iter.mStack.pop_back();
549  for (;;) {
550      // reached the root - original node was on boundary (no neighbors)
551    if (iter.mStack.empty())
552      return MB_SUCCESS;
553   
554      // get parent node data
555    parent = iter.mStack.back();
556    iter.childVect.clear();
557    rval = treeTool->moab()->get_child_meshsets( parent.entity, iter.childVect );
558    if (MB_SUCCESS != rval)
559      return rval;
560    rval = treeTool->get_split_plane( parent.entity, plane );
561    if (MB_SUCCESS != rval)
562      return rval;
563   
564    child_idx = iter.childVect[0] == node.entity ? 0 : 1;
565    assert(iter.childVect[child_idx] == node.entity);
566   
567      // if we found the split plane for the desired side
568      // push neighbor on stack and stop
569    if (plane.norm == norm && (int)neg == child_idx) {
570        // change from box of previous child to box of parent
571      iter.mBox[1-child_idx][plane.norm] = node.coord;
572        // push other child of parent onto stack
573      node.entity = iter.childVect[1-child_idx];
574      node.coord = iter.mBox[child_idx][plane.norm];
575      iter.mStack.push_back( node );
576        // change from parent box to box of new child
577      iter.mBox[child_idx][plane.norm] = plane.coord;
578      break;
579    }
580   
581      // continue up the tree
582    iter.mBox[1-child_idx][plane.norm] = node.coord;
583    node = parent;
584    iter.mStack.pop_back();
585  }
586
587    // now move down tree, searching for adjacent boxes
588  std::vector<MBAdaptiveKDTreeIter> list;
589    // loop over all potential paths to neighbors (until list is empty)
590  for (;;) {
591      // follow a single path to a leaf, append any other potential
592      // paths to neighbors to 'list'
593    node = iter.mStack.back();
594    for (;;) { 
595      iter.childVect.clear();
596      rval = treeTool->moab()->get_child_meshsets( node.entity, iter.childVect );
597      if (MB_SUCCESS != rval)
598        return rval;
599       
600        // if leaf
601      if (iter.childVect.empty()) {
602        results.push_back( iter );
603        break; 
604      }
605     
606      rval = treeTool->get_split_plane( node.entity, plane );
607      if (MB_SUCCESS != rval)
608        return rval;
609     
610        // if split parallel to side
611      if (plane.norm == norm) {
612          // continue with whichever child is on the correct side of the split
613        node.entity = iter.childVect[neg];
614        node.coord = iter.mBox[1-neg][plane.norm];
615        iter.mStack.push_back( node );
616        iter.mBox[1-neg][plane.norm] = plane.coord;
617      }
618        // if left child is adjacent
619      else if (this->mBox[BMIN][plane.norm] < plane.coord) {
620          // if right child is also adjacent, add to list
621        if (this->mBox[BMAX][plane.norm] > plane.coord) {
622          list.push_back( iter );
623          list.back().mStack.push_back( StackObj( iter.childVect[1], iter.mBox[BMIN][plane.norm] ) );
624          list.back().mBox[BMIN][plane.norm] = plane.coord;
625        }
626          // continue with left child
627        node.entity = iter.childVect[0];
628        node.coord = iter.mBox[BMAX][plane.norm];
629        iter.mStack.push_back( node );
630        iter.mBox[BMAX][plane.norm] = plane.coord;
631      }
632        // right child is adjacent
633      else {
634          // if left child is not adjacent, right must be or something
635          // is really messed up.
636        assert(this->mBox[BMAX][plane.norm] > plane.coord);
637           // continue with left child
638        node.entity = iter.childVect[1];
639        node.coord = iter.mBox[BMIN][plane.norm];
640        iter.mStack.push_back( node );
641        iter.mBox[BMIN][plane.norm] = plane.coord;
642      }
643    }
644   
645    if (list.empty())
646      break;
647   
648    iter = list.back();
649    list.pop_back();
650  }
651 
652  return MB_SUCCESS;
653}
654
655MBErrorCode MBAdaptiveKDTreeIter::sibling_side( 
656                            MBAdaptiveKDTree::Axis& axis_out,
657                            bool& neg_out ) const
658{
659  if (mStack.size() < 2) // at tree root
660    return MB_ENTITY_NOT_FOUND;
661 
662  MBEntityHandle parent = mStack[mStack.size()-2].entity;
663  MBAdaptiveKDTree::Plane plane;
664  MBErrorCode rval = tool()->get_split_plane( parent, plane );
665  if (MB_SUCCESS != rval)
666    return MB_FAILURE;
667   
668  childVect.clear();
669  rval = tool()->moab()->get_child_meshsets( parent, childVect );
670  if (MB_SUCCESS != rval || childVect.size() != 2)
671    return MB_FAILURE;
672 
673  axis_out = static_cast<MBAdaptiveKDTree::Axis>(plane.norm);
674  neg_out = (childVect[1] == handle());
675  assert(childVect[neg_out] == handle());
676  return MB_SUCCESS;
677}
678
679
680static MBErrorCode intersect_children_with_elems( MBInterface* moab,
681                                        const MBRange& elems,
682                                        MBAdaptiveKDTree::Plane plane,
683                                        MBCartVect box_min,
684                                        MBCartVect box_max,
685                                        MBRange& left_tris,
686                                        MBRange& right_tris,
687                                        MBRange& both_tris,
688                                        double& metric_value )
689{
690  left_tris.clear();
691  right_tris.clear();
692  both_tris.clear();
693  MBCartVect coords[8];
694 
695    // get extents of boxes for left and right sides
696  MBCartVect right_min( box_min ), left_max( box_max );
697  right_min[plane.norm] = left_max[plane.norm] = plane.coord;
698  right_min *= 0.5;
699  left_max *= 0.5;
700  box_min *= 0.5;
701  box_max *= 0.5;
702  const MBCartVect left_cen = left_max + box_min;
703  const MBCartVect left_dim = left_max - box_min;
704  const MBCartVect right_cen = box_max + right_min;
705  const MBCartVect right_dim = box_max - right_min;
706 
707 
708    // test each triangle
709  MBErrorCode rval;
710  int count;
711  const MBEntityHandle* conn;
712  for (MBRange::reverse_iterator i = elems.rbegin(); i != elems.rend(); ++i) {
713    rval = moab->get_connectivity( *i, conn, count, true );
714    if (MB_SUCCESS != rval) return rval;
715    if (count > (int)(sizeof(coords)/sizeof(coords[0])))
716      return MB_FAILURE;
717    rval = moab->get_coords( &conn[0], count, coords[0].array() );
718    if (MB_SUCCESS != rval) return rval;
719   
720    bool lo = MBGeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE(*i), left_cen, left_dim );
721    bool ro = MBGeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE(*i),right_cen,right_dim );
722   
723      // didn't intersect either - tolerance issue
724    while (!lo && !ro) {
725        // calculate a good tolerance
726      MBCartVect dim = box_max - box_min;
727      double max_dim;
728      if (dim[0] > dim[1] && dim[1] > dim[2])
729        max_dim = dim[0];
730      else if (dim[1] > dim[2])
731        max_dim = dim[1];
732      else
733        max_dim = dim[2];
734        // loop with increasing tolerance until we intersect something
735      double tol = std::numeric_limits<double>::epsilon();
736      while (!lo && !ro) {
737        lo = MBGeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE(*i), left_cen,tol*max_dim* left_dim );
738        ro = MBGeomUtil::box_elem_overlap( coords, TYPE_FROM_HANDLE(*i),right_cen,tol*max_dim*right_dim );
739        tol *= 10.0;
740        if (tol > 1e-3)
741          return MB_FAILURE;
742      }
743    }
744    if (lo && ro)
745      both_tris.insert( *i );
746    else if (lo)
747      left_tris.insert( *i );
748    else //if (ro)
749      right_tris.insert( *i );
750  }
751 
752  MBCartVect box_dim = box_max - box_min;
753  double area_left = left_dim[0]*left_dim[1] + left_dim[1]*left_dim[2] + left_dim[2]*left_dim[0];
754  double area_right = right_dim[0]*right_dim[1] + right_dim[1]*right_dim[2] + right_dim[2]*right_dim[0];
755  double area_both = box_dim[0]*box_dim[1] + box_dim[1]*box_dim[2] + box_dim[2]*box_dim[0];
756  metric_value = (area_left * left_tris.size() + area_right * right_tris.size()) / area_both + both_tris.size();
757  return MB_SUCCESS;
758}
759
760static MBErrorCode best_subdivision_plane( int num_planes,
761                                           const MBAdaptiveKDTreeIter& iter,
762                                           MBRange& best_left,
763                                           MBRange& best_right,
764                                           MBRange& best_both,
765                                           MBAdaptiveKDTree::Plane& best_plane,
766                                           double eps )
767{
768  double metric_val = std::numeric_limits<unsigned>::max();
769 
770  MBErrorCode r;
771  const MBCartVect box_min(iter.box_min());
772  const MBCartVect box_max(iter.box_max());
773  const MBCartVect diff(box_max - box_min);
774 
775  MBRange entities;
776  r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
777  if (MB_SUCCESS != r)
778    return r;
779  const size_t p_count = entities.size();
780 
781  for (int axis = 0; axis < 3; ++axis) {
782    int plane_count = num_planes;
783    if ((num_planes+1)*eps >= diff[axis])
784      plane_count = (int)(diff[axis] / eps) - 1;
785 
786    for (int p = 1; p <= plane_count; ++p) {
787      MBAdaptiveKDTree::Plane plane = { box_min[axis] + (p/(1.0+plane_count)) * diff[axis], axis };
788      MBRange left, right, both;
789      double val;
790      r = intersect_children_with_elems( iter.tool()->moab(),
791                                         entities, plane,
792                                         box_min, box_max,
793                                         left, right, both, 
794                                         val );
795      if (MB_SUCCESS != r)
796        return r;
797      const size_t diff = p_count - both.size();
798      if (left.size() == diff || right.size() == diff)
799        continue;
800     
801      if (val >= metric_val)
802        continue;
803     
804      metric_val = val;
805      best_plane = plane;
806      best_left.swap(left);
807      best_right.swap(right);
808      best_both.swap(both);
809    }
810  }
811     
812  return MB_SUCCESS;
813}
814
815
816static MBErrorCode best_subdivision_snap_plane( int num_planes,
817                                           const MBAdaptiveKDTreeIter& iter,
818                                           MBRange& best_left,
819                                           MBRange& best_right,
820                                           MBRange& best_both,
821                                           MBAdaptiveKDTree::Plane& best_plane,
822                                           std::vector<double>& tmp_data,
823                                           double eps )
824{
825  double metric_val = std::numeric_limits<unsigned>::max();
826 
827  MBErrorCode r;
828  const MBCartVect box_min(iter.box_min());
829  const MBCartVect box_max(iter.box_max());
830  const MBCartVect diff(box_max - box_min);
831 
832  MBRange entities, vertices;
833  r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
834  if (MB_SUCCESS != r)
835    return r;
836  const size_t p_count = entities.size();
837  r = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, MBInterface::UNION );
838  if (MB_SUCCESS != r)
839    return r;
840
841  tmp_data.resize( vertices.size() );
842  for (int axis = 0; axis < 3; ++axis) {
843    int plane_count = num_planes;
844    if ((num_planes+1)*eps >= diff[axis])
845      plane_count = (int)(diff[axis] / eps) - 1;
846
847    double *ptrs[] = { 0, 0, 0 };
848    ptrs[axis] = &tmp_data[0];
849    r = iter.tool()->moab()->get_coords( vertices, ptrs[0], ptrs[1], ptrs[2] );
850    if (MB_SUCCESS != r)
851      return r;
852 
853    for (int p = 1; p <= plane_count; ++p) {
854      double coord = box_min[axis] + (p/(1.0+plane_count)) * diff[axis];
855      double closest_coord = tmp_data[0];
856      for (unsigned i = 1; i < tmp_data.size(); ++i) 
857        if (fabs(coord-tmp_data[i]) < fabs(coord-closest_coord))
858          closest_coord = tmp_data[i];
859      if (closest_coord <= box_min[axis] || closest_coord >= box_max[axis])
860        continue;
861      MBAdaptiveKDTree::Plane plane = { closest_coord, axis };
862      MBRange left, right, both;
863      double val;
864      r = intersect_children_with_elems( iter.tool()->moab(),
865                                         entities, plane,
866                                         box_min, box_max,
867                                         left, right, both, 
868                                         val );
869      if (MB_SUCCESS != r)
870        return r;
871      const size_t diff = p_count - both.size();
872      if (left.size() == diff || right.size() == diff)
873        continue;
874     
875      if (val >= metric_val)
876        continue;
877     
878      metric_val = val;
879      best_plane = plane;
880      best_left.swap(left);
881      best_right.swap(right);
882      best_both.swap(both);
883    }
884  }
885     
886  return MB_SUCCESS;
887}
888
889static MBErrorCode best_vertex_median_plane( int num_planes,
890                                           const MBAdaptiveKDTreeIter& iter,
891                                           MBRange& best_left,
892                                           MBRange& best_right,
893                                           MBRange& best_both,
894                                           MBAdaptiveKDTree::Plane& best_plane,
895                                           std::vector<double>& coords,
896                                           double eps)
897{
898  double metric_val = std::numeric_limits<unsigned>::max();
899 
900  MBErrorCode r;
901  const MBCartVect box_min(iter.box_min());
902  const MBCartVect box_max(iter.box_max());
903 
904  MBRange entities, vertices;
905  r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
906  if (MB_SUCCESS != r)
907    return r;
908  const size_t p_count = entities.size();
909  r = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, MBInterface::UNION );
910  if (MB_SUCCESS != r)
911    return r;
912
913  coords.resize( vertices.size() );
914  for (int axis = 0; axis < 3; ++axis) {
915    if (box_max[axis] - box_min[axis] <= 2*eps)
916      continue;
917 
918    double *ptrs[] = { 0, 0, 0 };
919    ptrs[axis] = &coords[0];
920    r = iter.tool()->moab()->get_coords( vertices, ptrs[0], ptrs[1], ptrs[2] );
921    if (MB_SUCCESS != r)
922      return r;
923 
924    std::sort( coords.begin(), coords.end() );
925    std::vector<double>::iterator citer;
926    citer = std::upper_bound( coords.begin(), coords.end(), box_min[axis] + eps );
927    const size_t count = std::upper_bound( citer, coords.end(), box_max[axis] - eps ) - citer;
928    size_t step;
929    if ((int)count < 2*num_planes) {
930      step = 1; num_planes = count - 1;
931    }
932    else {
933      step = count / (num_planes + 1);
934    }
935 
936    for (int p = 1; p <= num_planes; ++p) {
937     
938      citer += step;
939      MBAdaptiveKDTree::Plane plane = { *citer, axis };
940      MBRange left, right, both;
941      double val;
942      r = intersect_children_with_elems( iter.tool()->moab(),
943                                         entities, plane,
944                                         box_min, box_max,
945                                         left, right, both, 
946                                         val );
947      if (MB_SUCCESS != r)
948        return r;
949      const size_t diff = p_count - both.size();
950      if (left.size() == diff || right.size() == diff)
951        continue;
952     
953      if (val >= metric_val)
954        continue;
955     
956      metric_val = val;
957      best_plane = plane;
958      best_left.swap(left);
959      best_right.swap(right);
960      best_both.swap(both);
961    }
962  }
963     
964  return MB_SUCCESS;
965}
966
967
968static MBErrorCode best_vertex_sample_plane( int num_planes,
969                                           const MBAdaptiveKDTreeIter& iter,
970                                           MBRange& best_left,
971                                           MBRange& best_right,
972                                           MBRange& best_both,
973                                           MBAdaptiveKDTree::Plane& best_plane,
974                                           std::vector<double>& coords,
975                                           std::vector<size_t>& indices,
976                                           double eps )
977{
978  double metric_val = std::numeric_limits<unsigned>::max();
979 
980  MBErrorCode r;
981  const MBCartVect box_min(iter.box_min());
982  const MBCartVect box_max(iter.box_max());
983 
984  MBRange entities, vertices;
985  r = iter.tool()->moab()->get_entities_by_handle( iter.handle(), entities );
986  if (MB_SUCCESS != r)
987    return r;
988  const size_t p_count = entities.size();
989  r = iter.tool()->moab()->get_adjacencies( entities, 0, false, vertices, MBInterface::UNION );
990  if (MB_SUCCESS != r)
991    return r;
992
993  coords.resize( vertices.size() );
994  for (int axis = 0; axis < 3; ++axis) {
995    if (box_max[axis] - box_min[axis] <= 2*eps)
996      continue;
997 
998    double *ptrs[] = { 0, 0, 0 };
999    ptrs[axis] = &coords[0];
1000    r = iter.tool()->moab()->get_coords( vertices, ptrs[0], ptrs[1], ptrs[2] );
1001    if (MB_SUCCESS != r)
1002      return r;
1003     
1004    size_t num_valid_coords = 0;
1005    for (size_t i = 0; i < coords.size(); ++i) 
1006      if (coords[i] > box_min[axis]+eps && coords[i] < box_max[axis]-eps)
1007        ++num_valid_coords;
1008     
1009    if (2*(size_t)num_planes > num_valid_coords) {
1010      indices.clear();
1011      for (size_t i = 0; i < coords.size(); ++i) 
1012        if (coords[i] > box_min[axis]+eps && coords[i] < box_max[axis]-eps)
1013          indices.push_back( i );
1014    }
1015    else {
1016      indices.resize( num_planes );
1017        // make sure random indices are sufficient to cover entire range
1018      const int num_rand = coords.size() / RAND_MAX + 1;
1019      for (int j = 0; j < num_planes; ++j)
1020      {
1021        size_t rnd;
1022        do { 
1023          size_t rnd = rand();
1024          for (int i = num_rand; i > 1; --i)
1025            rnd *= rand();
1026          rnd %= coords.size();
1027        } while (coords[rnd] <= box_min[axis]+eps || coords[rnd] >= box_max[axis]-eps);
1028        indices[j] = rnd;
1029      }
1030    }
1031 
1032    for (unsigned p = 0; p <= indices.size(); ++p) {
1033     
1034      MBAdaptiveKDTree::Plane plane = { coords[indices[p]], axis };
1035      MBRange left, right, both;
1036      double val;
1037      r = intersect_children_with_elems( iter.tool()->moab(),
1038                                         entities, plane,
1039                                         box_min, box_max,
1040                                         left, right, both, 
1041                                         val );
1042      if (MB_SUCCESS != r)
1043        return r;
1044      const size_t diff = p_count - both.size();
1045      if (left.size() == diff || right.size() == diff)
1046        continue;
1047     
1048      if (val >= metric_val)
1049        continue;
1050     
1051      metric_val = val;
1052      best_plane = plane;
1053      best_left.swap(left);
1054      best_right.swap(right);
1055      best_both.swap(both);
1056    }
1057  }
1058     
1059  return MB_SUCCESS;
1060}
1061
1062MBErrorCode MBAdaptiveKDTree::build_tree( MBRange& elems,
1063                                       MBEntityHandle& root_set_out,
1064                                       const Settings* settings_ptr )
1065{
1066  Settings settings;
1067  if (settings_ptr)
1068    settings = *settings_ptr;
1069  if (settings.maxEntPerLeaf < 1)
1070    settings.maxEntPerLeaf = 1;
1071  if (settings.maxTreeDepth < 1)
1072    settings.maxTreeDepth = std::numeric_limits<unsigned>::max();
1073  if (settings.candidateSplitsPerDir < 1)
1074    settings.candidateSplitsPerDir = 1;
1075 
1076    // calculate bounding box of elements
1077   
1078  std::vector<double> tmp_data;
1079  std::vector<size_t> tmp_data2;
1080  MBRange vertices;
1081  MBErrorCode rval = moab()->get_adjacencies( elems, 0, false, vertices, MBInterface::UNION );
1082  if (MB_SUCCESS != rval)
1083    return rval;
1084 
1085  MBCartVect bmin(HUGE_VAL), bmax(-HUGE_VAL), coords;
1086  for (MBRange::iterator i = vertices.begin(); i != vertices.end(); ++i) {
1087    rval = moab()->get_coords( &*i, 1, coords.array() );
1088    if (MB_SUCCESS != rval)
1089      return rval;
1090    for (unsigned j = 0; j < 3; ++j) {
1091      if (coords[j] < bmin[j])
1092        bmin[j] = coords[j];
1093      if (coords[j] > bmax[j])
1094        bmax[j] = coords[j];
1095    }
1096  }
1097 
1098    // create tree root
1099  rval = create_tree( bmin.array(), bmax.array(), root_set_out );
1100  if (MB_SUCCESS != rval)
1101    return rval;
1102  rval = moab()->add_entities( root_set_out, elems );
1103  if (MB_SUCCESS != rval)
1104    return rval;
1105 
1106  MBAdaptiveKDTreeIter iter;
1107  iter.initialize( this, root_set_out, bmin.array(), bmax.array(), MBAdaptiveKDTreeIter::LEFT );
1108 
1109  for (;;) {
1110 
1111    int pcount;
1112    rval = moab()->get_number_entities_by_handle( iter.handle(), pcount );
1113    if (MB_SUCCESS != rval)
1114      break;
1115
1116    const size_t p_count = pcount;
1117    MBRange best_left, best_right, best_both;
1118    Plane best_plane = { HUGE_VAL, -1 };
1119    if (p_count > settings.maxEntPerLeaf && iter.depth() < settings.maxTreeDepth) {
1120      switch (settings.candidatePlaneSet) {
1121        case MBAdaptiveKDTree::SUBDIVISION:
1122          rval = best_subdivision_plane( settings.candidateSplitsPerDir, 
1123                                               iter, 
1124                                               best_left, 
1125                                               best_right, 
1126                                               best_both, 
1127                                               best_plane, 
1128                                               settings.minBoxWidth );
1129          break;
1130        case MBAdaptiveKDTree::SUBDIVISION_SNAP:
1131          rval = best_subdivision_snap_plane( settings.candidateSplitsPerDir, 
1132                                               iter, 
1133                                               best_left, 
1134                                               best_right, 
1135                                               best_both, 
1136                                               best_plane, 
1137                                               tmp_data, 
1138                                               settings.minBoxWidth );
1139          break;
1140        case MBAdaptiveKDTree::VERTEX_MEDIAN:
1141          rval = best_vertex_median_plane( settings.candidateSplitsPerDir, 
1142                                               iter, 
1143                                               best_left, 
1144                                               best_right, 
1145                                               best_both, 
1146                                               best_plane, 
1147                                               tmp_data, 
1148                                               settings.minBoxWidth );
1149          break;
1150        case MBAdaptiveKDTree::VERTEX_SAMPLE:
1151          rval = best_vertex_sample_plane( settings.candidateSplitsPerDir, 
1152                                               iter, 
1153                                               best_left, 
1154                                               best_right, 
1155                                               best_both, 
1156                                               best_plane, 
1157                                               tmp_data, 
1158                                               tmp_data2,
1159                                               settings.minBoxWidth );
1160          break;
1161        default:
1162          rval = MB_FAILURE;
1163      }
1164   
1165      if (MB_SUCCESS != rval)
1166        return rval;
1167    }
1168   
1169    if (best_plane.norm >= 0) {
1170      best_left.merge( best_both );
1171      best_right.merge( best_both );
1172      rval = split_leaf( iter, best_plane,  best_left, best_right );
1173      if (MB_SUCCESS != rval)
1174        return rval;
1175    }
1176    else {
1177      rval = iter.step();
1178      if (MB_ENTITY_NOT_FOUND == rval) 
1179        return MB_SUCCESS;  // at end
1180      else if (MB_SUCCESS != rval)
1181        break;
1182    }
1183  }
1184 
1185  delete_tree( root_set_out );
1186  return rval;
1187}
1188
1189MBErrorCode MBAdaptiveKDTree::leaf_containing_point( MBEntityHandle tree_root,
1190                                                     const double point[3],
1191                                                     MBEntityHandle& leaf_out )
1192{
1193  std::vector<MBEntityHandle> children;
1194  Plane plane;
1195  MBEntityHandle node = tree_root;
1196  MBErrorCode rval = moab()->get_child_meshsets( node, children );
1197  if (MB_SUCCESS != rval)
1198    return rval;
1199  while (!children.empty()) {
1200    rval = get_split_plane( node, plane );
1201    if (MB_SUCCESS != rval)
1202      return rval;
1203     
1204    const double d = point[plane.norm] - plane.coord;
1205    node = children[(d > 0.0)];
1206   
1207    children.clear();
1208    rval = moab()->get_child_meshsets( node, children );
1209    if (MB_SUCCESS != rval)
1210      return rval;
1211  }
1212  leaf_out = node;
1213  return MB_SUCCESS;
1214}
1215
1216MBErrorCode MBAdaptiveKDTree::leaf_containing_point( MBEntityHandle root,
1217                                                     const double point[3],
1218                                                     MBAdaptiveKDTreeIter& result )
1219{
1220    // get bounding box of tree
1221  MBErrorCode rval = moab()->tag_get_data( rootTag, &root, 1, result.mBox );
1222  if (MB_SUCCESS != rval)
1223    return rval;
1224   
1225    // test that point is inside tree
1226  if (point[0] < result.box_min()[0] || point[0] > result.box_max()[0] ||
1227      point[1] < result.box_min()[1] || point[1] > result.box_max()[1] ||
1228      point[2] < result.box_min()[2] || point[2] > result.box_max()[2])
1229    return MB_ENTITY_NOT_FOUND; 
1230
1231    // initialize iterator at tree root
1232  result.treeTool = this;
1233  result.mStack.clear();
1234  result.mStack.push_back( MBAdaptiveKDTreeIter::StackObj(root,0) );
1235   
1236    // loop until we reach a leaf
1237  MBAdaptiveKDTree::Plane plane;
1238  for(;;) {
1239      // get children
1240    result.childVect.clear();
1241    rval = moab()->get_child_meshsets( result.handle(), result.childVect );
1242    if (MB_SUCCESS != rval)
1243      return rval;
1244     
1245      // if no children, then at leaf (done)
1246    if (result.childVect.empty())
1247      break;
1248
1249      // get split plane
1250    rval = get_split_plane( result.handle(), plane );
1251    if (MB_SUCCESS != rval) 
1252      return rval;
1253   
1254      // step iterator to appropriate child
1255      // idx: 0->left, 1->right
1256    const int idx = (point[plane.norm] > plane.coord);
1257    result.mStack.push_back( MBAdaptiveKDTreeIter::StackObj( result.childVect[idx], 
1258                                                             result.mBox[1-idx][plane.norm] ) );
1259    result.mBox[1-idx][plane.norm] = plane.coord;
1260  }
1261   
1262  return MB_SUCCESS;
1263}
1264
1265struct NodeDistance {
1266  MBEntityHandle handle;
1267  MBCartVect dist; // from_point - closest_point_on_box
1268};
1269
1270MBErrorCode MBAdaptiveKDTree::leaves_within_distance( MBEntityHandle tree_root,
1271                                                      const double from_point[3],
1272                                                      const double distance,
1273                                                      std::vector<MBEntityHandle>& result_list )
1274{
1275  const double dist_sqr = distance * distance;
1276  const MBCartVect from(from_point);
1277  std::vector<NodeDistance> list;     // list of subtrees to traverse
1278    // pre-allocate space for default max tree depth
1279  Settings tmp_settings;
1280  list.reserve( tmp_settings.maxTreeDepth );
1281
1282    // misc temporary values
1283  Plane plane;
1284  NodeDistance node; 
1285  MBErrorCode rval;
1286  std::vector<MBEntityHandle> children;
1287 
1288    // Get distance from input position to bounding box of tree
1289    // (zero if inside box)
1290  double min[3], max[3];
1291  rval = get_tree_box( tree_root, min, max );
1292    // if bounding box is not available (e.g. not starting from true root)
1293    // just start with zero.  Less efficient, but will work.
1294  node.dist = MBCartVect(0.0);
1295  if (MB_SUCCESS == rval) {
1296    for (int i = 0; i < 3; ++i) {
1297      if (from_point[i] < min[i])
1298        node.dist[i] = min[i] - from_point[i];
1299      else if (from_point[i] > max[i])
1300        node.dist[i] = from_point[i] - max[i];
1301    }
1302    if (node.dist % node.dist > dist_sqr)
1303      return MB_SUCCESS;
1304  }
1305 
1306    // begin with root in list 
1307  node.handle = tree_root;
1308  list.push_back( node );
1309 
1310  while( !list.empty() ) {
1311
1312    node = list.back();
1313    list.pop_back();
1314     
1315      // If leaf node, test contained triangles
1316    children.clear();
1317    rval = moab()->get_child_meshsets( node.handle, children );
1318    if (children.empty()) {
1319      result_list.push_back( node.handle );
1320      continue;
1321    }
1322     
1323      // If not leaf node, add children to working list
1324    rval = get_split_plane( node.handle, plane );
1325    if (MB_SUCCESS != rval)
1326      return rval;
1327   
1328    const double d = from[plane.norm] - plane.coord;
1329   
1330      // right of plane?
1331    if (d > 0) {
1332      node.handle = children[1];
1333      list.push_back( node );
1334        // if the split plane is close to the input point, add
1335        // the left child also (we'll check the exact distance
1336        /// when we pop it from the list.)
1337      if (d <= distance) {
1338        node.dist[plane.norm] = d;
1339        if (node.dist % node.dist <= dist_sqr) {
1340          node.handle = children[0];
1341          list.push_back( node );
1342        }
1343      }
1344    }
1345      // left of plane
1346    else {
1347      node.handle = children[0];
1348      list.push_back( node );
1349        // if the split plane is close to the input point, add
1350        // the right child also (we'll check the exact distance
1351        /// when we pop it from the list.)
1352      if (-d <= distance) {
1353        node.dist[plane.norm] = -d;
1354        if (node.dist % node.dist <= dist_sqr) {
1355          node.handle = children[1];
1356          list.push_back( node );
1357        }
1358      }
1359    }
1360  }
1361
1362  return MB_SUCCESS;
1363}
1364
1365/** Find the triangles in a set that are closer to the input
1366 *  position than any triangles in the 'closest_tris' list.
1367 *
1368 *  closest_tris is assumed to contain a list of triangles for
1369 *  which the first is the closest known triangle to the input
1370 *  position and the first entry in 'closest_pts' is the closest
1371 *  location on that triangle.  Any other values in the lists must
1372 *  be other triangles for which the closest point is within the
1373 *  input tolernace of the closest closest point.  This function
1374 *  will update the lists as appropriate if any closer triangles
1375 *  or triangles within the tolerance of the current closest location
1376 *  are found.  The fisrt entry is maintaned as the closest of the
1377 *  list of triangles.
1378 */
1379static MBErrorCode closest_to_triangles( MBInterface* moab,
1380                                         MBEntityHandle set_handle,
1381                                         double tolerance,
1382                                         const MBCartVect& from,
1383                                         std::vector<MBEntityHandle>& closest_tris,
1384                                         std::vector<MBCartVect>& closest_pts )
1385{
1386  MBErrorCode rval;
1387  MBRange tris;
1388  MBCartVect pos, diff, verts[3];
1389  const MBEntityHandle* conn;
1390  int len;
1391  double shortest_dist_sqr = HUGE_VAL;
1392  if (!closest_pts.empty()) {
1393    diff = from - closest_pts.front();
1394    shortest_dist_sqr = diff % diff;
1395  }
1396 
1397  rval = moab->get_entities_by_type( set_handle, MBTRI, tris );
1398  if (MB_SUCCESS != rval)
1399    return rval;
1400     
1401  for (MBRange::iterator i = tris.begin(); i != tris.end(); ++i) {
1402    rval = moab->get_connectivity( *i, conn, len );
1403    if (MB_SUCCESS != rval)
1404      return rval;
1405
1406    rval = moab->get_coords( conn, 3, verts[0].array() );
1407    if (MB_SUCCESS != rval)
1408      return rval;
1409
1410    MBGeomUtil::closest_location_on_tri( from, verts, pos );
1411    diff = pos - from;
1412    double dist_sqr = diff % diff;
1413    if (dist_sqr < shortest_dist_sqr) {
1414        // new closest location
1415      shortest_dist_sqr = dist_sqr;
1416
1417      if (closest_pts.empty()) {
1418        closest_tris.push_back( *i );
1419        closest_pts.push_back( pos );
1420      }
1421        // if have a previous closest location
1422      else {
1423          // if previous closest is more than 2*tolerance away
1424          // from new closest, then nothing in the list can
1425          // be within tolerance of new closest point.
1426        diff = pos - closest_pts.front();
1427        dist_sqr = diff % diff;
1428        if (dist_sqr > 4.0 * tolerance * tolerance) {
1429          closest_tris.clear();
1430          closest_pts.clear();
1431          closest_tris.push_back( *i );
1432          closest_pts.push_back( pos );
1433        }
1434          // otherwise need to remove any triangles that are
1435          // not within tolerance of the new closest point.
1436        else {
1437          unsigned r = 0, w = 0;
1438          for (r = 0; r < closest_pts.size(); ++r) {
1439            diff = pos - closest_pts[r];
1440            if (diff % diff <= tolerance*tolerance) {
1441              closest_pts[w] = closest_pts[r];
1442              closest_tris[w] = closest_tris[r];
1443              ++w;
1444            }
1445          }
1446          closest_pts.resize( w + 1 );
1447          closest_tris.resize( w + 1 );
1448            // always put the closest one in the front
1449          if (w > 0) {
1450            closest_pts.back() = closest_pts.front();
1451            closest_tris.back() = closest_tris.front();
1452          }
1453          closest_pts.front() = pos;
1454          closest_tris.front() = *i;
1455        }
1456      }
1457    }
1458    else {
1459        // If within tolerance of old closest triangle,
1460        // add this one to the list.
1461      diff = closest_pts.front() - pos;
1462      if (diff % diff <= tolerance*tolerance) {
1463        closest_pts.push_back( pos );
1464        closest_tris.push_back( *i );
1465      }
1466    }
1467  }
1468 
1469  return MB_SUCCESS;
1470}
1471
1472static MBErrorCode closest_to_triangles( MBInterface* moab,
1473                                         MBEntityHandle set_handle,
1474                                         const MBCartVect& from,
1475                                         double& shortest_dist_sqr,
1476                                         MBCartVect& closest_pt,
1477                                         MBEntityHandle& closest_tri )
1478{
1479  MBErrorCode rval;
1480  MBRange tris;
1481  MBCartVect pos, diff, verts[3];
1482  const MBEntityHandle* conn;
1483  int len;
1484 
1485  rval = moab->get_entities_by_type( set_handle, MBTRI, tris );
1486  if (MB_SUCCESS != rval)
1487    return rval;
1488     
1489  for (MBRange::iterator i = tris.begin(); i != tris.end(); ++i) {
1490    rval = moab->get_connectivity( *i, conn, len );
1491    if (MB_SUCCESS != rval)
1492      return rval;
1493
1494    rval = moab->get_coords( conn, 3, verts[0].array() );
1495    if (MB_SUCCESS != rval)
1496      return rval;
1497
1498    MBGeomUtil::closest_location_on_tri( from, verts, pos );
1499    diff = pos - from;
1500    double dist_sqr = diff % diff;
1501    if (dist_sqr < shortest_dist_sqr) {
1502        // new closest location
1503      shortest_dist_sqr = dist_sqr;
1504      closest_pt = pos;
1505      closest_tri = *i;
1506    }
1507  }
1508 
1509  return MB_SUCCESS;
1510}
1511
1512MBErrorCode MBAdaptiveKDTree::closest_triangle( MBEntityHandle tree_root,
1513                                 const double from_coords[3],
1514                                 double closest_point_out[3],
1515                                 MBEntityHandle& triangle_out )
1516{
1517  MBErrorCode rval;
1518  double shortest_dist_sqr = HUGE_VAL;
1519  std::vector<MBEntityHandle> leaves;
1520  const MBCartVect from(from_coords);
1521  MBCartVect closest_pt;
1522 
1523    // Find the leaf containing the input point
1524    // This search does not take into account any bounding box for the
1525    // tree, so it always returns one leaf.
1526  MBEntityHandle leaf;
1527  rval = leaf_containing_point( tree_root, from_coords, leaf );
1528  if (MB_SUCCESS != rval) return rval;
1529 
1530    // Find the closest triangle(s) in the leaf containing the point
1531  rval = closest_to_triangles( moab(), leaf, from, shortest_dist_sqr, 
1532                               closest_pt, triangle_out );
1533  if (MB_SUCCESS != rval) return rval;
1534 
1535    // Find any other leaves for which the bounding box is within
1536    // the same distance from the input point as the current closest
1537    // point is.
1538  MBCartVect diff = closest_pt - from;
1539  rval = leaves_within_distance( tree_root, from_coords, 
1540                                 sqrt(diff%diff), leaves );
1541  if (MB_SUCCESS != rval) return rval;
1542
1543    // Check any close leaves to see if they contain triangles that
1544    // are as close to or closer than the current closest triangle(s).
1545  for (unsigned i = 0; i < leaves.size(); ++i) {
1546    rval = closest_to_triangles( moab(), leaves[i], from, shortest_dist_sqr, 
1547                                 closest_pt, triangle_out );
1548    if (MB_SUCCESS != rval) return rval;
1549  }
1550 
1551    // pass back resulting position
1552  closest_pt.get( closest_point_out );
1553  return MB_SUCCESS;
1554}
1555
1556MBErrorCode MBAdaptiveKDTree::sphere_intersect_triangles( 
1557                                   MBEntityHandle tree_root,
1558                                   const double center[3],
1559                                   double radius,
1560                                   std::vector<MBEntityHandle>& triangles )
1561{
1562  MBErrorCode rval;
1563  std::vector<MBEntityHandle> leaves;
1564  const MBCartVect from(center);
1565  MBCartVect closest_pt;
1566  const MBEntityHandle* conn;
1567  MBCartVect coords[3];
1568  int conn_len;
1569
1570    // get leaves of tree that intersect sphere
1571  rval = leaves_within_distance( tree_root, center, radius, leaves );
1572  if (MB_SUCCESS != rval) return rval;
1573 
1574    // search each leaf for triangles intersecting sphere
1575  for (unsigned i = 0; i < leaves.size(); ++i) {
1576    MBRange tris;
1577    rval = moab()->get_entities_by_type( leaves[i], MBTRI, tris );
1578    if (MB_SUCCESS != rval) return rval;
1579   
1580    for (MBRange::iterator j = tris.begin(); j != tris.end(); ++j) {
1581      rval = moab()->get_connectivity( *j, conn, conn_len );
1582      if (MB_SUCCESS != rval) return rval;
1583      rval = moab()->get_coords( conn, 3, coords[0].array() );
1584      if (MB_SUCCESS != rval) return rval;
1585      MBGeomUtil::closest_location_on_tri( from, coords, closest_pt );
1586      closest_pt -= from;
1587      if ((closest_pt % closest_pt) <= (radius*radius)) 
1588        triangles.push_back( *j );
1589    }
1590  }
1591 
1592    // remove duplicates from triangle list
1593  std::sort( triangles.begin(), triangles.end() );
1594  triangles.erase( std::unique( triangles.begin(), triangles.end() ), triangles.end() );
1595  return MB_SUCCESS;
1596}
1597 
1598     
1599
1600struct NodeSeg {
1601  NodeSeg( MBEntityHandle h, double b, double e )
1602    : handle(h), beg(b), end(e) {}
1603  MBEntityHandle handle;
1604  double beg, end;
1605};
1606
1607MBErrorCode MBAdaptiveKDTree::ray_intersect_triangles( MBEntityHandle root,
1608                                                 const double tol,
1609                                                 const double ray_dir_in[3],
1610                                                 const double ray_pt_in[3],
1611                                                 std::vector<MBEntityHandle>& tris_out,
1612                                                 std::vector<double>& dists_out,
1613                                                 int max_ints,
1614                                                 double ray_end )
1615{
1616  MBErrorCode rval;
1617  double ray_beg = 0.0;
1618  if (ray_end < 0.0)
1619    ray_end = HUGE_VAL;
1620 
1621    // if root has bounding box, trim ray to that box
1622  MBCartVect bmin, bmax, tvec(tol);
1623  const MBCartVect ray_pt( ray_pt_in ), ray_dir( ray_dir_in );
1624  rval = get_tree_box( root, bmin.array(), bmax.array() );
1625  if (MB_SUCCESS == rval) {
1626    if (!MBGeomUtil::segment_box_intersect( bmin-tvec, bmax+tvec, ray_pt, ray_dir, ray_beg, ray_end ))
1627      return MB_SUCCESS; // ray misses entire tree.
1628  }
1629 
1630  MBRange tris;
1631  MBRange::iterator iter;
1632  MBCartVect tri_coords[3];
1633  const MBEntityHandle* tri_conn;
1634  int conn_len;
1635  double tri_t;
1636 
1637  Plane plane;
1638  std::vector<MBEntityHandle> children;
1639  std::vector<NodeSeg> list;
1640  NodeSeg seg(root, ray_beg, ray_end);
1641  list.push_back( seg );
1642 
1643  while (!list.empty()) {
1644    seg = list.back();
1645    list.pop_back();
1646   
1647      // If we are limited to a certain number of intersections
1648      // (max_ints != 0), then ray_end will contain the distance
1649      // to the furthest intersection we have so far.  If the
1650      // tree node is further than that, skip it.
1651    if (seg.beg > ray_end) 
1652      continue;
1653
1654      // Check if at a leaf
1655    children.clear();
1656    rval = moab()->get_child_meshsets( seg.handle, children );
1657    if (MB_SUCCESS != rval)
1658      return rval;
1659    if (children.empty()) { // leaf
1660
1661      tris.clear();
1662      rval = moab()->get_entities_by_type( seg.handle, MBTRI, tris );
1663      if (MB_SUCCESS != rval)
1664        return rval;
1665   
1666      for (iter = tris.begin(); iter != tris.end(); ++iter) {
1667        rval = moab()->get_connectivity( *iter, tri_conn, conn_len );
1668        if (MB_SUCCESS != rval) return rval;
1669        rval = moab()->get_coords( tri_conn, 3, tri_coords[0].array() );
1670        if (MB_SUCCESS != rval) return rval;
1671       
1672        if (MBGeomUtil::ray_tri_intersect( tri_coords, ray_pt, ray_dir, tol, tri_t, &ray_end )) {
1673          if (!max_ints) {
1674            if (std::find(tris_out.begin(),tris_out.end(),*iter) == tris_out.end()) {
1675              tris_out.push_back( *iter );
1676              dists_out.push_back( tri_t );
1677            }
1678          } 
1679          else if (tri_t < ray_end) {
1680            if (std::find(tris_out.begin(),tris_out.end(),*iter) == tris_out.end()) {
1681              if (tris_out.size() < (unsigned)max_ints) {
1682                tris_out.resize( tris_out.size() + 1 );
1683                dists_out.resize( dists_out.size() + 1 );
1684              }
1685              int w = tris_out.size() - 1;
1686              for (; tri_t < dists_out[w-1] && w > 0; --w) {
1687                tris_out[w] = tris_out[w-1];
1688                dists_out[w] = dists_out[w-1];
1689              }
1690              tris_out[w] = *iter;
1691              dists_out[w] = tri_t;
1692              ray_end = dists_out.back();
1693            }
1694          }
1695        }
1696      }
1697
1698      continue;
1699    }
1700   
1701    rval = get_split_plane( seg.handle, plane );
1702    if (MB_SUCCESS != rval)
1703      return rval;
1704   
1705    const double t = (plane.coord - ray_pt[plane.norm]) / ray_dir[plane.norm];
1706    if (!finite(t)) {         // ray parallel to plane
1707      if (ray_pt[plane.norm] - tol <= plane.coord)
1708        list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
1709      if (ray_pt[plane.norm] + tol >= plane.coord)
1710        list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
1711    }
1712    else if (ray_dir[plane.norm] < 0.0) {
1713      if (seg.beg > t) {      // segment left of plane
1714        list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
1715//        if (plane.coord - ray_pt[plane.norm] + ray_dir[plane.norm] * seg.beg < tol)
1716//          list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
1717      }
1718      else if (seg.end < t) { // segment right of plane
1719        list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
1720//        if (ray_pt[plane.norm] + ray_dir[plane.norm] * seg.end - plane.coord < tol)
1721//          list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
1722      }
1723      else {                  // segment crosses plane
1724        list.push_back( NodeSeg( children[1], seg.beg, t ) );
1725        list.push_back( NodeSeg( children[0], t, seg.end ) );
1726      }
1727    }
1728    else {
1729      if (seg.beg > t) {      // segment right of plane
1730        list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
1731//        if (ray_pt[plane.norm] + ray_dir[plane.norm] * seg.beg - plane.coord < tol)
1732//          list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
1733      }
1734      else if (seg.end < t) { // segment left of plane
1735        list.push_back( NodeSeg( children[0], seg.beg, seg.end ) );
1736//        if (plane.coord - ray_pt[plane.norm] + ray_dir[plane.norm] * seg.end < tol)
1737//          list.push_back( NodeSeg( children[1], seg.beg, seg.end ) );
1738      }
1739      else {                  // segment crosses plane
1740        list.push_back( NodeSeg( children[0], seg.beg, t ) );
1741        list.push_back( NodeSeg( children[1], t, seg.end ) );
1742      }
1743    }
1744  }
1745 
1746  return MB_SUCCESS;
1747}
1748         
Note: See TracBrowser for help on using the browser.