BSPTree.cpp

Go to the documentation of this file.

/*
 * MOAB, a Mesh-Oriented datABase, is a software component for creating,
 * storing and accessing finite element mesh data.
 * 
 * Copyright 2008 Sandia Corporation.  Under the terms of Contract
 * DE-AC04-94AL85000 with Sandia Coroporation, the U.S. Government
 * retains certain rights in this software.
 * 
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 */

#include "moab/BSPTree.hpp"
#include "moab/GeomUtil.hpp"
#include "moab/Range.hpp"
#include "Internals.hpp"
#include "moab/BSPTreePoly.hpp"

#include <assert.h>
#include <string.h>
#include <algorithm>
#include <limits>

#if defined(_MSC_VER) || defined(__MINGW32__)
#  include <float.h>
#  define finite(A) _finite(A)
#elif defined(HAVE_IEEEFP_H)
#  include <ieeefp.h>
#endif

#define MB_BSP_TREE_DEFAULT_TAG_NAME "BSPTree"

namespace moab {

static void corners_from_box( const double box_min[3],
                              const double box_max[3],
                              double corners[8][3] )
{
  const double* ranges[] = { box_min, box_max };
  for (int z = 0; z < 2; ++z) {
    corners[4*z  ][0] = box_min[0];
    corners[4*z  ][1] = box_min[1];
    corners[4*z  ][2] = ranges[z][2];

    corners[4*z+1][0] = box_max[0];
    corners[4*z+1][1] = box_min[1];
    corners[4*z+1][2] = ranges[z][2];

    corners[4*z+2][0] = box_max[0];
    corners[4*z+2][1] = box_max[1];
    corners[4*z+2][2] = ranges[z][2];

    corners[4*z+3][0] = box_min[0];
    corners[4*z+3][1] = box_max[1];
    corners[4*z+3][2] = ranges[z][2];
  }
}

// assume box has planar sides
// test if point is contained in box
static bool point_in_box( const double corners[8][3],
                          const double point[3] )
{
  const unsigned side_verts[6][3] = { { 0, 3, 1 },
                                      { 4, 5, 7 },
                                      { 0, 1, 4 },
                                      { 1, 2, 5 },
                                      { 2, 3, 6 },
                                      { 3, 0, 7 } };
    // If we assume planar sides, then the box is the intersection
    // of 6 half-spaces defined by the planes of the sides.
  const CartVect pt(point);
  for (unsigned s = 0; s < 6; ++s) {
    CartVect v0( corners[side_verts[s][0]] );
    CartVect v1( corners[side_verts[s][1]] );
    CartVect v2( corners[side_verts[s][2]] );
    CartVect N = (v1 - v0) * (v2 - v0);
    if ((v0 - pt) % N < 0)
      return false;
  }
  return true;
}

void BSPTree::Plane::set( const double pt1[3], const double pt2[3], const double pt3[3] )
{
  const double v1[] = { pt2[0] - pt1[0], pt2[1] - pt1[1], pt2[2] - pt1[2] };
  const double v2[] = { pt3[0] - pt1[0], pt3[1] - pt1[1], pt3[2] - pt1[2] };
  const double nrm[] = { v1[1]*v2[2] - v1[2]*v2[1],
                          v1[2]*v2[0] - v1[0]*v2[2],
                          v1[0]*v2[1] - v1[1]*v2[0] };
  set( nrm, pt1 );
}

ErrorCode BSPTree::init_tags( const char* tagname )
{
  if (!tagname) 
    tagname = MB_BSP_TREE_DEFAULT_TAG_NAME;
  
  std::string rootname(tagname);
  rootname += "_box";
  
  ErrorCode rval = moab()->tag_get_handle( tagname, 4, MB_TYPE_DOUBLE, planeTag, MB_TAG_CREAT|MB_TAG_DENSE );
  if (MB_SUCCESS != rval)
    planeTag = 0;
  else
    rval = moab()->tag_get_handle( rootname.c_str(), 24, MB_TYPE_DOUBLE, rootTag, MB_TAG_CREAT|MB_TAG_SPARSE );
  if (MB_SUCCESS != rval)
    rootTag = 0;
  return rval;
}

BSPTree::BSPTree( Interface* mb, 
                      const char* tagname, 
                      unsigned set_flags )
  : mbInstance(mb), meshSetFlags(set_flags), cleanUpTrees(false)
{ init_tags( tagname ); }

BSPTree::BSPTree( Interface* mb, 
                      bool destroy_created_trees,
                      const char* tagname, 
                      unsigned set_flags )
  : mbInstance(mb), meshSetFlags(set_flags), cleanUpTrees(destroy_created_trees)
{ init_tags( tagname ); }

BSPTree::~BSPTree()
{
  if (!cleanUpTrees)
    return;
    
  while (!createdTrees.empty()) {
    EntityHandle tree = createdTrees.back();
      // make sure this is a tree (rather than some other, stale handle)
    const void* data_ptr = 0;
    ErrorCode rval = moab()->tag_get_by_ptr( rootTag, &tree, 1, &data_ptr );
    if (MB_SUCCESS == rval)
      rval = delete_tree( tree );
    if (MB_SUCCESS != rval)
      createdTrees.pop_back();
  }
}

ErrorCode BSPTree::set_split_plane( EntityHandle node, const Plane& p )
{ 
    // check for unit-length normal
  const double lensqr = p.norm[0]*p.norm[0] 
                      + p.norm[1]*p.norm[1] 
                      + p.norm[2]*p.norm[2];
  if (fabs(lensqr - 1.0) < std::numeric_limits<double>::epsilon())
    return moab()->tag_set_data( planeTag, &node, 1, &p ); 
    
  const double inv_len = 1.0/sqrt(lensqr);
  Plane p2(p);
  p2.norm[0] *= inv_len;
  p2.norm[1] *= inv_len;
  p2.norm[2] *= inv_len;
  p2.coeff   *= inv_len;
  
    // check for zero-length normal
  if (!finite(p2.norm[0]+p2.norm[1]+p2.norm[2]+p2.coeff))
    return MB_FAILURE;

    // store plane
  return moab()->tag_set_data( planeTag, &node, 1, &p2 ); 
}

ErrorCode BSPTree::set_tree_box( EntityHandle root_handle,
                                     const double box_min[3],
                                     const double box_max[3] )
{
  double corners[8][3];
  corners_from_box( box_min, box_max, corners );
  return set_tree_box( root_handle, corners );
}

ErrorCode BSPTree::set_tree_box( EntityHandle root_handle,
                                     const double corners[8][3] )
{
  return moab()->tag_set_data( rootTag, &root_handle, 1, corners );
}

ErrorCode BSPTree::get_tree_box( EntityHandle root_handle,
                                     double corners[8][3] )
{
  return moab()->tag_get_data( rootTag, &root_handle, 1, corners );
}

ErrorCode BSPTree::get_tree_box( EntityHandle root_handle,
                                     double corners[24] )
{
  return moab()->tag_get_data( rootTag, &root_handle, 1, corners );
}

ErrorCode BSPTree::create_tree( EntityHandle& root_handle )
{
  const double min[3] = { -HUGE_VAL, -HUGE_VAL, -HUGE_VAL };
  const double max[3] = {  HUGE_VAL,  HUGE_VAL,  HUGE_VAL };
  return create_tree( min, max, root_handle );
}

ErrorCode BSPTree::create_tree( const double corners[8][3],
                                    EntityHandle& root_handle )
{
  ErrorCode rval = moab()->create_meshset( meshSetFlags, root_handle );
  if (MB_SUCCESS != rval)
    return rval;
  
  rval = set_tree_box( root_handle, corners );
  if (MB_SUCCESS != rval) {
    moab()->delete_entities( &root_handle, 1 );
    root_handle = 0;
    return rval;
  }
  
  createdTrees.push_back( root_handle );
  return MB_SUCCESS;
}
                                    

ErrorCode BSPTree::create_tree( const double box_min[3],
                                    const double box_max[3],
                                    EntityHandle& root_handle )
{
  double corners[8][3];
  corners_from_box( box_min, box_max, corners );
  return create_tree( corners, root_handle );
}

ErrorCode BSPTree::delete_tree( EntityHandle root_handle )
{
  ErrorCode rval;
  
  std::vector<EntityHandle> children, dead_sets, current_sets;
  current_sets.push_back( root_handle );
  while (!current_sets.empty()) {
    EntityHandle set = current_sets.back();
    current_sets.pop_back();
    dead_sets.push_back( set );
    rval = moab()->get_child_meshsets( set, children );
    if (MB_SUCCESS != rval)
      return rval;
    std::copy( children.begin(), children.end(), std::back_inserter(current_sets) );
    children.clear();
  }
  
  rval = moab()->tag_delete_data( rootTag, &root_handle, 1 );
  if (MB_SUCCESS != rval)
    return rval;
  
  createdTrees.erase(
    std::remove( createdTrees.begin(), createdTrees.end(), root_handle ),
    createdTrees.end() );
  return moab()->delete_entities( &dead_sets[0], dead_sets.size() );
}

ErrorCode BSPTree::find_all_trees( Range& results )
{
  return moab()->get_entities_by_type_and_tag( 0, MBENTITYSET, 
                                               &rootTag, 0, 1,
                                               results );
}

ErrorCode BSPTree::get_tree_iterator( EntityHandle root,
                                          BSPTreeIter& iter )
{
  ErrorCode rval = iter.initialize( this, root );
  if (MB_SUCCESS != rval)
    return rval;
  return iter.step_to_first_leaf( BSPTreeIter::LEFT );
}

ErrorCode BSPTree::get_tree_end_iterator( EntityHandle root,
                                          BSPTreeIter& iter )
{
  ErrorCode rval = iter.initialize( this, root );
  if (MB_SUCCESS != rval)
    return rval;
  return iter.step_to_first_leaf( BSPTreeIter::RIGHT );
}

ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf,
                                   Plane plane,
                                   EntityHandle& left,
                                   EntityHandle& right )
{
  ErrorCode rval;
  
  rval = moab()->create_meshset( meshSetFlags, left );
  if (MB_SUCCESS != rval)
    return rval;
  
  rval = moab()->create_meshset( meshSetFlags, right );
  if (MB_SUCCESS != rval) {
    moab()->delete_entities( &left, 1 );
    return rval;
  }
  
  if (MB_SUCCESS != set_split_plane( leaf.handle(), plane ) ||
      MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), left ) ||
      MB_SUCCESS != moab()->add_child_meshset( leaf.handle(), right) ||
      MB_SUCCESS != leaf.step_to_first_leaf(BSPTreeIter::LEFT)) {
    EntityHandle children[] = { left, right };
    moab()->delete_entities( children, 2 );
    return MB_FAILURE;
  }
  
  return MB_SUCCESS;
}

ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, Plane plane )
{
  EntityHandle left, right;
  return split_leaf( leaf, plane, left, right );
}

ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, 
                                   Plane plane,
                                   const Range& left_entities,
                                   const Range& right_entities )
{
  EntityHandle left, right, parent = leaf.handle();
  ErrorCode rval = split_leaf( leaf, plane, left, right );
  if (MB_SUCCESS != rval)
    return rval;
  
  if (MB_SUCCESS == moab()->add_entities( left, left_entities ) &&
      MB_SUCCESS == moab()->add_entities(right,right_entities ) &&
      MB_SUCCESS == moab()->clear_meshset( &parent, 1 ))
    return MB_SUCCESS;
  
  moab()->remove_child_meshset( parent, left );
  moab()->remove_child_meshset( parent, right );
  EntityHandle children[] = { left, right };
  moab()->delete_entities( children, 2 );
  return MB_FAILURE;
}

