Cppcheck report - MOAB: src/OrientedBoxTreeTool.cpp

/*
 * MOAB, a Mesh-Oriented datABase, is a software component for creating,
 * storing and accessing finite element mesh data.
 *
 * Copyright 2004 Sandia Corporation.  Under the terms of Contract
 * DE-AC04-94AL85000 with Sandia Corporation, 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.
 *
 */

/**\file OrientedBox.hpp
 *\author Jason Kraftcheck ([email protected])
 *\date 2006-07-18
 */

#include "moab/Interface.hpp"
#include "Internals.hpp"
#include "moab/OrientedBoxTreeTool.hpp"
#include "moab/Range.hpp"
#include "moab/CN.hpp"
#include "moab/GeomUtil.hpp"
#include "MBTagConventions.hpp"
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <limits>
#include <cassert>
#include <cmath>

//#define MB_OBB_USE_VECTOR_QUERIES
//#define MB_OBB_USE_TYPE_QUERIES

namespace moab
{

#if defined( MB_OBB_USE_VECTOR_QUERIES ) && defined( MB_OBB_USE_TYPE_QUERIES )
#undef MB_OBB_USE_TYPE_QUERIES
#endif

const char DEFAULT_TAG_NAME[] = "OBB";

OrientedBoxTreeTool::Op::~Op() {}

OrientedBoxTreeTool::OrientedBoxTreeTool( Interface* i, const char* tag_name, bool destroy_created_trees )
    : instance( i ), cleanUpTrees( destroy_created_trees )
{
    if( !tag_name ) tag_name = DEFAULT_TAG_NAME;
    ErrorCode rval = OrientedBox::tag_handle( tagHandle, instance, tag_name );
    if( MB_SUCCESS != rval ) tagHandle = 0;
}

OrientedBoxTreeTool::~OrientedBoxTreeTool()
{
    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       = instance->tag_get_by_ptr( tagHandle, &tree, 1, &data_ptr );
        if( MB_SUCCESS == rval ) rval = delete_tree( tree );
        if( MB_SUCCESS != rval ) createdTrees.pop_back();
    }
}

OrientedBoxTreeTool::Settings::Settings()
    : max_leaf_entities( 8 ), max_depth( 0 ), worst_split_ratio( 0.95 ), best_split_ratio( 0.4 ),
      set_options( MESHSET_SET )
{
}

bool OrientedBoxTreeTool::Settings::valid() const
{
    return max_leaf_entities > 0 && max_depth >= 0 && worst_split_ratio <= 1.0 && best_split_ratio >= 0.0 &&
           worst_split_ratio >= best_split_ratio;
}

/********************** Simple Tree Access Methods ****************************/

ErrorCode OrientedBoxTreeTool::box( EntityHandle set, OrientedBox& obb )
{
    return instance->tag_get_data( tagHandle, &set, 1, &obb );
}

ErrorCode OrientedBoxTreeTool::box( EntityHandle set,
                                    double center[3],
                                    double axis1[3],
                                    double axis2[3],
                                    double axis3[3] )
{
    OrientedBox obb;
    ErrorCode rval = this->box( set, obb );
    obb.center.get( center );
    obb.scaled_axis( 0 ).get( axis1 );
    obb.scaled_axis( 1 ).get( axis2 );
    obb.scaled_axis( 2 ).get( axis3 );
    return rval;
}

/********************** Tree Construction Code ****************************/

struct OrientedBoxTreeTool::SetData
{
    EntityHandle handle;
    OrientedBox::CovarienceData box_data;
    // Range vertices;
};

ErrorCode OrientedBoxTreeTool::build( const Range& entities, EntityHandle& set_handle_out, const Settings* settings )
{
    if( !entities.all_of_dimension( 2 ) ) return MB_TYPE_OUT_OF_RANGE;
    if( settings && !settings->valid() ) return MB_FAILURE;

    return build_tree( entities, set_handle_out, 0, settings ? *settings : Settings() );
}

ErrorCode OrientedBoxTreeTool::join_trees( const Range& sets, EntityHandle& set_handle_out, const Settings* settings )
{
    if( !sets.all_of_type( MBENTITYSET ) ) return MB_TYPE_OUT_OF_RANGE;
    if( settings && !settings->valid() ) return MB_FAILURE;

    // Build initial set data list.
    std::list< SetData > data;
    for( Range::const_iterator i = sets.begin(); i != sets.end(); ++i )
    {
        Range elements;
        ErrorCode rval = instance->get_entities_by_dimension( *i, 2, elements, true );
        if( MB_SUCCESS != rval ) return rval;
        if( elements.empty() ) continue;

        data.push_back( SetData() );
        SetData& set_data = data.back();
        set_data.handle   = *i;
        rval              = OrientedBox::covariance_data_from_tris( set_data.box_data, instance, elements );
        if( MB_SUCCESS != rval ) return rval;
    }

    ErrorCode result = build_sets( data, set_handle_out, 0, settings ? *settings : Settings() );
    if( MB_SUCCESS != result ) return result;

    for( Range::reverse_iterator i = sets.rbegin(); i != sets.rend(); ++i )
    {
        createdTrees.erase( std::remove( createdTrees.begin(), createdTrees.end(), *i ), createdTrees.end() );
    }
    createdTrees.push_back( set_handle_out );
    return MB_SUCCESS;
}

/**\brief Split triangles by which side of a plane they are on
 *
 * Given a plane specified as a bisecting plane normal to one
 * of the axes of a box, split triangles based on which side
 * of the plane they are on.
 *\param instance   MOAB instance
 *\param box        The oriented box containing all the entities
 *\param axis       The axis for which the split plane is orthogonal
 *\param left_list  Output, entities to the left of the plane
 *\param right_list Output, entities to the right of the plane
 *\param num_intersecting Output, number entities intersecting plane
 */
