ErrorCode OrientedBoxTreeTool::ray_intersect_all_sets( 
                                    std::vector<double>& distances_out,
                                    std::vector<EntityHandle>& sets_out,
                                    std::vector<EntityHandle>& facets_out,
                                    EntityHandle root_set,
                                    double tolerance,
                                    unsigned min_tolerace_intersections,
                                    const double ray_point[3],
                                    const double unit_ray_dir[3],
                                    const double* ray_length, 
                                    TrvStats* accum )
{
  Range boxes, box_sets;
  ErrorCode rval;
  
  rval = ray_intersect_boxes_sets( boxes, box_sets, root_set, tolerance,
				   ray_point, unit_ray_dir, ray_length, accum );
  if (MB_SUCCESS != rval)
    return rval;
    
  return ray_intersect_all_sets( distances_out, sets_out, facets_out,
				 boxes, box_sets, tolerance,
				 ray_point, unit_ray_dir, ray_length );


  RayIntersectAllSets op( this, ray_point, unit_ray_dir, ray_length, tolerance, 
			  boxes, box_sets ); 
  return preorder_traverse( root_set, op, accum );
}

ErrorCode OrientedBoxTreeTool::ray_intersect_all_sets( 
                          std::vector<double>& intersection_distances_out,
			  std::vector<EntityHandle>& sets_out,
			  std::vector<EntityHandle>& facets_out,
                          const Range& boxes,
			  const Range& sets,
                          double tolerance,
                          const double ray_point[3],
                          const double unit_ray_dir[3],
                          const double* ray_length )
{
  ErrorCode rval;
  intersection_distances_out.clear();
  sets_out.clear();
  facets_out.clear();
  std::vector<EntityHandle> tris;
    
  const CartVect point( ray_point );
  const CartVect dir( unit_ray_dir );
  Range::iterator s = sets.begin();

  for (Range::iterator b = boxes.begin(); b != boxes.end(); ++b, ++s)
  {
#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;
      int len;
      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;
      
      double td;
      if (GeomUtil::ray_tri_intersect( coords, point, dir, tolerance, td, ray_length )) {
        intersection_distances_out.push_back(td);
	sets_out.push_back(*s);
	facets_out.push_back(*t);
      }
    }
  }
  
  return MB_SUCCESS;
}

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

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

    EntityHandle lastSet;
    int lastSetDepth;
    
  public:
    RayIntersectAllSets( OrientedBoxTreeTool* tool_ptr,
			 const double* ray_point,
			 const double* unit_ray_dir,
			 const double *ray_length,
			 double tolerance,
			 Range& leaf_boxes,
			 Range& leaf_sets)
      : tool(tool_ptr),
        b(ray_point), m(unit_ray_dir),
        len(ray_length), tol(tolerance),
	boxes(leaf_boxes), sets(leaf_sets)
      { }
  
    virtual ErrorCode visit( EntityHandle node,
                               int depth,
                               bool& descend );
    virtual ErrorCode leaf( EntityHandle node );
};

ErrorCode RayIntersectAllSets::visit( EntityHandle node,
                                     int depth,
                                     bool& descend ) 
{
  OrientedBox box;
  ErrorCode rval = tool->box( node, box );
  if (MB_SUCCESS != rval)
    return rval;
  
  descend = box.intersect_ray( b, m, tol, len);

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

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

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

  return MB_SUCCESS;
}

ErrorCode RayIntersectAllSets::leaf( EntityHandle node )
{
  boxes.insert(node);

  if (!lastSet) {
    return MB_FAILURE;
  }
  sets.insert(lastSet);

  return MB_SUCCESS;
}