ErrorCode BSPTree::split_leaf( BSPTreeIter& leaf, Plane plane,
                                   const std::vector<EntityHandle>& left_entities,
                                   const std::vector<EntityHandle>& right_entities )
{
  EntityHandle left, right, parent = leaf.handle();
  ErrorCode rval = split_leaf( leaf, plane, left, right );
  if (MB_SUCCESS != rval)
    return rval;
  
  if (MB_SUCCESS == moab()->add_entities( left, &left_entities[0], left_entities.size() ) &&
      MB_SUCCESS == moab()->add_entities(right,&right_entities[0],right_entities.size() ) &&
      MB_SUCCESS == moab()->clear_meshset( &parent, 1 ))
    return MB_SUCCESS;
  
  moab()->remove_child_meshset( parent, left );
  moab()->remove_child_meshset( parent, right );
  EntityHandle children[] = { left, right };
  moab()->delete_entities( children, 2 );
  return MB_FAILURE;
}

ErrorCode BSPTree::merge_leaf( BSPTreeIter& iter )
{
  ErrorCode rval;
  if (iter.depth() == 1) // at root
    return MB_FAILURE;
  
    // Move iter to parent
  iter.up();

    // Get sets to merge
  EntityHandle parent = iter.handle();
  iter.childVect.clear();
  rval = moab()->get_child_meshsets( parent, iter.childVect );
  if (MB_SUCCESS != rval)
    return rval;
    
    // Remove child links
  moab()->remove_child_meshset( parent, iter.childVect[0] );
  moab()->remove_child_meshset( parent, iter.childVect[1] );
  std::vector<EntityHandle> stack( iter.childVect );
  
    // Get all entities from children and put them in parent
  Range range;
  while (!stack.empty()) {
    EntityHandle h = stack.back();
    stack.pop_back();
    range.clear();
    rval = moab()->get_entities_by_handle( h, range );
    if (MB_SUCCESS != rval)
      return rval;
    rval = moab()->add_entities( parent, range );
    if (MB_SUCCESS != rval)
      return rval;
    
    iter.childVect.clear();
    moab()->get_child_meshsets( h, iter.childVect );
    if (!iter.childVect.empty()) {
     moab()->remove_child_meshset( h, iter.childVect[0] );
     moab()->remove_child_meshset( h, iter.childVect[1] );
     stack.push_back( iter.childVect[0] );
     stack.push_back( iter.childVect[1] );
    }
  
    rval = moab()->delete_entities( &h, 1 );
    if (MB_SUCCESS != rval)
      return rval;
  }
  
  return MB_SUCCESS;
}

  