static ErrorCode split_box( Interface* instance,
                            const OrientedBox& box,
                            int axis,
                            const Range& entities,
                            Range& left_list,
                            Range& right_list )
{
    ErrorCode rval;
    left_list.clear();
    right_list.clear();

    std::vector< CartVect > coords;
    for( Range::reverse_iterator i = entities.rbegin(); i != entities.rend(); ++i )
    {
        const EntityHandle* conn = NULL;
        int conn_len             = 0;
        rval                     = instance->get_connectivity( *i, conn, conn_len );
        if( MB_SUCCESS != rval ) return rval;

        coords.resize( conn_len );
        rval = instance->get_coords( conn, conn_len, coords[0].array() );
        if( MB_SUCCESS != rval ) return rval;

        CartVect centroid( 0.0 );
        for( int j = 0; j < conn_len; ++j )
            centroid += coords[j];
        centroid /= conn_len;

        if( ( box.axis( axis ) % ( centroid - box.center ) ) < 0.0 )
            left_list.insert( *i );
        else
            right_list.insert( *i );
    }

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::build_tree( const Range& entities,
                                           EntityHandle& set,
                                           int depth,
                                           const Settings& settings )
{
    OrientedBox tmp_box;
    ErrorCode rval;

    if( entities.empty() )
    {
        Matrix3 axis;
        tmp_box = OrientedBox( axis, CartVect( 0. ) );
    }
    else
    {
        rval = OrientedBox::compute_from_2d_cells( tmp_box, instance, entities );
        if( MB_SUCCESS != rval ) return rval;
    }

    // create an entity set for the tree node
    rval = instance->create_meshset( settings.set_options, set );
    if( MB_SUCCESS != rval ) return rval;

    rval = instance->tag_set_data( tagHandle, &set, 1, &tmp_box );
    if( MB_SUCCESS != rval )
    {
        delete_tree( set );
        return rval;
    }

    // check if should create children
    bool leaf = true;
    ++depth;
    if( ( !settings.max_depth || depth < settings.max_depth ) &&
        entities.size() > (unsigned)settings.max_leaf_entities )
    {
        // try splitting with planes normal to each axis of the box
        // until we find an acceptable split
        double best_ratio = settings.worst_split_ratio;  // worst case ratio
        Range best_left_list, best_right_list;
        // Axes are sorted from shortest to longest, so search backwards
        for( int axis = 2; best_ratio > settings.best_split_ratio && axis >= 0; --axis )
        {
            Range left_list, right_list;

            rval = split_box( instance, tmp_box, axis, entities, left_list, right_list );
            if( MB_SUCCESS != rval )
            {
                delete_tree( set );
                return rval;
            }

            double ratio = fabs( (double)right_list.size() - left_list.size() ) / entities.size();

            if( ratio < best_ratio )
            {
                best_ratio = ratio;
                best_left_list.swap( left_list );
                best_right_list.swap( right_list );
            }
        }

        // create children
        if( !best_left_list.empty() )
        {
            EntityHandle child = 0;

            rval = build_tree( best_left_list, child, depth, settings );
            if( MB_SUCCESS != rval )
            {
                delete_tree( set );
                return rval;
            }
            rval = instance->add_child_meshset( set, child );
            if( MB_SUCCESS != rval )
            {
                delete_tree( set );
                delete_tree( child );
                return rval;
            }

            rval = build_tree( best_right_list, child, depth, settings );
            if( MB_SUCCESS != rval )
            {
                delete_tree( set );
                return rval;
            }
            rval = instance->add_child_meshset( set, child );
            if( MB_SUCCESS != rval )
            {
                delete_tree( set );
                delete_tree( child );
                return rval;
            }

            leaf = false;
        }
    }

    if( leaf )
    {
        rval = instance->add_entities( set, entities );
        if( MB_SUCCESS != rval )
        {
            delete_tree( set );
            return rval;
        }
    }

    createdTrees.push_back( set );
    return MB_SUCCESS;
}

static ErrorCode split_sets( Interface*,
                             const OrientedBox& box,
                             int axis,
                             const std::list< OrientedBoxTreeTool::SetData >& sets,
                             std::list< OrientedBoxTreeTool::SetData >& left,
                             std::list< OrientedBoxTreeTool::SetData >& right )
{
    left.clear();
    right.clear();

    std::list< OrientedBoxTreeTool::SetData >::const_iterator i;
    for( i = sets.begin(); i != sets.end(); ++i )
    {
        CartVect centroid( i->box_data.center / i->box_data.area );
        if( ( box.axis( axis ) % ( centroid - box.center ) ) < 0.0 )
            left.push_back( *i );
        else
            right.push_back( *i );
    }

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::build_sets( std::list< SetData >& sets,
                                           EntityHandle& node_set,
                                           int depth,
                                           const Settings& settings )
{
    ErrorCode rval;
    int count = sets.size();
    if( 0 == count ) return MB_FAILURE;

    // calculate box
    OrientedBox obox;

    // make vector go out of scope when done, so memory is released
    {
        Range elems;
        std::vector< OrientedBox::CovarienceData > data( sets.size() );
        data.clear();
        for( std::list< SetData >::iterator i = sets.begin(); i != sets.end(); ++i )
        {
            data.push_back( i->box_data );
            rval = instance->get_entities_by_dimension( i->handle, 2, elems, true );
            if( MB_SUCCESS != rval ) return rval;
        }

        Range points;
        rval = instance->get_adjacencies( elems, 0, false, points, Interface::UNION );
        if( MB_SUCCESS != rval ) return rval;

        rval = OrientedBox::compute_from_covariance_data( obox, instance, &data[0], data.size(), points );
        if( MB_SUCCESS != rval ) return rval;
    }

    // If only one set in list...
    if( count == 1 )
    {
        node_set = sets.front().handle;
        return instance->tag_set_data( tagHandle, &node_set, 1, &obox );
    }

    // create an entity set for the tree node
    rval = instance->create_meshset( settings.set_options, node_set );
    if( MB_SUCCESS != rval ) return rval;

    rval = instance->tag_set_data( tagHandle, &node_set, 1, &obox );
    if( MB_SUCCESS != rval )
    {
        delete_tree( node_set );
        return rval;
    }

    double best_ratio = 2.0;
    std::list< SetData > best_left_list, best_right_list;
    for( int axis = 0; axis < 2; ++axis )
    {
        std::list< SetData > left_list, right_list;
        rval = split_sets( instance, obox, axis, sets, left_list, right_list );
        if( MB_SUCCESS != rval )
        {
            delete_tree( node_set );
            return rval;
        }

        double ratio = fabs( (double)right_list.size() - left_list.size() ) / sets.size();

        if( ratio < best_ratio )
        {
            best_ratio = ratio;
            best_left_list.swap( left_list );
            best_right_list.swap( right_list );
        }
    }

    // We must subdivide the list of sets, because we want to guarantee that
    // there is a node in the tree corresponding to each of the sets.  So if
    // we couldn't find a usable split plane, just split them in an arbitrary
    // manner.
    if( best_left_list.empty() || best_right_list.empty() )
    {
        best_left_list.clear();
        best_right_list.clear();
        std::list< SetData >* lists[2] = { &best_left_list, &best_right_list };
        int i                          = 0;
        while( !sets.empty() )
        {
            lists[i]->push_back( sets.front() );
            sets.pop_front();
            i = 1 - i;
        }
    }
    else
    {
        sets.clear();  // release memory before recursion
    }

    // Create child sets

    EntityHandle child = 0;

    rval = build_sets( best_left_list, child, depth + 1, settings );
    if( MB_SUCCESS != rval )
    {
        delete_tree( node_set );
        return rval;
    }
    rval = instance->add_child_meshset( node_set, child );
    if( MB_SUCCESS != rval )
    {
        delete_tree( node_set );
        delete_tree( child );
        return rval;
    }

    rval = build_sets( best_right_list, child, depth + 1, settings );
    if( MB_SUCCESS != rval )
    {
        delete_tree( node_set );
        return rval;
    }
    rval = instance->add_child_meshset( node_set, child );
    if( MB_SUCCESS != rval )
    {
        delete_tree( node_set );
        delete_tree( child );
        return rval;
    }

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::delete_tree( EntityHandle set )
{
    std::vector< EntityHandle > children;
    ErrorCode rval = instance->get_child_meshsets( set, children, 0 );
    if( MB_SUCCESS != rval ) return rval;

    createdTrees.erase( std::remove( createdTrees.begin(), createdTrees.end(), set ), createdTrees.end() );
    children.insert( children.begin(), set );
    return instance->delete_entities( &children[0], children.size() );
}

ErrorCode OrientedBoxTreeTool::remove_root( EntityHandle root )
{
    std::vector< EntityHandle >::iterator i = find( createdTrees.begin(), createdTrees.end(), root );
    if( i != createdTrees.end() )
    {
        createdTrees.erase( i );
        return MB_SUCCESS;
    }
    else
        return MB_ENTITY_NOT_FOUND;
}

/********************** Generic Tree Traversal ****************************/
struct Data
{
    EntityHandle set;
    int depth;
};
ErrorCode OrientedBoxTreeTool::preorder_traverse( EntityHandle set, Op& operation, TrvStats* accum )
{
    ErrorCode rval;
    std::vector< EntityHandle > children;
    std::vector< Data > the_stack;
    Data data = { set, 0 };
    the_stack.push_back( data );
    int max_depth = -1;

    while( !the_stack.empty() )
    {
        data = the_stack.back();
        the_stack.pop_back();

        // increment traversal statistics
        if( accum )
        {
            accum->increment( data.depth );
            max_depth = std::max( max_depth, data.depth );
        }

        bool descend = true;
        rval         = operation.visit( data.set, data.depth, descend );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        if( !descend ) continue;

        children.clear();
        rval = instance->get_child_meshsets( data.set, children );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;
        if( children.empty() )
        {
            if( accum )
            {
                accum->increment_leaf( data.depth );
            }
            rval = operation.leaf( data.set );
            assert( MB_SUCCESS == rval );
            if( MB_SUCCESS != rval ) return rval;
        }
        else if( children.size() == 2 )
        {
            data.depth++;
            data.set = children[0];
            the_stack.push_back( data );
            data.set = children[1];
            the_stack.push_back( data );
        }
        else
            return MB_MULTIPLE_ENTITIES_FOUND;
    }

    if( accum )
    {
        accum->end_traversal( max_depth );
    }

    return MB_SUCCESS;
}

/********************** General Sphere/Triangle Intersection ***************/

struct OBBTreeSITFrame
{
    OBBTreeSITFrame( EntityHandle n, EntityHandle s, int dp ) : node( n ), surf( s ), depth( dp ) {}
    EntityHandle node;
    EntityHandle surf;
    int depth;
};

ErrorCode OrientedBoxTreeTool::sphere_intersect_triangles( const double* center_v,
                                                           double radius,
                                                           EntityHandle tree_root,
                                                           std::vector< EntityHandle >& facets_out,
                                                           std::vector< EntityHandle >* sets_out,
                                                           TrvStats* accum )
{
    const double radsqr = radius * radius;
    OrientedBox b;
    ErrorCode rval;
    Range sets;
    const CartVect center( center_v );
    CartVect closest, coords[3];
    const EntityHandle* conn;
    int num_conn;
#ifndef MB_OBB_USE_VECTOR_QUERIES
    Range tris;
    Range::const_iterator t;
#else
    std::vector< EntityHandle > tris;
    std::vector< EntityHandle >::const_iterator t;
#endif

    std::vector< OBBTreeSITFrame > stack;
    std::vector< EntityHandle > children;
    stack.reserve( 30 );
    stack.push_back( OBBTreeSITFrame( tree_root, 0, 0 ) );

    int max_depth = -1;

    while( !stack.empty() )
    {
        EntityHandle surf = stack.back().surf;
        EntityHandle node = stack.back().node;
        int current_depth = stack.back().depth;
        stack.pop_back();

        // increment traversal statistics.
        if( accum )
        {
            accum->increment( current_depth );
            max_depth = std::max( max_depth, current_depth );
        }

        if( !surf && sets_out )
        {
            rval = get_moab_instance()->get_entities_by_type( node, MBENTITYSET, sets );<--- rval is assigned
            if( !sets.empty() ) surf = sets.front();
            sets.clear();
        }

        // check if sphere intersects box
        rval = box( node, b );<--- rval is overwritten
        if( MB_SUCCESS != rval ) return rval;
        b.closest_location_in_box( center, closest );
        closest -= center;
        if( ( closest % closest ) > radsqr ) continue;

        // push child boxes on stack
        children.clear();
        rval = instance->get_child_meshsets( node, children );
        if( MB_SUCCESS != rval ) return rval;
        if( !children.empty() )
        {
            assert( children.size() == 2 );
            stack.push_back( OBBTreeSITFrame( children[0], surf, current_depth + 1 ) );
            stack.push_back( OBBTreeSITFrame( children[1], surf, current_depth + 1 ) );
            continue;
        }

        if( accum )
        {
            accum->increment_leaf( current_depth );
        }
        // if leaf, intersect sphere with triangles
#ifndef MB_OBB_USE_VECTOR_QUERIES
#ifdef MB_OBB_USE_TYPE_QUERIES
        rval = get_moab_instance()->get_entities_by_type( node, MBTRI, tris );
#else
        rval = get_moab_instance()->get_entities_by_handle( node, tris );
#endif
        t = tris.begin();<--- t is assigned
#else
        rval = get_moab_instance()->get_entities_by_handle( node, tris );
        t    = tris.lower_bound( MBTRI );
#endif
        if( MB_SUCCESS != rval ) return rval;

        for( t = tris.begin(); t != tris.end(); ++t )<--- t is overwritten
        {
#ifndef MB_OBB_USE_VECTOR_QUERIES
            if( TYPE_FROM_HANDLE( *t ) != MBTRI ) break;
#elif !defined( MB_OBB_USE_TYPE_QUERIES )
            if( TYPE_FROM_HANDLE( *t ) != MBTRI ) continue;
#endif
            rval = get_moab_instance()->get_connectivity( *t, conn, num_conn, true );
            if( MB_SUCCESS != rval ) return rval;
            if( num_conn != 3 ) continue;

            rval = get_moab_instance()->get_coords( conn, num_conn, coords[0].array() );
            if( MB_SUCCESS != rval ) return rval;

            GeomUtil::closest_location_on_tri( center, coords, closest );
            closest -= center;
            if( ( closest % closest ) <= radsqr &&
                std::find( facets_out.begin(), facets_out.end(), *t ) == facets_out.end() )
            {
                facets_out.push_back( *t );
                if( sets_out ) sets_out->push_back( surf );
            }
        }
    }

    if( accum )
    {
        accum->end_traversal( max_depth );
    }

    return MB_SUCCESS;
}

/********************** General Ray/Tree and Ray/Triangle Intersection ***************/

class RayIntersector : public OrientedBoxTreeTool::Op
{
  private:
    OrientedBoxTreeTool* tool;
    const CartVect b, m;
    const double* len;
    const double tol;
    Range& boxes;

  public:
    RayIntersector( OrientedBoxTreeTool* tool_ptr,
                    const double* ray_point,
                    const double* unit_ray_dir,
                    const double* ray_length,
                    double tolerance,
                    Range& leaf_boxes )
        : tool( tool_ptr ), b( ray_point ), m( unit_ray_dir ), len( ray_length ), tol( tolerance ), boxes( leaf_boxes )
    {
    }

    virtual ErrorCode visit( EntityHandle node, int depth, bool& descend );<--- Function in derived class
    virtual ErrorCode leaf( EntityHandle node );<--- Function in derived class
};

//#include <stdio.h>
// inline void dump_fragmentation( const Range& range ) {
//  static FILE* file = fopen( "fragmentation", "w" );
//  unsigned ranges = 0, entities = 0;
//  for (Range::const_pair_iterator i = range.const_pair_begin(); i != range.const_pair_end(); ++i)
//  {
//    ++ranges;
//    entities += i->second - i->first + 1;
//  }
//  fprintf( file, "%u %u\n", ranges, entities );
//}

ErrorCode OrientedBoxTreeTool::ray_intersect_triangles( std::vector< double >& intersection_distances_out,
                                                        std::vector< EntityHandle >& intersection_facets_out,
                                                        const Range& boxes,
                                                        double /*tolerance*/,
                                                        const double ray_point[3],
                                                        const double unit_ray_dir[3],
                                                        const double* ray_length,
                                                        unsigned int* raytri_test_count )
{
    ErrorCode rval;
    intersection_distances_out.clear();
#ifdef MB_OBB_USE_VECTOR_QUERIES
    std::vector< EntityHandle > tris;
#endif

    const CartVect point( ray_point );
    const CartVect dir( unit_ray_dir );

    for( Range::iterator b = boxes.begin(); b != boxes.end(); ++b )
    {
#ifndef MB_OBB_USE_VECTOR_QUERIES
        Range tris;
#ifdef MB_OBB_USE_TYPE_QUERIES
        rval = instance->get_entities_by_type( *b, MBTRI, tris );
#else
        rval = instance->get_entities_by_handle( *b, tris );
#endif
#else
        tris.clear();
        rval = instance->get_entities_by_handle( *b, tris );
#endif
        if( MB_SUCCESS != rval ) return rval;
            // dump_fragmentation( tris );

#ifndef MB_OBB_USE_VECTOR_QUERIES
        for( Range::iterator t = tris.begin(); t != tris.end(); ++t )
#else
        for( std::vector< EntityHandle >::iterator t = tris.begin(); t != tris.end(); ++t )
#endif
        {
#ifndef MB_OBB_USE_TYPE_QUERIES
            if( TYPE_FROM_HANDLE( *t ) != MBTRI ) continue;
#endif

            const EntityHandle* conn = NULL;
            int len                  = 0;
            rval                     = instance->get_connectivity( *t, conn, len, true );
            if( MB_SUCCESS != rval ) return rval;

            CartVect coords[3];
            rval = instance->get_coords( conn, 3, coords[0].array() );
            if( MB_SUCCESS != rval ) return rval;

            if( raytri_test_count ) *raytri_test_count += 1;

            double td;
            if( GeomUtil::plucker_ray_tri_intersect( coords, point, dir, td, ray_length ) )
            {
                intersection_distances_out.push_back( td );
                intersection_facets_out.push_back( *t );
            }
        }
    }

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::ray_intersect_triangles( std::vector< double >& intersection_distances_out,
                                                        std::vector< EntityHandle >& intersection_facets_out,
                                                        EntityHandle root_set,
                                                        double tolerance,
                                                        const double ray_point[3],
                                                        const double unit_ray_dir[3],
                                                        const double* ray_length,
                                                        TrvStats* accum )
{
    Range boxes;
    ErrorCode rval;

    rval = ray_intersect_boxes( boxes, root_set, tolerance, ray_point, unit_ray_dir, ray_length, accum );
    if( MB_SUCCESS != rval ) return rval;

    return ray_intersect_triangles( intersection_distances_out, intersection_facets_out, boxes, tolerance, ray_point,
                                    unit_ray_dir, ray_length, accum ? &( accum->ray_tri_tests_count ) : NULL );
}

ErrorCode OrientedBoxTreeTool::ray_intersect_boxes( Range& boxes_out,
                                                    EntityHandle root_set,
                                                    double tolerance,
                                                    const double ray_point[3],
                                                    const double unit_ray_dir[3],
                                                    const double* ray_length,
                                                    TrvStats* accum )
{
    RayIntersector op( this, ray_point, unit_ray_dir, ray_length, tolerance, boxes_out );
    return preorder_traverse( root_set, op, accum );
}

ErrorCode RayIntersector::visit( EntityHandle node, int, bool& descend )
{
    OrientedBox box;
    ErrorCode rval = tool->box( node, box );
    if( MB_SUCCESS != rval ) return rval;

    descend = box.intersect_ray( b, m, tol, len );
    return MB_SUCCESS;
}

ErrorCode RayIntersector::leaf( EntityHandle node )
{
    boxes.insert( node );
    return MB_SUCCESS;
}

/********************** Ray/Set Intersection ****************************/

ErrorCode OrientedBoxTreeTool::get_close_tris( CartVect int_pt,
                                               double tol,
                                               const EntityHandle* rootSet,
                                               const EntityHandle* geomVol,
                                               const Tag* senseTag,
                                               std::vector< EntityHandle >& close_tris,
                                               std::vector< int >& close_senses )
{
    std::vector< EntityHandle > close_surfs;
    ErrorCode rval = sphere_intersect_triangles( int_pt.array(), tol, *rootSet, close_tris, &close_surfs );
    assert( MB_SUCCESS == rval );
    if( MB_SUCCESS != rval ) return rval;

    // for each surface, get the surf sense wrt parent volume
    close_senses.resize( close_surfs.size() );
    for( unsigned i = 0; i < close_surfs.size(); ++i )
    {
        EntityHandle vols[2];
        rval = get_moab_instance()->tag_get_data( *senseTag, &( close_surfs[i] ), 1, vols );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;
        if( vols[0] == vols[1] )
        {
            std::cerr << "error: surf has positive and negative sense wrt same volume" << std::endl;
            return MB_FAILURE;
        }
        if( *geomVol == vols[0] )
        {
            close_senses[i] = 1;
        }
        else if( *geomVol == vols[1] )
        {
            close_senses[i] = -1;
        }
        else
        {
            return MB_FAILURE;
        }
    }

    return MB_SUCCESS;
}

class RayIntersectSets : public OrientedBoxTreeTool::Op
{
  private:
    // Input
    OrientedBoxTreeTool* tool;
    const CartVect ray_origin;
    const CartVect ray_direction;
    OrientedBoxTreeTool::IntersectSearchWindow& search_win; /* length to search ahead/behind of ray origin */
    const double tol;                                       /* used for box.intersect_ray, radius of
                                                               neighborhood for adjacent triangles,
                                                               and old mode of add_intersection */
    OrientedBoxTreeTool::IntRegCtxt& int_reg_callback;

    // Optional Input - to screen RTIs by orientation and edge/node intersection
    int* surfTriOrient; /* holds desired orientation of tri wrt surface */
    int surfTriOrient_val;

    // Other Variables
    unsigned int* raytri_test_count;
    EntityHandle lastSet;
    int lastSetDepth;

  public:
    RayIntersectSets( OrientedBoxTreeTool* tool_ptr,
                      const double* ray_point,
                      const double* unit_ray_dir,
                      const double tolerance,
                      OrientedBoxTreeTool::IntersectSearchWindow& win,
                      unsigned int* ray_tri_test_count,
                      OrientedBoxTreeTool::IntRegCtxt& intRegCallback )
        : tool( tool_ptr ), ray_origin( ray_point ), ray_direction( unit_ray_dir ), search_win( win ), tol( tolerance ),
          int_reg_callback( intRegCallback ), surfTriOrient_val( 0 ), raytri_test_count( ray_tri_test_count ),
          lastSet( 0 ), lastSetDepth( 0 )
    {

        // specified orientation should be 1 or -1, indicating ray and surface
        // normal in the same or opposite directions, respectively.
        if( int_reg_callback.getDesiredOrient() )
        {
            surfTriOrient = &surfTriOrient_val;
        }
        else
        {
            surfTriOrient = NULL;
        }

        // check the limits
        if( search_win.first )
        {
            assert( 0 <= *( search_win.first ) );
        }
        if( search_win.second )
        {
            assert( 0 >= *( search_win.second ) );
        }
    };

    virtual ErrorCode visit( EntityHandle node, int depth, bool& descend );<--- Function in derived class
    virtual ErrorCode leaf( EntityHandle node );<--- Function in derived class
};

ErrorCode RayIntersectSets::visit( EntityHandle node, int depth, bool& descend )
{
    OrientedBox box;
    ErrorCode rval = tool->box( node, box );
    assert( MB_SUCCESS == rval );
    if( MB_SUCCESS != rval ) return rval;

    descend = box.intersect_ray( ray_origin, ray_direction, tol, search_win.first, search_win.second );

    if( lastSet && depth <= lastSetDepth ) lastSet = 0;

    if( descend && !lastSet )
    {
        Range tmp_sets;
        rval = tool->get_moab_instance()->get_entities_by_type( node, MBENTITYSET, tmp_sets );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        if( !tmp_sets.empty() )
        {
            if( tmp_sets.size() > 1 ) return MB_FAILURE;
            lastSet      = *tmp_sets.begin();
            lastSetDepth = depth;

            rval = int_reg_callback.update_orient( lastSet, surfTriOrient );
            if( MB_SUCCESS != rval ) return rval;
        }
    }

    return MB_SUCCESS;
}

ErrorCode RayIntersectSets::leaf( EntityHandle node )
{
    assert( lastSet );
    if( !lastSet )  // if no surface has been visited yet, something's messed up.
        return MB_FAILURE;

#ifndef MB_OBB_USE_VECTOR_QUERIES
    Range tris;
#ifdef MB_OBB_USE_TYPE_QUERIES
    ErrorCode rval = tool->get_moab_instance()->get_entities_by_type( node, MBTRI, tris );
#else
    ErrorCode rval = tool->get_moab_instance()->get_entities_by_handle( node, tris );
#endif
#else
    std::vector< EntityHandle > tris;
    ErrorCode rval = tool->get_moab_instance()->get_entities_by_handle( node, tris );
#endif
    assert( MB_SUCCESS == rval );
    if( MB_SUCCESS != rval ) return rval;

#ifndef MB_OBB_USE_VECTOR_QUERIES
    for( Range::iterator t = tris.begin(); t != tris.end(); ++t )
#else
    for( std::vector< EntityHandle >::iterator t = tris.begin(); t != tris.end(); ++t )
#endif
    {
#ifndef MB_OBB_USE_TYPE_QUERIES
        if( TYPE_FROM_HANDLE( *t ) != MBTRI ) continue;
#endif

        const EntityHandle* conn;
        int num_conn;
        rval = tool->get_moab_instance()->get_connectivity( *t, conn, num_conn, true );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        CartVect coords[3];
        rval = tool->get_moab_instance()->get_coords( conn, 3, coords[0].array() );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        if( raytri_test_count ) *raytri_test_count += 1;

        double int_dist;
        GeomUtil::intersection_type int_type = GeomUtil::NONE;

        if( GeomUtil::plucker_ray_tri_intersect( coords, ray_origin, ray_direction, int_dist, search_win.first,
                                                 search_win.second, surfTriOrient, &int_type ) )
        {
            int_reg_callback.register_intersection( lastSet, *t, int_dist, search_win, int_type );
        }
    }
    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::ray_intersect_sets( std::vector< double >& distances_out,
                                                   std::vector< EntityHandle >& sets_out,
                                                   std::vector< EntityHandle >& facets_out,
                                                   EntityHandle root_set,
                                                   const double tolerance,
                                                   const double ray_point[3],
                                                   const double unit_ray_dir[3],
                                                   IntersectSearchWindow& search_win,
                                                   IntRegCtxt& int_reg_callback,
                                                   TrvStats* accum )

{
    RayIntersectSets op( this, ray_point, unit_ray_dir, tolerance, search_win,
                         accum ? &( accum->ray_tri_tests_count ) : NULL, int_reg_callback );
    ErrorCode rval = preorder_traverse( root_set, op, accum );

    distances_out = int_reg_callback.get_intersections();
    sets_out      = int_reg_callback.get_sets();
    facets_out    = int_reg_callback.get_facets();

    return rval;
}

ErrorCode OrientedBoxTreeTool::ray_intersect_sets( std::vector< double >& distances_out,
                                                   std::vector< EntityHandle >& sets_out,
                                                   std::vector< EntityHandle >& facets_out,
                                                   EntityHandle root_set,
                                                   double tolerance,
                                                   const double ray_point[3],
                                                   const double unit_ray_dir[3],
                                                   const double* ray_length,
                                                   TrvStats* accum )
{
    IntRegCtxt int_reg_ctxt;

    OrientedBoxTreeTool::IntersectSearchWindow search_win( ray_length, (double*)0 );

    RayIntersectSets op( this, ray_point, unit_ray_dir, tolerance, search_win,
                         accum ? &( accum->ray_tri_tests_count ) : NULL, int_reg_ctxt );

    ErrorCode rval = preorder_traverse( root_set, op, accum );

    if( MB_SUCCESS != rval )
    {
        return rval;
    }

    distances_out = int_reg_ctxt.get_intersections();
    sets_out      = int_reg_ctxt.get_sets();
    facets_out    = int_reg_ctxt.get_facets();

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::ray_intersect_sets( EntityHandle root_set,
                                                   const double tolerance,
                                                   const double ray_point[3],
                                                   const double unit_ray_dir[3],
                                                   IntersectSearchWindow& search_win,
                                                   IntRegCtxt& int_reg_callback,
                                                   TrvStats* accum )

{
    RayIntersectSets op( this, ray_point, unit_ray_dir, tolerance, search_win,
                         accum ? &( accum->ray_tri_tests_count ) : NULL, int_reg_callback );
    return preorder_traverse( root_set, op, accum );
}

/********************** Closest Point code ***************/

struct OBBTreeCPFrame
{
    OBBTreeCPFrame( double d, EntityHandle n, EntityHandle s, int dp )
        : dist_sqr( d ), node( n ), mset( s ), depth( dp )
    {
    }
    double dist_sqr;
    EntityHandle node;
    EntityHandle mset;
    int depth;
};

ErrorCode OrientedBoxTreeTool::closest_to_location( const double* point,
                                                    EntityHandle root,
                                                    double* point_out,
                                                    EntityHandle& facet_out,
                                                    EntityHandle* set_out,
                                                    TrvStats* accum )
{
    ErrorCode rval;
    const CartVect loc( point );
    double smallest_dist_sqr = std::numeric_limits< double >::max();

    EntityHandle current_set = 0;
    Range sets;
    std::vector< EntityHandle > children( 2 );
    std::vector< double > coords;
    std::vector< OBBTreeCPFrame > stack;
    int max_depth = -1;

    stack.push_back( OBBTreeCPFrame( 0.0, root, current_set, 0 ) );

    while( !stack.empty() )
    {

        // pop from top of stack
        EntityHandle node = stack.back().node;
        double dist_sqr   = stack.back().dist_sqr;
        current_set       = stack.back().mset;
        int current_depth = stack.back().depth;
        stack.pop_back();

        // If current best result is closer than the box, skip this tree node.
        if( dist_sqr > smallest_dist_sqr ) continue;

        // increment traversal statistics.
        if( accum )
        {
            accum->increment( current_depth );
            max_depth = std::max( max_depth, current_depth );
        }

        // Check if this node has a set associated with it
        if( set_out && !current_set )
        {
            sets.clear();
            rval = instance->get_entities_by_type( node, MBENTITYSET, sets );
            if( MB_SUCCESS != rval ) return rval;
            if( !sets.empty() )
            {
                if( sets.size() != 1 ) return MB_MULTIPLE_ENTITIES_FOUND;
                current_set = sets.front();
            }
        }

        // Get child boxes
        children.clear();
        rval = instance->get_child_meshsets( node, children );
        if( MB_SUCCESS != rval ) return rval;

        // if not a leaf node
        if( !children.empty() )
        {
            if( children.size() != 2 ) return MB_MULTIPLE_ENTITIES_FOUND;

            // get boxes
            OrientedBox box1, box2;
            rval = box( children[0], box1 );
            if( MB_SUCCESS != rval ) return rval;
            rval = box( children[1], box2 );
            if( MB_SUCCESS != rval ) return rval;

            // get distance from each box
            CartVect pt1, pt2;
            box1.closest_location_in_box( loc, pt1 );
            box2.closest_location_in_box( loc, pt2 );
            pt1 -= loc;
            pt2 -= loc;
            const double dsqr1 = pt1 % pt1;
            const double dsqr2 = pt2 % pt2;

            // push children on tree such that closer one is on top
            if( dsqr1 < dsqr2 )
            {
                stack.push_back( OBBTreeCPFrame( dsqr2, children[1], current_set, current_depth + 1 ) );
                stack.push_back( OBBTreeCPFrame( dsqr1, children[0], current_set, current_depth + 1 ) );
            }
            else
            {
                stack.push_back( OBBTreeCPFrame( dsqr1, children[0], current_set, current_depth + 1 ) );
                stack.push_back( OBBTreeCPFrame( dsqr2, children[1], current_set, current_depth + 1 ) );
            }
        }
        else
        {  // LEAF NODE
            if( accum )
            {
                accum->increment_leaf( current_depth );
            }

            Range facets;
            rval = instance->get_entities_by_dimension( node, 2, facets );
            if( MB_SUCCESS != rval ) return rval;

            const EntityHandle* conn = NULL;
            int len                  = 0;
            CartVect tmp, diff;
            for( Range::iterator i = facets.begin(); i != facets.end(); ++i )
            {
                rval = instance->get_connectivity( *i, conn, len, true );
                if( MB_SUCCESS != rval ) return rval;

                coords.resize( 3 * len );
                rval = instance->get_coords( conn, len, &coords[0] );
                if( MB_SUCCESS != rval ) return rval;

                if( len == 3 )
                    GeomUtil::closest_location_on_tri( loc, (CartVect*)( &coords[0] ), tmp );
                else
                    GeomUtil::closest_location_on_polygon( loc, (CartVect*)( &coords[0] ), len, tmp );

                diff     = tmp - loc;
                dist_sqr = diff % diff;
                if( dist_sqr < smallest_dist_sqr )
                {
                    smallest_dist_sqr = dist_sqr;
                    facet_out         = *i;
                    tmp.get( point_out );
                    if( set_out ) *set_out = current_set;
                }
            }
        }  // LEAF NODE
    }

    if( accum )
    {
        accum->end_traversal( max_depth );
    }

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::closest_to_location( const double* point,
                                                    EntityHandle root,
                                                    double tolerance,
                                                    std::vector< EntityHandle >& facets_out,
                                                    std::vector< EntityHandle >* sets_out,
                                                    TrvStats* accum )
{
    ErrorCode rval;
    const CartVect loc( point );
    double smallest_dist_sqr = std::numeric_limits< double >::max();
    double smallest_dist     = smallest_dist_sqr;

    EntityHandle current_set = 0;
    Range sets;
    std::vector< EntityHandle > children( 2 );
    std::vector< double > coords;
    std::vector< OBBTreeCPFrame > stack;
    int max_depth = -1;

    stack.push_back( OBBTreeCPFrame( 0.0, root, current_set, 0 ) );

    while( !stack.empty() )
    {

        // pop from top of stack
        EntityHandle node = stack.back().node;
        double dist_sqr   = stack.back().dist_sqr;
        current_set       = stack.back().mset;
        int current_depth = stack.back().depth;
        stack.pop_back();

        // If current best result is closer than the box, skip this tree node.
        if( dist_sqr > smallest_dist_sqr + tolerance ) continue;

        // increment traversal statistics.
        if( accum )
        {
            accum->increment( current_depth );
            max_depth = std::max( max_depth, current_depth );
        }

        // Check if this node has a set associated with it
        if( sets_out && !current_set )
        {
            sets.clear();
            rval = instance->get_entities_by_type( node, MBENTITYSET, sets );
            if( MB_SUCCESS != rval ) return rval;
            if( !sets.empty() )
            {
                if( sets.size() != 1 ) return MB_MULTIPLE_ENTITIES_FOUND;
                current_set = *sets.begin();
            }
        }

        // Get child boxes
        children.clear();
        rval = instance->get_child_meshsets( node, children );
        if( MB_SUCCESS != rval ) return rval;

        // if not a leaf node
        if( !children.empty() )
        {
            if( children.size() != 2 ) return MB_MULTIPLE_ENTITIES_FOUND;

            // get boxes
            OrientedBox box1, box2;
            rval = box( children[0], box1 );
            if( MB_SUCCESS != rval ) return rval;
            rval = box( children[1], box2 );
            if( MB_SUCCESS != rval ) return rval;

            // get distance from each box
            CartVect pt1, pt2;
            box1.closest_location_in_box( loc, pt1 );
            box2.closest_location_in_box( loc, pt2 );
            pt1 -= loc;
            pt2 -= loc;
            const double dsqr1 = pt1 % pt1;
            const double dsqr2 = pt2 % pt2;

            // push children on tree such that closer one is on top
            if( dsqr1 < dsqr2 )
            {
                stack.push_back( OBBTreeCPFrame( dsqr2, children[1], current_set, current_depth + 1 ) );
                stack.push_back( OBBTreeCPFrame( dsqr1, children[0], current_set, current_depth + 1 ) );
            }
            else
            {
                stack.push_back( OBBTreeCPFrame( dsqr1, children[0], current_set, current_depth + 1 ) );
                stack.push_back( OBBTreeCPFrame( dsqr2, children[1], current_set, current_depth + 1 ) );
            }
        }
        else
        {  // LEAF NODE
            if( accum )
            {
                accum->increment_leaf( current_depth );
            }

            Range facets;
            rval = instance->get_entities_by_dimension( node, 2, facets );
            if( MB_SUCCESS != rval ) return rval;

            const EntityHandle* conn = NULL;
            int len                  = 0;
            CartVect tmp, diff;
            for( Range::iterator i = facets.begin(); i != facets.end(); ++i )
            {
                rval = instance->get_connectivity( *i, conn, len, true );
                if( MB_SUCCESS != rval ) return rval;

                coords.resize( 3 * len );
                rval = instance->get_coords( conn, len, &coords[0] );
                if( MB_SUCCESS != rval ) return rval;

                if( len == 3 )
                    GeomUtil::closest_location_on_tri( loc, (CartVect*)( &coords[0] ), tmp );
                else
                    GeomUtil::closest_location_on_polygon( loc, (CartVect*)( &coords[0] ), len, tmp );

                diff     = tmp - loc;
                dist_sqr = diff % diff;
                if( dist_sqr < smallest_dist_sqr )
                {
                    if( 0.5 * dist_sqr < 0.5 * smallest_dist_sqr + tolerance * ( 0.5 * tolerance - smallest_dist ) )
                    {
                        facets_out.clear();
                        if( sets_out ) sets_out->clear();
                    }
                    smallest_dist_sqr = dist_sqr;
                    smallest_dist     = sqrt( smallest_dist_sqr );
                    // put closest result at start of lists
                    facets_out.push_back( *i );
                    std::swap( facets_out.front(), facets_out.back() );
                    if( sets_out )
                    {
                        sets_out->push_back( current_set );
                        std::swap( sets_out->front(), sets_out->back() );
                    }
                }
                else if( dist_sqr <= smallest_dist_sqr + tolerance * ( tolerance + 2 * smallest_dist ) )
                {
                    facets_out.push_back( *i );
                    if( sets_out ) sets_out->push_back( current_set );
                }
            }
        }  // LEAF NODE
    }

    if( accum )
    {
        accum->end_traversal( max_depth );
    }

    return MB_SUCCESS;
}

/********************** Tree Printing Code ****************************/

class TreeLayoutPrinter : public OrientedBoxTreeTool::Op
{
  public:
    TreeLayoutPrinter( std::ostream& stream, Interface* instance );

    virtual ErrorCode visit( EntityHandle node, int depth, bool& descend );<--- Function in derived class
    virtual ErrorCode leaf( EntityHandle node );<--- Function in derived class

  private:
    Interface* instance;
    std::ostream& outputStream;
    std::vector< bool > path;
};

TreeLayoutPrinter::TreeLayoutPrinter( std::ostream& stream, Interface* interface )
    : instance( interface ), outputStream( stream )
{
}

ErrorCode TreeLayoutPrinter::visit( EntityHandle node, int depth, bool& descend )
{
    descend = true;

    if( (unsigned)depth > path.size() )
    {
        // assert(depth+1 == path.size); // preorder traversal
        path.push_back( true );
    }
    else
    {
        path.resize( depth );
        if( depth ) path.back() = false;
    }

    for( unsigned i = 0; i + 1 < path.size(); ++i )
    {
        if( path[i] )
            outputStream << "|   ";
        else
            outputStream << "    ";
    }
    if( depth )
    {
        if( path.back() )
            outputStream << "+---";
        else
            outputStream << "\\---";
    }
    outputStream << instance->id_from_handle( node ) << std::endl;
    return MB_SUCCESS;
}

ErrorCode TreeLayoutPrinter::leaf( EntityHandle )
{
    return MB_SUCCESS;
}

class TreeNodePrinter : public OrientedBoxTreeTool::Op
{
  public:
    TreeNodePrinter( std::ostream& stream,
                     bool list_contents,
                     bool list_box,
                     const char* id_tag_name,
                     OrientedBoxTreeTool* tool_ptr );

    virtual ErrorCode visit( EntityHandle node, int depth, bool& descend );<--- Function in derived class

    virtual ErrorCode leaf( EntityHandle )<--- Function in derived class
    {
        return MB_SUCCESS;
    }

  private:
    ErrorCode print_geometry( EntityHandle node );
    ErrorCode print_contents( EntityHandle node );
    ErrorCode print_counts( EntityHandle node );

    bool printContents;
    bool printGeometry;
    bool haveTag;
    Tag tag, gidTag, geomTag;
    Interface* instance;
    OrientedBoxTreeTool* tool;
    std::ostream& outputStream;
};

TreeNodePrinter::TreeNodePrinter( std::ostream& stream,
                                  bool list_contents,
                                  bool list_box,
                                  const char* id_tag_name,
                                  OrientedBoxTreeTool* tool_ptr )
    : printContents( list_contents ), printGeometry( list_box ), haveTag( false ), tag( 0 ), gidTag( 0 ), geomTag( 0 ),
      instance( tool_ptr->get_moab_instance() ), tool( tool_ptr ), outputStream( stream )
{
    ErrorCode rval;
    if( id_tag_name )
    {
        rval = instance->tag_get_handle( id_tag_name, 1, MB_TYPE_INTEGER, tag );
        if( !rval )
        {
            std::cerr << "Could not get tag \"" << id_tag_name << "\"\n";
            stream << "Could not get tag \"" << id_tag_name << "\"\n";
        }
        else
        {
            haveTag = true;
        }
    }

    gidTag = instance->globalId_tag();

    rval = instance->tag_get_handle( GEOM_DIMENSION_TAG_NAME, 1, MB_TYPE_INTEGER, geomTag );
    if( MB_SUCCESS != rval ) geomTag = 0;
}

ErrorCode TreeNodePrinter::visit( EntityHandle node, int, bool& descend )
{
    descend            = true;
    EntityHandle setid = instance->id_from_handle( node );
    outputStream << setid << ":" << std::endl;

    Range surfs;
    ErrorCode r3 = MB_SUCCESS;
    if( geomTag )
    {
        const int two         = 2;
        const void* tagdata[] = { &two };
        r3 = instance->get_entities_by_type_and_tag( node, MBENTITYSET, &geomTag, tagdata, 1, surfs );

        if( MB_SUCCESS == r3 && surfs.size() == 1 )
        {
            EntityHandle surf = *surfs.begin();
            int id;
            if( gidTag && MB_SUCCESS == instance->tag_get_data( gidTag, &surf, 1, &id ) )
                outputStream << "  Surface " << id << std::endl;
            else
                outputStream << "  Surface w/ unknown ID (" << surf << ")" << std::endl;
        }
    }

    ErrorCode r1 = printGeometry ? print_geometry( node ) : MB_SUCCESS;
    ErrorCode r2 = printContents ? print_contents( node ) : print_counts( node );
    outputStream << std::endl;

    if( MB_SUCCESS != r1 )
        return r1;
    else if( MB_SUCCESS != r2 )
        return r2;
    else
        return r3;
}

ErrorCode TreeNodePrinter::print_geometry( EntityHandle node )
{
    OrientedBox box;
    ErrorCode rval = tool->box( node, box );
    if( MB_SUCCESS != rval ) return rval;

    CartVect length = box.dimensions();

    outputStream << box.center << "  Radius: " << box.inner_radius() << " - " << box.outer_radius() << std::endl
                 << '+' << box.axis( 0 ) << " : " << length[0] << std::endl
                 << 'x' << box.axis( 1 ) << " : " << length[1] << std::endl
                 << 'x' << box.axis( 2 ) << " : " << length[2] << std::endl;
    return MB_SUCCESS;
}

ErrorCode TreeNodePrinter::print_counts( EntityHandle node )
{
    for( EntityType type = MBVERTEX; type != MBMAXTYPE; ++type )
    {
        int count      = 0;
        ErrorCode rval = instance->get_number_entities_by_type( node, type, count );
        if( MB_SUCCESS != rval ) return rval;
        if( count > 0 ) outputStream << " " << count << " " << CN::EntityTypeName( type ) << std::endl;
    }
    return MB_SUCCESS;
}

ErrorCode TreeNodePrinter::print_contents( EntityHandle node )
{
    // list contents
    for( EntityType type = MBVERTEX; type != MBMAXTYPE; ++type )
    {
        Range range;
        ErrorCode rval = instance->get_entities_by_type( node, type, range );
        if( MB_SUCCESS != rval ) return rval;
        if( range.empty() ) continue;
        outputStream << " " << CN::EntityTypeName( type ) << " ";
        std::vector< int > ids( range.size() );
        if( haveTag )
        {
            rval = instance->tag_get_data( tag, range, &ids[0] );
            std::sort( ids.begin(), ids.end() );
        }
        else
        {
            Range::iterator ri              = range.begin();
            std::vector< int >::iterator vi = ids.begin();
            while( ri != range.end() )
            {
                *vi = instance->id_from_handle( *ri );
                ++ri;
                ++vi;
            }
        }

        unsigned i = 0;
        for( ;; )
        {
            unsigned beg = i, end;
            do
            {
                end = i++;
            } while( i < ids.size() && ids[end] + 1 == ids[i] );
            if( end == beg )
                outputStream << ids[end];
            else if( end == beg + 1 )
                outputStream << ids[beg] << ", " << ids[end];
            else
                outputStream << ids[beg] << "-" << ids[end];

            if( i == ids.size() )
            {
                outputStream << std::endl;
                break;
            }
            else
                outputStream << ", ";
        }
    }

    return MB_SUCCESS;
}

void OrientedBoxTreeTool::print( EntityHandle set, std::ostream& str, bool list, const char* tag )
{
    TreeLayoutPrinter op1( str, instance );
    TreeNodePrinter op2( str, list, true, tag, this );
    ErrorCode r1 = preorder_traverse( set, op1 );
    str << std::endl;
    ErrorCode r2 = preorder_traverse( set, op2 );
    if( r1 != MB_SUCCESS || r2 != MB_SUCCESS )
    {
        std::cerr << "Errors encountered while printing tree\n";
        str << "Errors encountered while printing tree\n";
    }
}

/********************* Traversal Metrics Code  **************************/

void OrientedBoxTreeTool::TrvStats::reset()
{
    nodes_visited_count.clear();
    leaves_visited_count.clear();
    traversals_ended_count.clear();
    ray_tri_tests_count = 0;
}

void OrientedBoxTreeTool::TrvStats::increment( unsigned depth )
{

    while( nodes_visited_count.size() <= depth )
    {
        nodes_visited_count.push_back( 0 );
        leaves_visited_count.push_back( 0 );
        traversals_ended_count.push_back( 0 );
    }
    nodes_visited_count[depth] += 1;
}

void OrientedBoxTreeTool::TrvStats::increment_leaf( unsigned depth )
{
    // assume safe depth, because increment is called first
    leaves_visited_count[depth] += 1;
}

void OrientedBoxTreeTool::TrvStats::end_traversal( unsigned depth )
{
    // assume safe depth, because increment is always called on a given
    // tree level first
    traversals_ended_count[depth] += 1;
}

void OrientedBoxTreeTool::TrvStats::print( std::ostream& str ) const
{

    const std::string h1 = "OBBTree Depth";
    const std::string h2 = " - NodesVisited";
    const std::string h3 = " - LeavesVisited";
    const std::string h4 = " - TraversalsEnded";

    str << h1 << h2 << h3 << h4 << std::endl;

    unsigned num_visited = 0, num_leaves = 0, num_traversals = 0;
    for( unsigned i = 0; i < traversals_ended_count.size(); ++i )
    {

        num_visited += nodes_visited_count[i];
        num_leaves += leaves_visited_count[i];
        num_traversals += traversals_ended_count[i];

        str << std::setw( h1.length() ) << i << std::setw( h2.length() ) << nodes_visited_count[i]
            << std::setw( h3.length() ) << leaves_visited_count[i] << std::setw( h4.length() )
            << traversals_ended_count[i] << std::endl;
    }

    str << std::setw( h1.length() ) << "---- Totals:" << std::setw( h2.length() ) << num_visited
        << std::setw( h3.length() ) << num_leaves << std::setw( h4.length() ) << num_traversals << std::endl;

    if( ray_tri_tests_count )
    {
        str << std::setw( h1.length() ) << "---- Total ray-tri tests: " << ray_tri_tests_count << std::endl;
    }
}

/********************** Tree Statistics Code ****************************/

class StatData
{
  public:
    struct Ratio
    {
        double min, max, sum, sqr;
        int hist[10];
        Ratio()
            : min( std::numeric_limits< double >::max() ), max( -std::numeric_limits< double >::max() ), sum( 0.0 ),
              sqr( 0.0 )
        {
            hist[0] = hist[1] = hist[2] = hist[3] = hist[4] = hist[5] = hist[6] = hist[7] = hist[8] = hist[9] = 0;
        }
        void accum( double v )
        {
            if( v < min ) min = v;
            if( v > max ) max = v;
            sum += v;
            sqr += v * v;
            int i = (int)( 10 * v );
            if( i < 0 )
                i = 0;
            else if( i > 9 )
                i = 9;
            ++hist[i];
        }
    };

    template < typename T >
    struct Stat
    {
        T min, max;
        double sum, sqr;
        Stat() : sum( 0.0 ), sqr( 0.0 )
        {
            std::numeric_limits< T > lim;
            min = lim.max();
            if( lim.is_integer )
                max = lim.min();
            else
                max = -lim.max();
        }
        void accum( T v )
        {
            if( v < min ) min = v;
            if( v > max ) max = v;
            sum += v;
            sqr += (double)v * v;
        }
    };

    StatData() : count( 0 ) {}

    Ratio volume;
    Ratio entities;
    Ratio radius;
    Stat< unsigned > leaf_ent;
    Stat< double > vol;
    Stat< double > area;
    std::vector< unsigned > leaf_depth;
    unsigned count;
};

static int measure( const CartVect& v, double& result )
{
    const double tol = 1e-6;
    int dims         = 0;
    result           = 1;
    for( int i = 0; i < 3; ++i )
        if( v[i] > tol )
        {
            ++dims;
            result *= v[i];
        }
    return dims;
}

ErrorCode OrientedBoxTreeTool::recursive_stats( OrientedBoxTreeTool* tool,
                                                Interface* inst,
                                                EntityHandle set,
                                                int depth,
                                                StatData& data,
                                                unsigned& count_out,
                                                CartVect& dimensions_out )
{
    ErrorCode rval;
    OrientedBox tmp_box;
    std::vector< EntityHandle > children( 2 );
    unsigned counts[2];
    bool isleaf;

    ++data.count;

    rval = tool->box( set, tmp_box );
    if( MB_SUCCESS != rval ) return rval;
    children.clear();
    rval = inst->get_child_meshsets( set, children );
    if( MB_SUCCESS != rval ) return rval;
    isleaf = children.empty();
    if( !isleaf && children.size() != 2 ) return MB_MULTIPLE_ENTITIES_FOUND;

    dimensions_out = tmp_box.dimensions();
    data.radius.accum( tmp_box.inner_radius() / tmp_box.outer_radius() );
    data.vol.accum( tmp_box.volume() );
    data.area.accum( tmp_box.area() );

    if( isleaf )
    {
        if( data.leaf_depth.size() <= (unsigned)depth ) data.leaf_depth.resize( depth + 1, 0 );
        ++data.leaf_depth[depth];

        int count;
        rval = inst->get_number_entities_by_handle( set, count );
        if( MB_SUCCESS != rval ) return rval;
        count_out = count;
        data.leaf_ent.accum( count_out );
    }
    else
    {
        for( int i = 0; i < 2; ++i )
        {
            CartVect dims;
            rval = recursive_stats( tool, inst, children[i], depth + 1, data, counts[i], dims );
            if( MB_SUCCESS != rval ) return rval;
            double this_measure, chld_measure;
            int this_dim = measure( dimensions_out, this_measure );
            int chld_dim = measure( dims, chld_measure );
            double ratio;
            if( chld_dim < this_dim )
                ratio = 0;
            else
                ratio = chld_measure / this_measure;

            data.volume.accum( ratio );
        }
        count_out = counts[0] + counts[1];
        data.entities.accum( (double)counts[0] / count_out );
        data.entities.accum( (double)counts[1] / count_out );
    }
    return MB_SUCCESS;
}

static inline double std_dev( double sqr, double sum, double count )
{
    sum /= count;
    sqr /= count;
    return sqrt( sqr - sum * sum );
}

//#define WW <<std::setw(10)<<std::fixed<<
#define WE << std::setw( 10 ) <<
#define WW WE
ErrorCode OrientedBoxTreeTool::stats( EntityHandle set,
                                      unsigned& total_entities,
                                      double& rv,
                                      double& tot_node_volume,
                                      double& tot_to_root_volume,
                                      unsigned& tree_height,
                                      unsigned& node_count,
                                      unsigned& num_leaves )
{
    StatData d;
    ErrorCode rval;
    unsigned i;
    CartVect total_dim;

    rval = recursive_stats( this, instance, set, 0, d, total_entities, total_dim );
    if( MB_SUCCESS != rval ) return rval;

    tree_height                 = d.leaf_depth.size();
    unsigned min_leaf_depth     = tree_height;
    num_leaves                  = 0;
    unsigned max_leaf_per_depth = 0;
    // double sum_leaf_depth = 0, sqr_leaf_depth = 0;
    for( i = 0; i < d.leaf_depth.size(); ++i )
    {
        unsigned val = d.leaf_depth[i];
        num_leaves += val;
        // sum_leaf_depth += (double)val*i;
        // sqr_leaf_depth += (double)val*i*i;
        if( val && i < min_leaf_depth ) min_leaf_depth = i;
        if( max_leaf_per_depth < val ) max_leaf_per_depth = val;
    }
    rv                 = total_dim[0] * total_dim[1] * total_dim[2];
    tot_node_volume    = d.vol.sum;
    tot_to_root_volume = d.vol.sum / rv;
    node_count         = d.count;

    return MB_SUCCESS;
}

ErrorCode OrientedBoxTreeTool::stats( EntityHandle set, std::ostream& s )
{
    StatData d;
    ErrorCode rval;
    unsigned total_entities, i;
    CartVect total_dim;

    rval = recursive_stats( this, instance, set, 0, d, total_entities, total_dim );
    if( MB_SUCCESS != rval ) return rval;

    unsigned tree_height    = d.leaf_depth.size();
    unsigned min_leaf_depth = tree_height, num_leaves = 0;
    unsigned max_leaf_per_depth = 0;
    double sum_leaf_depth = 0, sqr_leaf_depth = 0;
    for( i = 0; i < d.leaf_depth.size(); ++i )
    {
        unsigned val = d.leaf_depth[i];
        num_leaves += val;
        sum_leaf_depth += (double)val * i;
        sqr_leaf_depth += (double)val * i * i;
        if( val && i < min_leaf_depth ) min_leaf_depth = i;
        if( max_leaf_per_depth < val ) max_leaf_per_depth = val;
    }
    unsigned num_non_leaf = d.count - num_leaves;

    double rv = total_dim[0] * total_dim[1] * total_dim[2];
    s << "entities in tree:  " << total_entities << std::endl
      << "root volume:       " << rv << std::endl
      << "total node volume: " << d.vol.sum << std::endl
      << "total/root volume: " << d.vol.sum / rv << std::endl
      << "tree height:       " << tree_height << std::endl
      << "node count:        " << d.count << std::endl
      << "leaf count:        " << num_leaves << std::endl
      << std::endl;

    double avg_leaf_depth = sum_leaf_depth / num_leaves;
    double rms_leaf_depth = sqrt( sqr_leaf_depth / num_leaves );
    double std_leaf_depth = std_dev( sqr_leaf_depth, sum_leaf_depth, num_leaves );

    double avg_leaf_ent = d.leaf_ent.sum / num_leaves;
    double rms_leaf_ent = sqrt( d.leaf_ent.sqr / num_leaves );
    double std_leaf_ent = std_dev( d.leaf_ent.sqr, d.leaf_ent.sum, num_leaves );

    unsigned num_child = 2 * num_non_leaf;

    double avg_vol_ratio = d.volume.sum / num_child;
    double rms_vol_ratio = sqrt( d.volume.sqr / num_child );
    double std_vol_ratio = std_dev( d.volume.sqr, d.volume.sum, num_child );

    double avg_ent_ratio = d.entities.sum / num_child;
    double rms_ent_ratio = sqrt( d.entities.sqr / num_child );
    double std_ent_ratio = std_dev( d.entities.sqr, d.entities.sum, num_child );

    double avg_rad_ratio = d.radius.sum / d.count;
    double rms_rad_ratio = sqrt( d.radius.sqr / d.count );
    double std_rad_ratio = std_dev( d.radius.sqr, d.radius.sum, d.count );

    double avg_vol = d.vol.sum / d.count;
    double rms_vol = sqrt( d.vol.sqr / d.count );
    double std_vol = std_dev( d.vol.sqr, d.vol.sum, d.count );

    double avg_area = d.area.sum / d.count;
    double rms_area = sqrt( d.area.sqr / d.count );
    double std_area = std_dev( d.area.sqr, d.area.sum, d.count );

    int prec = s.precision();
    s << "                   " WW "Minimum" WW "Average" WW "RMS" WW "Maximum" WW "Std.Dev." << std::endl;
    s << std::setprecision( 1 )
      << "Leaf Depth         " WW min_leaf_depth WW avg_leaf_depth WW rms_leaf_depth WW d.leaf_depth.size() -
             1 WW std_leaf_depth
      << std::endl;
    s << std::setprecision( 0 )
      << "Entities/Leaf      " WW d.leaf_ent.min WW avg_leaf_ent WW rms_leaf_ent WW d.leaf_ent.max WW std_leaf_ent
      << std::endl;
    s << std::setprecision( 3 )
      << "Child Volume Ratio " WW d.volume.min WW avg_vol_ratio WW rms_vol_ratio WW d.volume.max WW std_vol_ratio
      << std::endl;
    s << std::setprecision( 3 )
      << "Child Entity Ratio " WW d.entities.min WW avg_ent_ratio WW rms_ent_ratio WW d.entities.max WW std_ent_ratio
      << std::endl;
    s << std::setprecision( 3 )
      << "Box Radius Ratio   " WW d.radius.min WW avg_rad_ratio WW rms_rad_ratio WW d.radius.max WW std_rad_ratio
      << std::endl;
    s << std::setprecision( 0 ) << "Box volume         " WE d.vol.min WE avg_vol WE rms_vol WE d.vol.max WE std_vol
      << std::endl;
    s << std::setprecision( 0 ) << "Largest side area  " WE d.area.min WE avg_area WE rms_area WE d.area.max WE std_area
      << std::endl;
    s << std::setprecision( prec ) << std::endl;

    s << "Leaf Depth Histogram (Root depth is 0)" << std::endl;
    double f = 60.0 / max_leaf_per_depth;
    for( i = min_leaf_depth; i < d.leaf_depth.size(); ++i )
        s << std::setw( 2 ) << i << " " << std::setw( 5 ) << d.leaf_depth[i] << " |" << std::setfill( '*' )
          << std::setw( (int)floor( f * d.leaf_depth[i] + 0.5 ) ) << "" << std::setfill( ' ' ) << std::endl;
    s << std::endl;

    s << "Child/Parent Volume Ratio Histogram" << std::endl;
    f = 60.0 / *( std::max_element( d.volume.hist, d.volume.hist + 10 ) );
    for( i = 0; i < 10u; ++i )
        s << "0." << i << " " << std::setw( 5 ) << d.volume.hist[i] << " |" << std::setfill( '*' )
          << std::setw( (int)floor( f * d.volume.hist[i] + 0.5 ) ) << "" << std::setfill( ' ' ) << std::endl;
    s << std::endl;

    s << "Child/Parent Entity Count Ratio Histogram" << std::endl;
    f = 60.0 / *( std::max_element( d.entities.hist, d.entities.hist + 10 ) );
    for( i = 0; i < 10u; ++i )
        s << "0." << i << " " << std::setw( 5 ) << d.entities.hist[i] << " |" << std::setfill( '*' )
          << std::setw( (int)floor( f * d.entities.hist[i] + 0.5 ) ) << "" << std::setfill( ' ' ) << std::endl;
    s << std::endl;

    s << "Inner/Outer Radius Ratio Histogram (~0.70 for cube)" << std::endl;
    // max radius ratio for a box is about 0.7071.  Move any boxes
    // in the .7 bucket into .6 and print .0 to .6.
    d.radius.hist[6] += d.radius.hist[7];
    f = 60.0 / *( std::max_element( d.entities.hist, d.entities.hist + 7 ) );
    for( i = 0; i < 7u; ++i )
        s << "0." << i << " " << std::setw( 5 ) << d.entities.hist[i] << " |" << std::setfill( '*' )
          << std::setw( (int)floor( f * d.entities.hist[i] + 0.5 ) ) << "" << std::setfill( ' ' ) << std::endl;
    s << std::endl;

    return MB_SUCCESS;
}

}  // namespace moab