ErrorCode BSPTreeIter::initialize( BSPTree* btool,
                                       EntityHandle root,
                                       const double* /*point*/ )
{
  treeTool = btool;
  mStack.clear();
  mStack.push_back( root );
  return MB_SUCCESS;
}


ErrorCode BSPTreeIter::step_to_first_leaf( Direction direction )
{
  ErrorCode rval;
  for (;;) {
    childVect.clear();
    rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
    if (MB_SUCCESS != rval)
      return rval;
    if (childVect.empty()) // leaf
      break;
  
    mStack.push_back( childVect[direction] );
  }
  return MB_SUCCESS;
}

ErrorCode BSPTreeIter::step( Direction direction )
{
  EntityHandle node, parent;
  ErrorCode rval;
  const Direction opposite = static_cast<Direction>(1-direction);
  
    // If stack is empty, then either this iterator is uninitialized
    // or we reached the end of the iteration (and return 
    // MB_ENTITY_NOT_FOUND) already.
  if (mStack.empty())
    return MB_FAILURE;
    
    // Pop the current node from the stack.
    // The stack should then contain the parent of the current node.
    // If the stack is empty after this pop, then we've reached the end.
  node = mStack.back();
  mStack.pop_back();
  
  while(!mStack.empty()) {
      // Get data for parent entity
    parent = mStack.back();
    childVect.clear();
    rval = tool()->moab()->get_child_meshsets( parent, childVect );
    if (MB_SUCCESS != rval)
      return rval;
    
      // If we're at the left child
    if (childVect[opposite] == node) {
        // push right child on stack
      mStack.push_back( childVect[direction] );
        // descend to left-most leaf of the right child
      return step_to_first_leaf(opposite);
    }
    
      // The current node is the right child of the parent,
      // continue up the tree.
    assert( childVect[direction] == node );
    node = parent;
    mStack.pop_back();
  }
  
  return MB_ENTITY_NOT_FOUND;
}

ErrorCode BSPTreeIter::up() 
{
  if (mStack.size() < 2)
    return MB_ENTITY_NOT_FOUND;
  mStack.pop_back();
  return MB_SUCCESS;
}

ErrorCode BSPTreeIter::down( const BSPTree::Plane& /*plane*/, Direction dir ) 
{
  childVect.clear();
  ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
  if (MB_SUCCESS != rval)
    return rval;
  if (childVect.empty())
    return MB_ENTITY_NOT_FOUND;
  
  mStack.push_back( childVect[dir] );
  return MB_SUCCESS;
}

ErrorCode BSPTreeIter::get_parent_split_plane( BSPTree::Plane& plane ) const
{
  if (mStack.size() < 2) // at tree root
    return MB_ENTITY_NOT_FOUND;
  
  EntityHandle parent = mStack[mStack.size()-2];
  return tool()->get_split_plane( parent, plane );
}

double BSPTreeIter::volume() const
{
  BSPTreePoly polyhedron;
  ErrorCode rval = calculate_polyhedron( polyhedron );
  return MB_SUCCESS == rval ? polyhedron.volume() : -1.0;
}

bool BSPTreeIter::is_sibling( const BSPTreeIter& other_leaf ) const
{
  const size_t s = mStack.size();
  return (s > 1) && (s == other_leaf.mStack.size()) &&
         (other_leaf.mStack[s-2] == mStack[s-2]) &&
         other_leaf.handle() != handle();
}

bool BSPTreeIter::is_sibling( EntityHandle other_leaf ) const
{
  if (mStack.size() < 2 || other_leaf == handle())
    return false;
  EntityHandle parent = mStack[mStack.size()-2];
  childVect.clear();
  ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
  if (MB_SUCCESS != rval || childVect.size() != 2) {
    assert(false);
    return false;
  }
  return childVect[0] == other_leaf || childVect[1] == other_leaf;
}

bool BSPTreeIter::sibling_is_forward() const
{
  if (mStack.size() < 2) // if root
    return false;
  EntityHandle parent = mStack[mStack.size()-2];
  childVect.clear();
  ErrorCode rval = tool()->moab()->get_child_meshsets( parent, childVect );
  if (MB_SUCCESS != rval || childVect.size() != 2) {
    assert(false);
    return false;
  }
  return childVect[0] == handle();
}  

ErrorCode BSPTreeIter::calculate_polyhedron( BSPTreePoly& poly_out ) const
{
  ErrorCode rval;
  
  assert( sizeof(CartVect) == 3*sizeof(double) );
  CartVect corners[8];
  rval = treeTool->get_tree_box( mStack.front(), corners[0].array() );
  if (MB_SUCCESS != rval)
    return rval;
  
  rval = poly_out.set( corners );
  if (MB_SUCCESS != rval)
    return rval;
  
  BSPTree::Plane plane;
  std::vector<EntityHandle>::const_iterator i = mStack.begin();
  std::vector<EntityHandle>::const_iterator here = mStack.end() - 1;
  while (i != here) {
    rval = treeTool->get_split_plane( *i, plane );
    if (MB_SUCCESS != rval)
      return rval;
    
    childVect.clear();
    rval = treeTool->moab()->get_child_meshsets( *i, childVect );
    if (MB_SUCCESS != rval)
      return rval;
    if (childVect.size() != 2)
      return MB_FAILURE;
      
    ++i;
    if (childVect[1] == *i)
      plane.flip();
    
    CartVect norm( plane.norm );
    poly_out.cut_polyhedron( norm, plane.coeff );
  }
  
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::initialize( BSPTree* tool_ptr,
                                          EntityHandle root,
                                          const double* point )
{
  ErrorCode rval = BSPTreeIter::initialize( tool_ptr, root );
  if (MB_SUCCESS != rval)
    return rval;
  
  tool()->get_tree_box( root, leafCoords );
  if (MB_SUCCESS != rval)
    return rval;

  if (point && !point_in_box( leafCoords, point ))
    return MB_ENTITY_NOT_FOUND;

  stackData.resize(1);
  return MB_SUCCESS;
}

BSPTreeBoxIter::SideBits
BSPTreeBoxIter::side_above_plane( const double hex_coords[8][3],
                                    const BSPTree::Plane& plane )
{
  unsigned result  = 0;
  for (unsigned i = 0; i < 8u; ++i) 
    result |= plane.above(hex_coords[i]) << i;
  return (BSPTreeBoxIter::SideBits)result;
}

BSPTreeBoxIter::SideBits
BSPTreeBoxIter::side_on_plane( const double hex_coords[8][3],
                                 const BSPTree::Plane& plane )
{
  unsigned result  = 0;
  for (unsigned i = 0; i < 8u; ++i) {
    bool on = plane.distance(hex_coords[i]) <= BSPTree::epsilon();
    result |= on << i;
  }
  return (BSPTreeBoxIter::SideBits)result;
}

static inline void copy_coords( const double src[3], double dest[3] )
{
  dest[0] = src[0];
  dest[1] = src[1];
  dest[2] = src[2];
}

ErrorCode BSPTreeBoxIter::face_corners( const SideBits face,
                                            const double hex_corners[8][3],
                                            double face_corners[4][3] )
{
  switch (face) {
    case BSPTreeBoxIter::B0154:
      copy_coords( hex_corners[0], face_corners[0] );
      copy_coords( hex_corners[1], face_corners[1] );
      copy_coords( hex_corners[5], face_corners[2] );
      copy_coords( hex_corners[4], face_corners[3] );
      break;
    case BSPTreeBoxIter::B1265:
      copy_coords( hex_corners[1], face_corners[0] );
      copy_coords( hex_corners[2], face_corners[1] );
      copy_coords( hex_corners[6], face_corners[2] );
      copy_coords( hex_corners[5], face_corners[3] );
      break;
    case BSPTreeBoxIter::B2376:
      copy_coords( hex_corners[2], face_corners[0] );
      copy_coords( hex_corners[3], face_corners[1] );
      copy_coords( hex_corners[7], face_corners[2] );
      copy_coords( hex_corners[6], face_corners[3] );
      break;
    case BSPTreeBoxIter::B3047:
      copy_coords( hex_corners[3], face_corners[0] );
      copy_coords( hex_corners[0], face_corners[1] );
      copy_coords( hex_corners[4], face_corners[2] );
      copy_coords( hex_corners[7], face_corners[3] );
      break;
    case BSPTreeBoxIter::B3210:
      copy_coords( hex_corners[3], face_corners[0] );
      copy_coords( hex_corners[2], face_corners[1] );
      copy_coords( hex_corners[1], face_corners[2] );
      copy_coords( hex_corners[0], face_corners[3] );
      break;
    case BSPTreeBoxIter::B4567:
      copy_coords( hex_corners[4], face_corners[0] );
      copy_coords( hex_corners[5], face_corners[1] );
      copy_coords( hex_corners[6], face_corners[2] );
      copy_coords( hex_corners[7], face_corners[3] );
      break;
    default:
      return MB_FAILURE; // child is not a box
  }
  
  return MB_SUCCESS;

}

static inline
void plane_cut_edge( double old_coords_out[3],
                     const double keep_end_coords[3],
                     double cut_end_coords[3],
                     const BSPTree::Plane& plane )
{
  const CartVect start( keep_end_coords ), end( cut_end_coords );
  const CartVect norm( plane.norm );
  CartVect xsect_point;
  
  const CartVect m = end - start;
  const double t = -(norm % start + plane.coeff) / (norm % m);
  assert( t > 0.0 && t < 1.0 );
  xsect_point = start + t * m;
  
  end.get( old_coords_out );
  xsect_point.get( cut_end_coords );
}

static ErrorCode plane_cut_box( double cut_face_out[4][3],
                                  double corners_inout[8][3],
                                  const BSPTree::Plane& plane )
{
  switch (BSPTreeBoxIter::side_above_plane( corners_inout, plane )) {
    case BSPTreeBoxIter::B0154:
      plane_cut_edge( cut_face_out[0], corners_inout[3], corners_inout[0], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[2], corners_inout[1], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[6], corners_inout[5], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[7], corners_inout[4], plane );
      break;
    case BSPTreeBoxIter::B1265:
      plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[1], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[3], corners_inout[2], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[7], corners_inout[6], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[5], plane );
      break;
    case BSPTreeBoxIter::B2376:
      plane_cut_edge( cut_face_out[0], corners_inout[1], corners_inout[2], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[0], corners_inout[3], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[4], corners_inout[7], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[5], corners_inout[6], plane );
      break;
    case BSPTreeBoxIter::B3047:
      plane_cut_edge( cut_face_out[0], corners_inout[2], corners_inout[3], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[0], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[4], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[6], corners_inout[7], plane );
      break;
    case BSPTreeBoxIter::B3210:
      plane_cut_edge( cut_face_out[0], corners_inout[7], corners_inout[3], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[6], corners_inout[2], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[5], corners_inout[1], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[4], corners_inout[0], plane );
      break;
    case BSPTreeBoxIter::B4567:
      plane_cut_edge( cut_face_out[0], corners_inout[0], corners_inout[4], plane );
      plane_cut_edge( cut_face_out[1], corners_inout[1], corners_inout[5], plane );
      plane_cut_edge( cut_face_out[2], corners_inout[2], corners_inout[6], plane );
      plane_cut_edge( cut_face_out[3], corners_inout[3], corners_inout[7], plane );
      break;
    default:
      return MB_FAILURE; // child is not a box
  }
  
  return MB_SUCCESS;
}

static inline
void copy_coords( double dest[3], const double source[3] )
{
  dest[0] = source[0];
  dest[1] = source[1];
  dest[2] = source[2];
}

static inline
ErrorCode plane_uncut_box( const double cut_face_in[4][3],
                             double corners_inout[8][3],
                             const BSPTree::Plane& plane )
{
  switch (BSPTreeBoxIter::side_on_plane( corners_inout, plane )) {
    case BSPTreeBoxIter::B0154:
      copy_coords( corners_inout[0], cut_face_in[0] );
      copy_coords( corners_inout[1], cut_face_in[1] );
      copy_coords( corners_inout[5], cut_face_in[2] );
      copy_coords( corners_inout[4], cut_face_in[3] );
      break;
    case BSPTreeBoxIter::B1265:
      copy_coords( corners_inout[1], cut_face_in[0] );
      copy_coords( corners_inout[2], cut_face_in[1] );
      copy_coords( corners_inout[6], cut_face_in[2] );
      copy_coords( corners_inout[5], cut_face_in[3] );
      break;
    case BSPTreeBoxIter::B2376:
      copy_coords( corners_inout[2], cut_face_in[0] );
      copy_coords( corners_inout[3], cut_face_in[1] );
      copy_coords( corners_inout[7], cut_face_in[2] );
      copy_coords( corners_inout[6], cut_face_in[3] );
      break;
    case BSPTreeBoxIter::B3047:
      copy_coords( corners_inout[3], cut_face_in[0] );
      copy_coords( corners_inout[0], cut_face_in[1] );
      copy_coords( corners_inout[4], cut_face_in[2] );
      copy_coords( corners_inout[7], cut_face_in[3] );
      break;
    case BSPTreeBoxIter::B3210:
      copy_coords( corners_inout[3], cut_face_in[0] );
      copy_coords( corners_inout[2], cut_face_in[1] );
      copy_coords( corners_inout[1], cut_face_in[2] );
      copy_coords( corners_inout[0], cut_face_in[3] );
      break;
    case BSPTreeBoxIter::B4567:
      copy_coords( corners_inout[4], cut_face_in[0] );
      copy_coords( corners_inout[5], cut_face_in[1] );
      copy_coords( corners_inout[6], cut_face_in[2] );
      copy_coords( corners_inout[7], cut_face_in[3] );
      break;
    default:
      return MB_FAILURE; // child is not a box
  }
  
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::step_to_first_leaf( Direction direction )
{
  ErrorCode rval;
  BSPTree::Plane plane;
  Corners clipped_corners;
  
  for (;;) {
    childVect.clear();
    rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
    if (MB_SUCCESS != rval)
      return rval;
    if (childVect.empty()) // leaf
      break;
  
    rval = tool()->get_split_plane( mStack.back(), plane );
    if (MB_SUCCESS != rval)
      return rval;
    
    if (direction == RIGHT)
      plane.flip();
    rval = plane_cut_box( clipped_corners.coords, leafCoords, plane );
    if (MB_SUCCESS != rval)
      return rval; 
    mStack.push_back( childVect[direction] );
    stackData.push_back( clipped_corners );
  }
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::up()
{
  ErrorCode rval;
  if (mStack.size() == 1)
    return MB_ENTITY_NOT_FOUND;
  
  EntityHandle node = mStack.back();
  Corners clipped_face = stackData.back();
  mStack.pop_back();
  stackData.pop_back();
  
  BSPTree::Plane plane;
  rval = tool()->get_split_plane( mStack.back(), plane );
  if (MB_SUCCESS != rval) {
    mStack.push_back( node );
    stackData.push_back( clipped_face );
    return rval;
  }
  
  rval = plane_uncut_box( clipped_face.coords, leafCoords, plane );
  if (MB_SUCCESS != rval) {
    mStack.push_back( node );
    stackData.push_back( clipped_face );
    return rval;
  }
  
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::down( const BSPTree::Plane& plane_ref, Direction direction )
{
  childVect.clear();
  ErrorCode rval = tool()->moab()->get_child_meshsets( mStack.back(), childVect );
  if (MB_SUCCESS != rval)
    return rval;
  if (childVect.empty())
    return MB_ENTITY_NOT_FOUND;
  
  BSPTree::Plane plane(plane_ref);
  if (direction == RIGHT)
    plane.flip();
  
  Corners clipped_face;
  rval = plane_cut_box( clipped_face.coords, leafCoords, plane );
  if (MB_SUCCESS != rval)
    return rval;
  
  mStack.push_back( childVect[direction] );
  stackData.push_back( clipped_face );
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::step( Direction direction )
{
  EntityHandle node, parent;
  Corners clipped_face;
  ErrorCode rval;
  BSPTree::Plane plane;
  const Direction opposite = static_cast<Direction>(1-direction);
  
    // If stack is empty, then either this iterator is uninitialized
    // or we reached the end of the iteration (and return 
    // MB_ENTITY_NOT_FOUND) already.
  if (mStack.empty())
    return MB_FAILURE;
    
    // Pop the current node from the stack.
    // The stack should then contain the parent of the current node.
    // If the stack is empty after this pop, then we've reached the end.
  node = mStack.back();
  mStack.pop_back();
  clipped_face = stackData.back();
  stackData.pop_back();
  
  while(!mStack.empty()) {
      // Get data for parent entity
    parent = mStack.back();
    childVect.clear();
    rval = tool()->moab()->get_child_meshsets( parent, childVect );
    if (MB_SUCCESS != rval)
      return rval;
    rval = tool()->get_split_plane( parent, plane );
    if (MB_SUCCESS != rval)
      return rval;
    if (direction == LEFT)
      plane.flip();
    
      // If we're at the left child
    if (childVect[opposite] == node) {
        // change from box of left child to box of parent
      plane_uncut_box( clipped_face.coords, leafCoords, plane );
        // change from box of parent to box of right child
      plane.flip();
      plane_cut_box( clipped_face.coords, leafCoords, plane );
        // push right child on stack
      mStack.push_back( childVect[direction] );
      stackData.push_back( clipped_face );
        // descend to left-most leaf of the right child
      return step_to_first_leaf(opposite);
    }
    
      // The current node is the right child of the parent,
      // continue up the tree.
    assert( childVect[direction] == node );
    plane.flip();
    plane_uncut_box( clipped_face.coords, leafCoords, plane );
    node = parent;
    clipped_face = stackData.back();
    mStack.pop_back();
    stackData.pop_back();
  }
  
  return MB_ENTITY_NOT_FOUND;
}

ErrorCode BSPTreeBoxIter::get_box_corners( double coords[8][3] ) const
{
  memcpy( coords, leafCoords, 24*sizeof(double) );
  return MB_SUCCESS;
}

// result = a - b
static void subtr( double result[3], const double a[3], const double b[3] )
{
  result[0] = a[0] - b[0];
  result[1] = a[1] - b[1];
  result[2] = a[2] - b[2];
}

// result = a + b + c + d
static void sum( double result[3], 
                 const double a[3], 
                 const double b[3],
                 const double c[3],
                 const double d[3] )
{
  result[0] = a[0] + b[0] + c[0] + d[0];
  result[1] = a[1] + b[1] + c[1] + d[1];
  result[2] = a[2] + b[2] + c[2] + d[2];
}

// result = a cross b
static void cross( double result[3], const double a[3], const double b[3] )
{
  result[0] = a[1]*b[2] - a[2]*b[1];
  result[1] = a[2]*b[0] - a[0]*b[2];
  result[2] = a[0]*b[1] - a[1]*b[0];
}

static double dot( const double a[3], const double b[3] )
{
  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

double BSPTreeBoxIter::volume() const
{
    // have planar sides, so use mid-face tripple product
  double f1[3], f2[3], f3[3], f4[3], f5[3], f6[3];
  sum( f1, leafCoords[0], leafCoords[1], leafCoords[4], leafCoords[5] );
  sum( f2, leafCoords[1], leafCoords[2], leafCoords[5], leafCoords[6] );
  sum( f3, leafCoords[2], leafCoords[3], leafCoords[6], leafCoords[7] );
  sum( f4, leafCoords[0], leafCoords[3], leafCoords[4], leafCoords[7] );
  sum( f5, leafCoords[0], leafCoords[1], leafCoords[2], leafCoords[3] );
  sum( f6, leafCoords[4], leafCoords[5], leafCoords[6], leafCoords[7] );
  double v13[3], v24[3], v65[3];
  subtr( v13, f1, f3 );
  subtr( v24, f2, f4 );
  subtr( v65, f6, f5 );
  double cr[3];
  cross( cr, v13, v24 );
  return (1./64) * dot( cr, v65 );
}

BSPTreeBoxIter::XSect 
BSPTreeBoxIter::splits( const BSPTree::Plane& plane ) const
{
  // test each corner relative to the plane
  unsigned result  = 0;
  for (unsigned i = 0; i < 8u; ++i) {
    double d = plane.signed_distance( leafCoords[i] );
      // if corner is on plane, than intersection 
      // will result in a degenerate hex
    if (fabs(d) < BSPTree::epsilon())
      return NONHEX;
      // if mark vertices above plane
    if (d > 0.0)
      result |= 1<<i;
  }
  
  switch (result) {
      // if all vertices or no vertices above plane,
      // then plane doesn't intersect
    case 0:
    case 0xFF:
      return MISS;
  
      // if there are four vertices above the plane
      // and they compose a single face of the hex,
      // then the cut will result in two hexes
    case B0154:
    case B1265:
    case B2376:
    case B3047:
    case B3210:
    case B4567:
      return SPLIT;
      
      // otherwise intersects, but split would not result
      // in two hexahedrons
    default:
      return NONHEX;
  }
}

bool BSPTreeBoxIter::intersects( const BSPTree::Plane& plane ) const
{
  // test each corner relative to the plane
  unsigned count  = 0;
  for (unsigned i = 0; i < 8u; ++i) 
    count += plane.above( leafCoords[i] );
  return count > 0 && count < 8u;
}

ErrorCode BSPTreeBoxIter::sibling_side( SideBits& side_out ) const
{
  if (mStack.size() < 2) // at tree root
    return MB_ENTITY_NOT_FOUND;
  
  EntityHandle parent = mStack[mStack.size()-2];
  BSPTree::Plane plane;
  ErrorCode rval = tool()->get_split_plane( parent, plane );
  if (MB_SUCCESS != rval)
    return MB_FAILURE;
  
  side_out = side_on_plane( leafCoords, plane );
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::get_neighbors( 
                      SideBits side,
                      std::vector<BSPTreeBoxIter>& results,
                      double epsilon ) const
{
  EntityHandle tmp_handle;
  BSPTree::Plane plane;
  ErrorCode rval;
  int n;
   
  Corners face;
  rval = face_corners( side, leafCoords, face.coords );
  if (MB_SUCCESS != rval)
    return rval;
  
    // Move up tree until we find the split that created the specified side.
    // Push the sibling at that level onto the iterator stack as
    // all neighbors will be rooted at that node.
  BSPTreeBoxIter iter( *this ); // temporary iterator (don't modifiy *this)
  for (;;) {
    tmp_handle = iter.handle();
  
    rval = iter.up();
    if (MB_SUCCESS != rval) // reached root - no neighbors on that side
      return (rval == MB_ENTITY_NOT_FOUND) ? MB_SUCCESS : rval;
    
    iter.childVect.clear();
    rval = tool()->moab()->get_child_meshsets( iter.handle(), iter.childVect );
    if (MB_SUCCESS!= rval)
      return rval;
    
    rval = tool()->get_split_plane( iter.handle(), plane );
    if (MB_SUCCESS != rval)
      return rval;
    SideBits s = side_above_plane( iter.leafCoords, plane );

    if (tmp_handle == iter.childVect[0] && s == side) {
      rval = iter.down( plane, RIGHT );
      if (MB_SUCCESS != rval)
        return rval;
      break;
    }
    else if (tmp_handle == iter.childVect[1] && opposite_face(s) == side) {
      rval = iter.down( plane, LEFT );
      if (MB_SUCCESS != rval)
        return rval;
      break;
    }
  }

    // now move down tree, searching for adjacent boxes
  std::vector<BSPTreeBoxIter> list;
    // loop over all potential paths to neighbors (until list is empty)
  for (;;) {
      // follow a single path to a leaf, append any other potential
      // paths to neighbors to 'list'
    for (;;) { 
      rval = tool()->moab()->num_child_meshsets( iter.handle(), &n );
      if (MB_SUCCESS != rval)
        return rval;
        
        // if leaf
      if (!n) {
        results.push_back( iter );
        break; 
      }
      
      rval = tool()->get_split_plane( iter.handle(), plane );
      if (MB_SUCCESS != rval)
        return rval;
     
      bool some_above = false, some_below = false;
      for (int i = 0; i < 4; ++i) {
        double signed_d = plane.signed_distance( face.coords[i] );
        if (signed_d > -epsilon)
          some_above = true;
        if (signed_d < epsilon)
          some_below = true;
      }
     
      if (some_above && some_below) {
        list.push_back( iter );
        list.back().down( plane, RIGHT );
        iter.down( plane, LEFT );
      }
      else if (some_above) {
        iter.down( plane, RIGHT );
      }
      else if (some_below) {
        iter.down( plane, LEFT );
      }
      else {
        // tolerance issue -- epsilon to small? 2D box?
        return MB_FAILURE;
      }
    }
    
    if (list.empty())
      break;
    
    iter = list.back();
    list.pop_back();
  }
  
  return MB_SUCCESS;
}

ErrorCode BSPTreeBoxIter::calculate_polyhedron( BSPTreePoly& poly_out ) const
{
  const CartVect* ptr = reinterpret_cast<const CartVect*>(leafCoords);
  return poly_out.set( ptr );
}

ErrorCode BSPTree::leaf_containing_point( EntityHandle tree_root,
                                              const double point[3],
                                              EntityHandle& leaf_out )
{
  std::vector<EntityHandle> children;
  Plane plane;
  EntityHandle node = tree_root;
  ErrorCode rval = moab()->get_child_meshsets( node, children );
  if (MB_SUCCESS != rval)
    return rval;
  while (!children.empty()) {
    rval = get_split_plane( node, plane );
    if (MB_SUCCESS != rval)
      return rval;
      
    node = children[plane.above(point)];
    children.clear();
    rval = moab()->get_child_meshsets( node, children );
    if (MB_SUCCESS != rval)
      return rval;
  }
  leaf_out = node;
  return MB_SUCCESS;
}

ErrorCode BSPTree::leaf_containing_point( EntityHandle root,
                                              const double point[3],
                                              BSPTreeIter& iter )
{
  ErrorCode rval;
  
  rval = iter.initialize( this, root, point );
  if (MB_SUCCESS != rval)
    return rval;
  
  for (;;) {
    iter.childVect.clear();
    rval = moab()->get_child_meshsets( iter.handle(), iter.childVect );
    if (MB_SUCCESS != rval || iter.childVect.empty())
      return rval;

    Plane plane;
    rval = get_split_plane( iter.handle(), plane );
    if (MB_SUCCESS != rval)
      return rval;

    rval = iter.down( plane, (BSPTreeIter::Direction)(plane.above( point )) );
    if (MB_SUCCESS != rval)
      return rval;
  }
}



template <typename PlaneIter> static inline
bool ray_intersect_halfspaces( const CartVect& ray_pt,
                               const CartVect& ray_dir,
                               const PlaneIter& begin, 
                               const PlaneIter& end,
                               double& t_enter, 
                               double& t_exit )
{
  const double epsilon = 1e-12;

    // begin with inifinite ray
  t_enter = 0.0;
  t_exit  = std::numeric_limits<double>::infinity();

    // cut ray such that we keep only the portion contained
    // in each halfspace
  for (PlaneIter i = begin; i != end; ++i) {
    CartVect norm( i->norm );
    double coeff = i->coeff;
    double den = norm % ray_dir;
    if (fabs(den) < epsilon) { // ray is parallel to plane
      if (i->above( ray_pt.array() ))
        return false; // ray entirely outside half-space
    }
    else {
      double t_xsect = (-coeff - (norm % ray_pt)) / den;
        // keep portion of ray/segment below plane
      if (den > 0) {
        if (t_xsect < t_exit)
          t_exit = t_xsect;
      }
      else {
        if (t_xsect > t_enter)
          t_enter = t_xsect;
      }
    }
  }
  
  return t_exit >= t_enter;
}
                               
class BoxPlaneIter {
      int faceNum;
      BSPTree::Plane facePlanes[6];
  
  public:  
    BoxPlaneIter( const double coords[8][3] );
    BoxPlaneIter( ) : faceNum(6) {} // initialize to 'end'
    const BSPTree::Plane* operator->() const
      { return facePlanes + faceNum; }
    bool operator==( const BoxPlaneIter& other ) const
      { return faceNum == other.faceNum; }
    bool operator!=( const BoxPlaneIter& other ) const
      { return faceNum != other.faceNum; }
    BoxPlaneIter& operator++()
      { ++faceNum; return *this; }
};

static const int box_face_corners[6][4] = { { 0, 1, 5, 4 },
                                            { 1, 2, 6, 5 },
                                            { 2, 3, 7, 6 },
                                            { 3, 0, 4, 7 },
                                            { 3, 2, 1, 0 },
                                            { 4, 5, 6, 7 } };
 
BoxPlaneIter::BoxPlaneIter( const double coords[8][3] )
  : faceNum(0)
{
    // NOTE:  In the case of a BSP tree, all sides of the
    //        leaf will planar.
  assert( sizeof(CartVect) == sizeof(coords[0]) );
  const CartVect* corners = reinterpret_cast<const CartVect*>(coords);
  for (int i = 0; i < 6; ++i) {
    const int* indices = box_face_corners[i];
    CartVect v1 = corners[indices[1]] - corners[indices[0]];
    CartVect v2 = corners[indices[3]] - corners[indices[0]];
    CartVect n = v1 * v2;
    facePlanes[i] = BSPTree::Plane( n.array(), -(n % corners[indices[2]]) );
  }
}


bool BSPTreeBoxIter::intersect_ray( const double ray_point[3],
                                      const double ray_vect[3],
                                      double& t_enter, double& t_exit ) const
{
  BoxPlaneIter iter( this->leafCoords ), end;
  return ray_intersect_halfspaces( CartVect(ray_point),
                                   CartVect(ray_vect),
                                   iter, end,
                                   t_enter, t_exit );
}

class BSPTreePlaneIter {
    BSPTree* toolPtr;
    const EntityHandle* const pathToRoot;
    int pathPos;
    BSPTree::Plane tmpPlane;
    std::vector<EntityHandle> tmpChildren;
  public:
    BSPTreePlaneIter( BSPTree* tool, const EntityHandle* path, int path_len )
      : toolPtr(tool), pathToRoot(path), pathPos(path_len-1)
      { operator++(); }
    BSPTreePlaneIter() // initialize to 'end'
      : toolPtr(0), pathToRoot(0), pathPos(-1) {}
  
    const BSPTree::Plane* operator->() const
      { return &tmpPlane; }
    bool operator==( const BSPTreePlaneIter& other ) const
      { return pathPos == other.pathPos; }
    bool operator!=( const BSPTreePlaneIter& other ) const
      { return pathPos != other.pathPos; }
    BSPTreePlaneIter& operator++();
};

BSPTreePlaneIter& BSPTreePlaneIter::operator++()
{
  if (--pathPos < 0)
    return *this;

  EntityHandle prev = pathToRoot[pathPos+1];
  EntityHandle curr = pathToRoot[pathPos];
  
  ErrorCode rval = toolPtr->get_split_plane( curr, tmpPlane );
  if (MB_SUCCESS != rval) {
    assert(false);
    pathPos = 0;
    return *this;
  }
  
  tmpChildren.clear();
  rval = toolPtr->moab()->get_child_meshsets( curr, tmpChildren );
  if (MB_SUCCESS != rval || tmpChildren.size() != 2) {
    assert(false);
    pathPos = 0;
    return *this;
  }
  
  if (tmpChildren[1] == prev) 
    tmpPlane.flip();
  return *this;
}


bool BSPTreeIter::intersect_ray( const double ray_point[3],
                                   const double ray_vect[3],
                                   double& t_enter, double& t_exit ) const
{
    // intersect with half-spaces defining tree
  BSPTreePlaneIter iter1( tool(), &mStack[0], mStack.size() ), end1;
  if (!ray_intersect_halfspaces( CartVect(ray_point),
                                 CartVect(ray_vect),
                                 iter1, end1,
                                 t_enter, t_exit ))
    return false;
  

    // itersect with box bounding entire tree
  double corners[8][3];
  ErrorCode rval = tool()->get_tree_box( mStack.front(), corners );
  if (MB_SUCCESS != rval) {
    assert(false); 
    return false;
  }
  
  BoxPlaneIter iter2( corners ), end2;
  double t2_enter, t2_exit;
  if (!ray_intersect_halfspaces( CartVect(ray_point),
                                 CartVect(ray_vect),
                                 iter2, end2,
                                 t2_enter, t2_exit ))
    return false;
  
    // if intersect both box and halfspaces, check that
    // two intersections overlap
  if (t_enter < t2_enter)
    t_enter = t2_enter;
  if (t_exit > t2_exit)
    t_exit = t2_exit;
  return t_enter <= t_exit;
}
  
} // namespace moab