root/cgm/trunk/geom/AnalyticGeometryTool.cpp

//- Class:       AnalyticGeometryTool
//- Description: This class performs calculations on analytic geometry
//               (points, lines, arcs, planes, polygons).  Capabilities 
//               include vector and point math, matrix operations, 
//               measurements, intersections and comparison/containment checks.
// 
//- Owner: Steve Storm, Caterpillar Inc.
//- Checked by:


#include <float.h>
#include "AnalyticGeometryTool.hpp"
#include "CubitMessage.hpp"
#include "CubitBox.hpp"
#include "CubitPlane.hpp"
#include "CubitVector.hpp"
#include "DLIList.hpp"
#include <math.h>
#include "CastTo.hpp"

#include <fstream>
using std::cout;
using std::endl;
using std::ofstream;
using std::ios;
using std::ostream;

static double AGT_IDENTITY_MTX_4X4[4][4] = { {1.0, 0.0, 0.0, 0.0},
                                             {0.0, 1.0, 0.0, 0.0},
                                             {0.0, 0.0, 1.0, 0.0},
                                             {0.0, 0.0, 0.0, 1.0} };

static double AGT_IDENTITY_MTX_3X3[3][3] = { {1.0, 0.0, 0.0},
                                             {0.0, 1.0, 0.0},
                                             {0.0, 0.0, 1.0} };

AnalyticGeometryTool* AnalyticGeometryTool::instance_ = 0;

Point2 operator+ (const Point2& p, const Point2& q)
{
    Point2 add;
    add.x = p.x + q.x;
    add.y = p.y + q.y;
    return add;
}

Point2 operator- (const Point2& p, const Point2& q)
{
    Point2 sub;
    sub.x = p.x - q.x;
    sub.y = p.y - q.y;
    return sub;
}

Point2 operator* (double t, const Point2& p)
{
    Point2 prod;
    prod.x = t*p.x;
    prod.y = t*p.y;
    return prod;
}

Point2 operator* (const Point2& p, double t)
{
    Point2 prod;
    prod.x = t*p.x;
    prod.y = t*p.y;
    return prod;
}

Point2 operator- (const Point2& p)
{
    Point2 neg;
    neg.x = -p.x;
    neg.y = -p.y;
    return neg;
}

inline double AnalyticGeometryTool::Dot (const Point2& p, const Point2& q)
{
    return double(p.x*q.x + p.y*q.y);
}

Point3 operator+ (const Point3& p, const Point3& q)
{
    Point3 add;
    add.x = p.x + q.x;
    add.y = p.y + q.y;
    add.z = p.z + q.z;
    return add;
}

Point3 operator- (const Point3& p, const Point3& q)
{
    Point3 sub;
    sub.x = p.x - q.x;
    sub.y = p.y - q.y;
    sub.z = p.z - q.z;
    return sub;
}

Point3 operator* (double t, const Point3& p)
{
    Point3 prod;
    prod.x = t*p.x;
    prod.y = t*p.y;
    prod.z = t*p.z;
    return prod;
}

Point3 operator* (const Point3& p, double t)
{
    Point3 prod;
    prod.x = t*p.x;
    prod.y = t*p.y;
    prod.z = t*p.z;
    return prod;
}

Point3 operator- (const Point3& p)
{
    Point3 neg;
    neg.x = -p.x;
    neg.y = -p.y;
    neg.z = -p.z;
    return neg;
}

// Method: instance
// provides access to the unique model for this execution.
// sets up this instance on first access
AnalyticGeometryTool* AnalyticGeometryTool::instance()
{
  if (instance_ == 0) {
    instance_ = new AnalyticGeometryTool;
  }
  return instance_;
}

AnalyticGeometryTool::AnalyticGeometryTool() 
{
   agtEpsilon = 1e-8;
}

AnalyticGeometryTool::~AnalyticGeometryTool()
{
}

//***************************************************************************
// Double numbers
//***************************************************************************


void AnalyticGeometryTool::round_near_val( double &val )
{
   if( dbl_eq( val, 0.0 ) )
      val = 0.0;
   else if( dbl_eq( val, 1.0 ) )
      val  = 1.0;
   else if( dbl_eq( val, -1.0 ) )
      val = -1.0;
}

//***************************************************************************
// Matrices & Transforms
//***************************************************************************
void AnalyticGeometryTool::transform_pnt( double m[4][4], 
                                         double pin[3],
                                         double pout[3] )
{
    double p[3];  // working buffer
    
    // Check if transformation can occur 
    if (!m) {
       if (pin && pout)
          copy_pnt(pin, pout);
       return;
    }
    
    // Perform transformation    
    p[0] = m[0][0] * pin[0] + m[1][0] * pin[1] + m[2][0] * pin[2] + m[3][0];
    p[1] = m[0][1] * pin[0] + m[1][1] * pin[1] + m[2][1] * pin[2] + m[3][1];
    p[2] = m[0][2] * pin[0] + m[1][2] * pin[1] + m[2][2] * pin[2] + m[3][2];
    
    // Copy work buffer to out point    
    copy_pnt(p,pout);
} 

void AnalyticGeometryTool::transform_vec( double m3[3][3],
                                         double vin[3],
                                         double vout[3] )
{
   double v[3];    // working buffer
   
   // Determine if transformation can occur 
   if (!m3) {      
      if (vin && vout)
         copy_vec(vin, vout);
      return;
   }
   
   // Perform transformation 
   v[0] = m3[0][0] * vin[0] + m3[1][0] * vin[1] + m3[2][0] * vin[2];
   v[1] = m3[0][1] * vin[0] + m3[1][1] * vin[1] + m3[2][1] * vin[2];
   v[2] = m3[0][2] * vin[0] + m3[1][2] * vin[1] + m3[2][2] * vin[2];
   
   // Copy work buffer to vector out    
   copy_pnt(v,vout);
}

void AnalyticGeometryTool::transform_vec( double m4[4][4], 
                                         double vin[3],
                                         double vout[3] )
{
   double v[3];    // working buffer
   
   // Determine if transformation can occur 
   if (!m4) {      
      if (vin && vout)
         copy_vec(vin, vout);
      return;
   }
   
   // Perform transformation
   
   v[0] = m4[0][0] * vin[0] + m4[1][0] * vin[1] + m4[2][0] * vin[2];
   v[1] = m4[0][1] * vin[0] + m4[1][1] * vin[1] + m4[2][1] * vin[2];
   v[2] = m4[0][2] * vin[0] + m4[1][2] * vin[1] + m4[2][2] * vin[2];
   
   // Copy work buffer to vector out    
   copy_pnt(v,vout);
}

void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
                                           double origin[3], double axis[3] )
{
   double end_pnt[3]; // Find arbitrary end point on line
   next_pnt( origin, axis, 10.0, end_pnt );
   
   transform_pnt( rot_mtx, origin, origin );
   transform_pnt( rot_mtx, end_pnt, end_pnt );
   
   axis[0] = end_pnt[0] - origin[0];
   axis[1] = end_pnt[1] - origin[1];
   axis[2] = end_pnt[2] - origin[2];
}

void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
                                          CubitVector &origin, CubitVector &axis )
{
   CubitVector end_point; // Find arbitrary end point on line
   origin.next_point( axis, 10.0, end_point );
   
   double start_pnt[3], end_pnt[3];
   copy_pnt( origin, start_pnt );
   copy_pnt( end_point, end_pnt );
   
   transform_pnt( rot_mtx, start_pnt, start_pnt );
   transform_pnt( rot_mtx, end_pnt, end_pnt );
   
   axis.x( end_pnt[0] - start_pnt[0] );
   axis.y( end_pnt[1] - start_pnt[1] );
   axis.z( end_pnt[2] - start_pnt[2] );
   
   origin.set( start_pnt );
}

void AnalyticGeometryTool::copy_mtx( double from[3][3],double to[3][3] )
{   
   // Determine if identity matrix needed 
   if (!from) // copy in the identity matrix 
      memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9); 
   else // Copy from to to 
      memcpy(to, from, sizeof(double)*9);   
}

void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[4][4] )
{   
   // determine if identity matrix needed 
   if (!from) // copy in the identity matrix 
      memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);  
   else // copy from to to 
      memcpy(to, from, sizeof(double)*16);   
}

void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[3][3] )
{   
   size_t dbl3;
   
   dbl3 = sizeof(double) * 3;
   
   // Determine if identity matrix needed 
   if (!from) // Copy in the identity matrix 
      memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9); 
   else { // Copy each upper left element of from to to 
      memcpy(to[0], from[0], dbl3);
      memcpy(to[1], from[1], dbl3);
      memcpy(to[2], from[2], dbl3);
   }   
}

void AnalyticGeometryTool::copy_mtx(double from[3][3], double to[4][4],
                                    double* origin )
{   
   size_t dbl3;
   
   dbl3 = sizeof(double) * 3;
   
   // Determine if identity matrix needed 
   if (!from) { // Copy in the identity matrix 
   
      memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
      
      if (origin)
         memcpy(to[3], origin, dbl3);
   }   
             
   else { // Copy each upper element of from to to 
      
      memcpy(to[0], from[0], dbl3);
      memcpy(to[1], from[1], dbl3);
      memcpy(to[2], from[2], dbl3);
      
      to[0][3] = 0.0;
      to[1][3] = 0.0;
      to[2][3] = 0.0;
      to[3][3] = 1.0;
      
      if (origin)
      {
         memcpy(to[3], origin, dbl3);
//         to[3][0] = origin[0];
//         to[3][1] = origin[1];
//         to[3][2] = origin[2];
      }
      else
         memcpy(to[3], AGT_IDENTITY_MTX_4X4[3], sizeof(double)*3);
   } 
}

void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
                                               double mtx3x3[3][3] )
{        
   double coeff1;
   double coeff2;
   double coeff3;
   double v_unit[3];
   
   if (!mtx3x3)
      return;
   
   coeff1 = cos(theta);
   coeff2 = (1.0l - coeff1);
   coeff3 = sin(theta);
   
   unit_vec(v, v_unit);
   
   mtx3x3[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
   mtx3x3[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
   mtx3x3[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
   
   mtx3x3[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
   mtx3x3[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
   mtx3x3[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
   
   mtx3x3[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
   mtx3x3[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
   mtx3x3[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
}

void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
                                               double mtx4x4[4][4] )
{        
   double coeff1;
   double coeff2;
   double coeff3;
   double v_unit[3];
   
   if (!mtx4x4)
      return;
   
   coeff1 = cos(theta);
   coeff2 = (1.0l - coeff1);
   coeff3 = sin(theta);

   unit_vec(v, v_unit);
   
   mtx4x4[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
   mtx4x4[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
   mtx4x4[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
   
   mtx4x4[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
   mtx4x4[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
   mtx4x4[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
   
   mtx4x4[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
   mtx4x4[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
   mtx4x4[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
   
   mtx4x4[0][3] = 0.0;
   mtx4x4[1][3] = 0.0;
   mtx4x4[2][3] = 0.0;
   mtx4x4[3][3] = 1.0;
   mtx4x4[3][0] = 0.0;
   mtx4x4[3][1] = 0.0;
   mtx4x4[3][2] = 0.0;  
}

void AnalyticGeometryTool::add_origin_to_rotation_mtx( double rot_mtx[4][4], 
                                                      double origin[3] )
{  
   double tmp_mtx[4][4];

   // Translate to origin
   double t[4][4]; 
   memcpy(t, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
   //PRINT_INFO( "Rotation matrix, before origin: \n" );
   //print_mtx( rot_mtx );
   t[3][0]=-origin[0]; t[3][1]=-origin[1]; t[3][2]=-origin[2];
   mult_mtx( t, rot_mtx, tmp_mtx );
   
   //PRINT_INFO( "Origin times rotation: \n" );
   //print_mtx( tmp_mtx );
   
   // Translate back
   t[3][0]=origin[0]; t[3][1]=origin[1]; t[3][2]=origin[2];
   mult_mtx( tmp_mtx, t, rot_mtx );
   
   //PRINT_INFO( "Rotation x -origin: \n" );
   //print_mtx( rot_mtx );
}

void AnalyticGeometryTool::identity_mtx( double mtx3x3[3][3] )
{ 
   memcpy(mtx3x3, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
}

void AnalyticGeometryTool::identity_mtx( double mtx4x4[4][4] )
{ 
   memcpy(mtx4x4, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
}

void AnalyticGeometryTool::mtx_to_angs( double mtx3x3[3][3],
                                       double &ax, double &ay,
                                       double &az )
{   
//   METHOD:
//   o Rotate x-vector onto xz plane
//   o  Check xp dotted into y  
//   o   If xp dot y is zero  ==> az = 0 (x-vector already in xz plane)
//   o   Otherwise, compute rotation of vector into xz plane to acquire *az     
//   o    Use atan2 (on x-vector) to get *az
//   o    Rotate the system about z 
//   o Use atan2 function (on x-vector in xz plane) to determine *ay 
//   o Rotate the system about y      
//   o Compute ax using y-vector
//   o Resultant angles are negated (to reverse above procedure)
 
   double x[3];                    // x-axis vector 
   double y[3];                    // y-axis vector 
   double z[3];                    // z-axis vector 
   double ar[3][3];                // Rotation matrix 
   double sinr,cosr;               // Used for atan2 function 
   double work_sys[3][3];          // Temporary holder for system 
   double *xp = work_sys[0];       // x-axis vector of system: x-primed 
   double *yp = work_sys[1];       // y-axis vector of system: y-primed 
   
   x[0] = 1.0; x[1] = 0.0; x[2] = 0.0;
   y[0] = 0.0; y[1] = 1.0; y[2] = 0.0;
   z[0] = 0.0; z[1] = 0.0; z[2] = 1.0;
   
   if (!mtx3x3)
      return;
   
   // Copy matrix over to work csys 
   copy_mtx(mtx3x3,work_sys);
      
   // Check xp dotted into y 
   //   If xp dot y is zero  ==> az = 0 
   
   // Otherwise, compute rotation of vector into xz plane to acquire *az 
   
   if (dbl_eq(dot_vec(xp,y), 0.0))
      
      az = 0.0;
         
   else {
      
      /* 
      Compute *az - rotate xp to xz-plane about z-axis          
               y    xp                                           
               |  /                                              
               | /  negative angle about z (negative of atan2)   
                -----x              (use RH rule about z)        
                 \                                               
                  \ positive angle about z (negative of atan2)         
                   xp                                            
      */
         
      sinr = dot_vec(xp,y);
      cosr = dot_vec(xp,x);
      az = - atan2(sinr, cosr);
      
      // Rotate the system about z 
      create_rotation_mtx(az,z,ar);
      rotate_system(ar,work_sys,work_sys);
      
   }
   
      /* 
      Compute *ay - rotate xp to x-axis about y-axis            
              z    xp                                           
              |  /                                              
              | /  positive angle about y (positive of atan2)   
               -----x     (use RH rule about y)                 
                \                                               
                 \ negative angle about y (positive of atan2)   
                  xp                                            
      */
   
   sinr = dot_vec(xp,z);
   cosr = dot_vec(xp,x);
   ay = atan2(sinr, cosr);
   
   // Rotate the system about y       
   create_rotation_mtx(ay,y,ar);      
   rotate_system(ar,work_sys,work_sys);
  
   /*
      Compute *ax - rotate yp to y-axis about x-axis            
              z    yp                                           
              |  /                                              
              | /  negative angle about x (negative of atan2)   
               -----y     (use RH rule about x)                 
                \                                               
                 \ positive angle about x (negative of atan2)   
                  yp                                            
   */
   
   sinr = dot_vec(yp,z);
   cosr = dot_vec(yp,y);
   ax = atan2(sinr,cosr);     // Negative of negative - see below 
   
   // Negate above angles for rotation of the system back to original 
   az = -(az);
   ay = -(ay);
   
   // Make sure near zero angles are actually zero 
   if (dbl_eq(ax, 0.0))
      ax = 0.0;
   
   if (dbl_eq(ay, 0.0))
      ay = 0.0;   
}

void AnalyticGeometryTool::mtx_to_angs( double mtx4x4[4][4],
                                       double &ax, double &ay,
                                       double &az )
{   
   double work_sys[3][3];

   if(!mtx4x4)
      return;

   copy_mtx(mtx4x4,work_sys);
   mtx_to_angs( work_sys, ax, ay, az );
}

void AnalyticGeometryTool::rotate_system( double mtx[3][3], 
                                         double sys_in[3][3],
                                         double sys_out[3][3] )
{
   double sys_tmp[3][3];
   double *p_sys_tmp;
   
   // Check to see if rotating in place 
   if (sys_in == sys_out) {
      copy_mtx( sys_in, sys_tmp );
      p_sys_tmp = (double *)sys_tmp; 
   }
   else 
      // Have sys_tmp point at outgoing memory location 
      p_sys_tmp = (double *)sys_out;
   
   
   // X-vector 
   p_sys_tmp[0] =   mtx[0][0] * sys_in[0][0]
                  + mtx[1][0] * sys_in[0][1]
                  + mtx[2][0] * sys_in[0][2];
   p_sys_tmp[1] =   mtx[0][1] * sys_in[0][0]
                  + mtx[1][1] * sys_in[0][1]
                  + mtx[2][1] * sys_in[0][2];
   p_sys_tmp[2] =   mtx[0][2] * sys_in[0][0]
                  + mtx[1][2] * sys_in[0][1]
                  + mtx[2][2] * sys_in[0][2];   
   
   // Y-vector 
   p_sys_tmp[3] =   mtx[0][0] * sys_in[1][0]
                  + mtx[1][0] * sys_in[1][1]
                  + mtx[2][0] * sys_in[1][2];
   p_sys_tmp[4] =   mtx[0][1] * sys_in[1][0]
                  + mtx[1][1] * sys_in[1][1]
                  + mtx[2][1] * sys_in[1][2];
   p_sys_tmp[5] =   mtx[0][2] * sys_in[1][0]
                  + mtx[1][2] * sys_in[1][1]
                  + mtx[2][2] * sys_in[1][2];
   
   // Z-vector 
   p_sys_tmp[6] =   mtx[0][0] * sys_in[2][0]
                  + mtx[1][0] * sys_in[2][1]
                  + mtx[2][0] * sys_in[2][2];
   p_sys_tmp[7] =   mtx[0][1] * sys_in[2][0]
                  + mtx[1][1] * sys_in[2][1]
                  + mtx[2][1] * sys_in[2][2];
   p_sys_tmp[8] =   mtx[0][2] * sys_in[2][0]
                  + mtx[1][2] * sys_in[2][1]
                  + mtx[2][2] * sys_in[2][2];
   
    // Copy sys_tmp to sys_out if rotating in place 
   if (sys_in == sys_out) {
      memcpy(sys_out, sys_tmp, sizeof(double)*9); 
   }
}

void AnalyticGeometryTool::rotate_system( double mtx[4][4], 
                                         double sys_in[4][4],
                                         double sys_out[4][4] )
{
   double sys_tmp[4][4];
   double* p_sys_tmp;
   
   // Check to see if rotating in place 
   if (sys_in == sys_out) {
      copy_mtx( sys_in, sys_tmp );
      p_sys_tmp = (double *)sys_tmp; 
   }
   else 
      // Have p_sys_tmp point at outgoing memory location 
      p_sys_tmp = (double *)sys_out;
   
   
   // X-vector 
   p_sys_tmp[0] =   mtx[0][0] * sys_in[0][0]
                  + mtx[1][0] * sys_in[0][1]
                  + mtx[2][0] * sys_in[0][2];
   p_sys_tmp[1] =   mtx[0][1] * sys_in[0][0]
                  + mtx[1][1] * sys_in[0][1]
                  + mtx[2][1] * sys_in[0][2];
   p_sys_tmp[2] =   mtx[0][2] * sys_in[0][0]
                  + mtx[1][2] * sys_in[0][1]
                  + mtx[2][2] * sys_in[0][2];   
   
   // Y-vector 
   p_sys_tmp[4] =   mtx[0][0] * sys_in[1][0]
                  + mtx[1][0] * sys_in[1][1]
                  + mtx[2][0] * sys_in[1][2];
   p_sys_tmp[5] =   mtx[0][1] * sys_in[1][0]
                  + mtx[1][1] * sys_in[1][1]
                  + mtx[2][1] * sys_in[1][2];
   p_sys_tmp[6] =   mtx[0][2] * sys_in[1][0]
                  + mtx[1][2] * sys_in[1][1]
                  + mtx[2][2] * sys_in[1][2];
   
   // Z-vector 
   p_sys_tmp[8] =   mtx[0][0] * sys_in[2][0]
                  + mtx[1][0] * sys_in[2][1]
                  + mtx[2][0] * sys_in[2][2];
   p_sys_tmp[9] =   mtx[0][1] * sys_in[2][0]
                  + mtx[1][1] * sys_in[2][1]
                  + mtx[2][1] * sys_in[2][2];
   p_sys_tmp[10] =  mtx[0][2] * sys_in[2][0]
                  + mtx[1][2] * sys_in[2][1]
                  + mtx[2][2] * sys_in[2][2];
   
   // Maintain the origin 
   p_sys_tmp[3] = sys_in[0][3];
   p_sys_tmp[7] = sys_in[1][3];
   p_sys_tmp[11] = sys_in[2][3];
   p_sys_tmp[15] = sys_in[3][3];   
   p_sys_tmp[12] = sys_in[3][0];
   p_sys_tmp[13] = sys_in[3][1];    
   p_sys_tmp[14] = sys_in[3][2];   
   
    // Copy sys_tmp to sys_out if rotating in place 
   if (sys_in == sys_out) {
      memcpy(sys_out, sys_tmp, sizeof(double)*16);
   }
}

double AnalyticGeometryTool::det_mtx( double m[3][3] ) 
{
   double determinant;
   
   if (!m)
      return (0.0);
   
   determinant = m[0][0]*(m[1][1]*m[2][2]-m[1][2]*m[2][1])
               + m[0][1]*(m[1][2]*m[2][0]-m[1][0]*m[2][2])
               + m[0][2]*(m[1][0]*m[2][1]-m[1][1]*m[2][0]);
   
   return (determinant);
}  

void AnalyticGeometryTool::mult_mtx( double a[3][3],double b[3][3], 
                                    double d[3][3] ) 
{       
   double c[3][3];    // working buffer 
      
   if( a != 0 && b != 0 ) {   // a & b are valid 
            
      c[0][0] = (  a[0][0] * b[0][0] + a[0][1] * b[1][0] 
                 + a[0][2] * b[2][0]);
      c[1][0] = (  a[1][0] * b[0][0] + a[1][1] * b[1][0] 
                 + a[1][2] * b[2][0]);
      c[2][0] = (  a[2][0] * b[0][0] + a[2][1] * b[1][0] 
                 + a[2][2] * b[2][0]);
      
      c[0][1] = (  a[0][0] * b[0][1] + a[0][1] * b[1][1] 
                 + a[0][2] * b[2][1]);
      c[1][1] = (  a[1][0] * b[0][1] + a[1][1] * b[1][1] 
                 + a[1][2] * b[2][1]);
      c[2][1] = (  a[2][0] * b[0][1] + a[2][1] * b[1][1] 
                 + a[2][2] * b[2][1]);
      
      c[0][2] = (  a[0][0] * b[0][2] + a[0][1] * b[1][2] 
                 + a[0][2] * b[2][2]);
      c[1][2] = (  a[1][0] * b[0][2] + a[1][1] * b[1][2]
                 + a[1][2] * b[2][2]);
      c[2][2] = (  a[2][0] * b[0][2] + a[2][1] * b[1][2]
                 + a[2][2] * b[2][2]);
      
      copy_mtx(c, d);
      
   }
   else if (a) {                 // b equals 0     
      copy_mtx(a, d);    
   }
   else if (b) {                  // a equals 0 
      copy_mtx(b, d);    
   }
   else {                         // a & b equal 0 
      
      copy_mtx(AGT_IDENTITY_MTX_3X3, d);         
   }
}

void AnalyticGeometryTool::mult_mtx( double a[4][4], 
                                    double b[4][4],
                                    double d[4][4] ) 
{       
   double c[4][4];    // working buffer 
      
   if( a != 0 && b != 0 ) {   // a & b are valid 
            
      c[0][0] = (  a[0][0] * b[0][0] + a[0][1] * b[1][0] 
                 + a[0][2] * b[2][0] + a[0][3] * b[3][0]);
      c[1][0] = (  a[1][0] * b[0][0] + a[1][1] * b[1][0] 
                 + a[1][2] * b[2][0] + a[1][3] * b[3][0]);
      c[2][0] = (  a[2][0] * b[0][0] + a[2][1] * b[1][0] 
                 + a[2][2] * b[2][0] + a[2][3] * b[3][0]);
      c[3][0] = (  a[3][0] * b[0][0] + a[3][1] * b[1][0] 
                 + a[3][2] * b[2][0] + a[3][3] * b[3][0]);
      
      c[0][1] = (  a[0][0] * b[0][1] + a[0][1] * b[1][1] 
                 + a[0][2] * b[2][1] + a[0][3] * b[3][1]);
      c[1][1] = (  a[1][0] * b[0][1] + a[1][1] * b[1][1] 
                 + a[1][2] * b[2][1] + a[1][3] * b[3][1]);
      c[2][1] = (  a[2][0] * b[0][1] + a[2][1] * b[1][1] 
                 + a[2][2] * b[2][1] + a[2][3] * b[3][1]);
      c[3][1] = (  a[3][0] * b[0][1] + a[3][1] * b[1][1] 
                 + a[3][2] * b[2][1] + a[3][3] * b[3][1]);
      
      c[0][2] = (  a[0][0] * b[0][2] + a[0][1] * b[1][2] 
                 + a[0][2] * b[2][2] + a[0][3] * b[3][2]);
      c[1][2] = (  a[1][0] * b[0][2] + a[1][1] * b[1][2]
                 + a[1][2] * b[2][2] + a[1][3] * b[3][2]);
      c[2][2] = (  a[2][0] * b[0][2] + a[2][1] * b[1][2]
                 + a[2][2] * b[2][2] + a[2][3] * b[3][2]);
      c[3][2] = (  a[3][0] * b[0][2] + a[3][1] * b[1][2]
                 + a[3][2] * b[2][2] + a[3][3] * b[3][2]);
      
      c[0][3] = (  a[0][0] * b[0][3] + a[0][1] * b[1][3]
                 + a[0][2] * b[2][3] + a[0][3] * b[3][3]);
      c[1][3] = (  a[1][0] * b[0][3] + a[1][1] * b[1][3]
                 + a[1][2] * b[2][3] + a[1][3] * b[3][3]);
      c[2][3] = (  a[2][0] * b[0][3] + a[2][1] * b[1][3]
                 + a[2][2] * b[2][3] + a[2][3] * b[3][3]);
      c[3][3] = (  a[3][0] * b[0][3] + a[3][1] * b[1][3]
                 + a[3][2] * b[2][3] + a[3][3] * b[3][3]);
      
      copy_mtx(c, d);            
   }
   else if (a) {                 // b equals 0     
      copy_mtx(a, d);    
   }
   else if (b) {                  // a equals 0 
      copy_mtx(b, d);    
   }
   else {                         // a & b equal 0 
      
      copy_mtx(AGT_IDENTITY_MTX_4X4, d);         
   }
}

CubitStatus AnalyticGeometryTool::inv_mtx_adj( double mtx[3][3],
                                              double inv_mtx[3][3] )
{
   int i,i1,i2,j,j1,j2;
   double work_mtx[3][3];
   double determinant;
   
   // Check for null input 
   if (!mtx) {
      copy_mtx(AGT_IDENTITY_MTX_3X3, inv_mtx);
      return CUBIT_SUCCESS;
   }      
   
   // Calculate determinant 
   determinant = det_mtx(mtx);
   
   // Check for singularity 
   if (dbl_eq(determinant,0.0))
      return CUBIT_FAILURE;
   
   // Get work matrix (allow inverting in place) 
   copy_mtx(mtx, work_mtx);
   
   // Inverse is adjoint matrix divided by determinant 
   for (i=1; i<4; i++) {
      
      i1 = (i % 3) + 1;  i2 = (i1 % 3) + 1;
      
      for (j=1; j<4; j++) {
         
         j1 = (j % 3) + 1;  j2 = (j1 % 3) + 1;
         
         inv_mtx[j-1][i-1] = (work_mtx[i1-1][j1-1]*work_mtx[i2-1][j2-1] - 
            work_mtx[i1-1][j2-1]*work_mtx[i2-1][j1-1]) 
            / determinant;
         
      }
   }
   return CUBIT_SUCCESS;
}

CubitStatus AnalyticGeometryTool::inv_trans_mtx( double transf[4][4], 
                                                double inv_transf[4][4] )
{
   double scale_sq;
   double inv_sq_scale;
   double tmp_transf[4][4]; // For temporary storage of incoming matrix
   double *p_tmp_transf = NULL;

   // If input transform is 0 set output to identity matrix
   if (!transf) {
      copy_mtx( AGT_IDENTITY_MTX_4X4, inv_transf );
      return CUBIT_SUCCESS;
   }
   
   // Obtain the matrix scale
   scale_sq = transf[0][0]*transf[0][0] + transf[0][1]*transf[0][1] +
              transf[0][2]*transf[0][2];
   
   // Check for singular matrix
   if (scale_sq < (.000000001 * .000000001))
      return CUBIT_FAILURE;
   
   // Need the inverse scale squared
   inv_sq_scale = 1.0 / scale_sq;   
   
   // Check to see if inverting in place
   if (transf == inv_transf) {
      copy_mtx( transf, tmp_transf );
      p_tmp_transf = (double *)tmp_transf;
   }
   else
      p_tmp_transf = (double *)transf;
   
   // The X vector
   inv_transf[0][0] = p_tmp_transf[0] * inv_sq_scale;
   inv_transf[1][0] = p_tmp_transf[1] * inv_sq_scale;
   inv_transf[2][0] = p_tmp_transf[2] * inv_sq_scale;
   
   // The Y vector
   inv_transf[0][1] = p_tmp_transf[4] * inv_sq_scale;
   inv_transf[1][1] = p_tmp_transf[5] * inv_sq_scale;
   inv_transf[2][1] = p_tmp_transf[6] * inv_sq_scale;
   
   // The Z vector 
   inv_transf[0][2] = p_tmp_transf[8] * inv_sq_scale;
   inv_transf[1][2] = p_tmp_transf[9] * inv_sq_scale;
   inv_transf[2][2] = p_tmp_transf[10] * inv_sq_scale;
   
   // Column 4 
   inv_transf[0][3] = 0.0;
   inv_transf[1][3] = 0.0;
   inv_transf[2][3] = 0.0;
   
   // The X origin 
   inv_transf[3][0] = -inv_sq_scale * (  p_tmp_transf[0] * p_tmp_transf[12]
                                       + p_tmp_transf[1] * p_tmp_transf[13]
                                       + p_tmp_transf[2] * p_tmp_transf[14]);
   
   // The Y origin 
   inv_transf[3][1] = -inv_sq_scale * (  p_tmp_transf[4] * p_tmp_transf[12]
                                       + p_tmp_transf[5] * p_tmp_transf[13]
                                       + p_tmp_transf[6] * p_tmp_transf[14]);
   
   // The Z origin 
   inv_transf[3][2] = -inv_sq_scale * (  p_tmp_transf[8] * p_tmp_transf[12]
                                       + p_tmp_transf[9] * p_tmp_transf[13]
                                       + p_tmp_transf[10] * p_tmp_transf[14]);
   
   // This is always one 
   inv_transf[3][3] = 1.0;
   
   return CUBIT_SUCCESS;
}

void AnalyticGeometryTool::vecs_to_mtx( double xvec[3], 
                                       double yvec[3],
                                       double zvec[3],
                                       double matrix[3][3] )
{      
   if (xvec) 
      copy_pnt(xvec, matrix[0]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
   
   if (yvec)
      copy_pnt(yvec, matrix[1]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
   
   if (zvec)
      copy_pnt(zvec, matrix[2]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
}   

void AnalyticGeometryTool::vecs_to_mtx( double xvec[3], 
                                       double yvec[3],
                                       double zvec[3], 
                                       double origin[3],
                                       double matrix[4][4] )
{      
   if (xvec) 
      copy_pnt(xvec, matrix[0]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
   
   if (yvec)
      copy_pnt(yvec, matrix[1]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
   
   if (zvec)
      copy_pnt(zvec, matrix[2]);
   else
      copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
   
   if( origin )
      copy_pnt(origin, matrix[3]);
   else
   {
      matrix[3][0] = 0.0;
      matrix[3][1] = 0.0;
      matrix[3][2] = 0.0;
   }

   matrix[0][3] = 0.0;
   matrix[1][3] = 0.0;
   matrix[2][3] = 0.0;
   matrix[3][3] = 1.0;    
}

void AnalyticGeometryTool::print_mtx( double mtx[3][3] )
{
   PRINT_INFO( "%f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2] );
   PRINT_INFO( "%f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2] );
   PRINT_INFO( "%f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2] );
}

void AnalyticGeometryTool::print_mtx( double mtx[4][4] )
{
   PRINT_INFO( "%f %f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2], mtx[0][3] );
   PRINT_INFO( "%f %f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2], mtx[1][3] );
   PRINT_INFO( "%f %f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2], mtx[2][3] );
   PRINT_INFO( "%f %f %f %f\n", mtx[3][0], mtx[3][1], mtx[3][2], mtx[3][3] );
}

//***************************************************************************
// 3D Points
//***************************************************************************
void AnalyticGeometryTool::copy_pnt( double pnt_in[3], double pnt_out[3] )
{   
   if (pnt_in == pnt_out)
      return;
   
   if (pnt_out == NULL)
      return;
   
   if (pnt_in == NULL) {
      pnt_out[0] = 0.0;
      pnt_out[1] = 0.0;
      pnt_out[2] = 0.0;
      return;
   }
   
   // Simply copy first point into second point    
   memcpy(pnt_out, pnt_in, sizeof(double)*3);
}

void AnalyticGeometryTool::copy_pnt( double pnt_in[3], CubitVector &cubit_vec )
{
   cubit_vec.set( pnt_in );
}
   
void AnalyticGeometryTool::copy_pnt( CubitVector &cubit_vec, double pnt_out[3] )
{
   pnt_out[0] = cubit_vec.x();
   pnt_out[1] = cubit_vec.y();
   pnt_out[2] = cubit_vec.z();
}


CubitBoolean AnalyticGeometryTool::pnt_eq( double pnt1[3],double pnt2[3] )
{
   double x = pnt2[0] - pnt1[0];  // difference in the x direction 
   double y = pnt2[1] - pnt1[1];  // difference in the y direction 
   double z = pnt2[2] - pnt1[2];  // difference in the z direction 
  
   return (dbl_eq(sqrt(x*x + y*y + z*z), 0.0));
}


CubitStatus AnalyticGeometryTool::mirror_pnt( double pnt[3], 
                                             double pln_orig[3],
                                             double pln_norm[3],
                                             double m_pnt[3])
{
   double int_pnt[3],
      vec[3];
   
   // Find intersection of point and plane 
   if (int_pnt_pln(pnt, pln_orig, pln_norm, int_pnt)) {
      // If intersection is on the plane, return 
      copy_pnt(pnt, m_pnt);
      return CUBIT_FAILURE;
   }
   
   // Find vector from pnt to int_pnt 
   get_vec(pnt, int_pnt, vec);
   
   // Traverse twice the length of vec in vec direction 
   m_pnt[0] = pnt[0] + 2.0 * vec[0];
   m_pnt[1] = pnt[1] + 2.0 * vec[1];
   m_pnt[2] = pnt[2] + 2.0 * vec[2];
   
   return CUBIT_SUCCESS;   
}


CubitStatus AnalyticGeometryTool::next_pnt( double str_pnt[3], 
                                           double vec_dir[3], 
                                           double len,
                                           double new_pnt[3])
{        
   double uv[3];  // unit vector representation of vector direction 
   
   // unitize specified direction 
   if (!unit_vec(vec_dir,uv)) {
      copy_pnt(str_pnt, new_pnt);
      return CUBIT_FAILURE;
   }
   
   // determine next point in space 
   
   new_pnt[0] = str_pnt[0] + (len * uv[0]);     
   new_pnt[1] = str_pnt[1] + (len * uv[1]);     
   new_pnt[2] = str_pnt[2] + (len * uv[2]); 
   
   return CUBIT_SUCCESS;
}


//***************************************************************************
// 3D Vectors
//***************************************************************************
CubitStatus AnalyticGeometryTool::unit_vec( double vin[3], double vout[3] )
{
   double rmag; // holds magnitude of vector 
      
   // Calculate vector magnitude 
   rmag = sqrt(vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]);
   
   // check if vector has a magnitude - catch division by zero 
   
   if (dbl_eq(rmag, 0.0)) {
      if (vin != vout)
         copy_pnt(vin, vout);
      return CUBIT_FAILURE;
   }
   
   // divide each element of the vector by the magnitude 
      
   vout[0] = vin[0] / rmag;
   vout[1] = vin[1] / rmag;
   vout[2] = vin[2] / rmag;
      
   return CUBIT_SUCCESS;
}

double AnalyticGeometryTool::dot_vec( double uval[3], double vval[3] )
{
   double dot_val;
   
   // perform dot calculation = v[0]*u[0] + v[1]*u[1] + v[1]*u[1] 
            
   dot_val = uval[0]*vval[0] + uval[1]*vval[1] + uval[2]*vval[2];
      
   return(dot_val);   
}

void AnalyticGeometryTool::cross_vec( double uval[3], double vval[3],
                                     double cross[3] )
{      
   // determine cross product of the two vectors 
   
   cross[0] = uval[1] * vval[2] - uval[2] * vval[1];
   cross[1] = uval[2] * vval[0] - uval[0] * vval[2];
   cross[2] = uval[0] * vval[1] - uval[1] * vval[0];   
}

void AnalyticGeometryTool::cross_vec_unit( double uval[3], double vval[3],
                                          double cross[3] )
{      
   // determine cross product of the two vectors 
   cross_vec(uval,vval,cross);
   
   // convert to unit vector 
   unit_vec(cross,cross);
}

void AnalyticGeometryTool::orth_vecs( double uvect[3], double vvect[3], 
                                     double wvect[3] )
{
   double x[3];
   unsigned short i = 0;
   unsigned short imin = 3;
   double rmin = 1.0E20;
   unsigned short iperm1[3];
   unsigned short iperm2[3];
   unsigned short cont_flag = 1;
   double vec[3];
   
   // Initialize perm flags
   iperm1[0] = 1; iperm1[1] = 2; iperm1[2] = 0;
   iperm2[0] = 2; iperm2[1] = 0; iperm2[2] = 1;
   
   // unitize vector 
   
   unit_vec(uvect,vec);
   
   while (i<3 && cont_flag ) {
      if (dbl_eq(vec[i], 0.0)) {
         vvect[i] = 1.0;
         vvect[iperm1[i]] = 0.0;
         vvect[iperm2[i]] = 0.0;
         cont_flag = 0;
      }
      
      if (fabs(vec[i]) < rmin) {
         imin = i;
         rmin = fabs(vec[i]);
      }
      ++i;
   }
   
   if (cont_flag) {
      x[imin] = 1.0;
      x[iperm1[imin]] = 0.0;
      x[iperm2[imin]] = 0.0;
      
      // determine cross product 
      cross_vec_unit(vec,x,vvect);
   }
   
   // cross vector to determine last orthogonal vector 
   cross_vec(vvect,vec,wvect);
}

double AnalyticGeometryTool::mag_vec( double vec[3] )
{
   return (sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]));
}

CubitStatus AnalyticGeometryTool::get_vec ( double str_pnt[3], 
                                           double stp_pnt[3],
                                           double vector_out[3] )
{  
   // Make sure we can create a vector 
   if (pnt_eq(str_pnt, stp_pnt)) {
      vector_out[0] = 0.0;
      vector_out[1] = 0.0;
      vector_out[2] = 0.0;
      return CUBIT_FAILURE;
   }
   
   // determine vector by subtracting starting point from stopping point 
   
  vector_out[0] = stp_pnt[0] - str_pnt[0];
  vector_out[1] = stp_pnt[1] - str_pnt[1];
  vector_out[2] = stp_pnt[2] - str_pnt[2];  
  
  return CUBIT_SUCCESS;
}

CubitStatus AnalyticGeometryTool::get_vec_unit( double str_pnt[3], 
                                               double stp_pnt[3],
                                               double uv_out[3] )
{
  // determine vector between points 
   if (!get_vec(str_pnt,stp_pnt,uv_out))
      return CUBIT_FAILURE;
  
  // unitize vector 
  if (!unit_vec(uv_out,uv_out))
     return CUBIT_FAILURE;
     
  return CUBIT_SUCCESS;  
}

void AnalyticGeometryTool::mult_vecxconst( double constant,
                                          double vec[3],
                                          double vec_out[3] )
{
   // multiply each element of the vector by the constant
   vec_out[0] = constant * vec[0];
   vec_out[1] = constant * vec[1];
   vec_out[2] = constant * vec[2];   
}


void AnalyticGeometryTool::reverse_vec( double vin[3],double vout[3] )
{
   // Multiply the vector components by a -1.0    
   mult_vecxconst(-1.0, vin, vout);      
}

double AnalyticGeometryTool::angle_vec_vec( double v1[3],double v2[3] )
{   
  double denom, dot, cosang, sinang, acrsb, angle;
  double crossed_vec[3];
     
  // For accuracy, use cosine for large angles, sine for small angles 
  denom = sqrt((v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2])
              *(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));
  
  // Check for a zero length vector 
  if (dbl_eq(denom, 0.0))
     return (0.0);
  
  dot = dot_vec(v1, v2);
  
  cosang = dot/denom;
  
  if (1.0 - fabs(cosang) < 0.01) {
     cross_vec(v1, v2, crossed_vec);
     acrsb = mag_vec(crossed_vec);
     sinang = acrsb/denom;
     if (cosang > 0.0) 
        angle = asin(sinang);
     else 
        angle = AGT_PI - asin(sinang);
  }
  else 
     angle = acos(cosang);
  
  return (angle); 
}

//***************************************************************************
// Distances
//***************************************************************************
double AnalyticGeometryTool::dist_pnt_pnt( double pnt1[3], double pnt2[3] )
{  
  double x = pnt2[0] - pnt1[0];  // difference in the x direction
  double y = pnt2[1] - pnt1[1];  // difference in the y direction
  double z = pnt2[2] - pnt1[2];  // difference in the z direction
  
  // return the distance   
  return(sqrt(x*x + y*y + z*z));
}



double AnalyticGeometryTool::dist_pln_pln( double pln_1_orig[3],
                                          double pln_1_norm[3], 
                                          double pln_2_orig[3],
                                          double pln_2_norm[3],
                                          AgtSide *side,
                                          AgtOrientation *orien,
                                          unsigned short *status )   
{
   double distance;
   double int_pnt[3];
   double vec[3];
   
   // Check to see if planes are parallel
   if (is_vec_par(pln_1_norm, pln_2_norm)) {    
      
      // Set successful status
      if (status)
         *status = 1;
      
      // Calculate perpendicular line plane intersection on plane_2 from 
      // pln_1_origin 
      int_ln_pln(pln_1_orig, pln_1_norm, pln_2_orig, pln_2_norm, 
                 int_pnt);
      
      // Find distance between pln_1_origin and this intersection pnt
      distance = dist_pnt_pnt(pln_1_orig, int_pnt);
      
      // Get side if required
      if (side) {
         
         if (dbl_eq(distance, 0.0))
            
            *side = AGT_ON_PLANE;
         
         else {
            
            // Get vector to intersection point
            get_vec(pln_1_orig, int_pnt, vec);
            
            // Compare angles
            if (dbl_eq(angle_vec_vec(vec, pln_1_norm), 0.0))
               *side = AGT_POS_SIDE;
            else
               *side = AGT_NEG_SIDE;
            
         }
      }
      
      // Get orientation if required
      if (orien) {
         
         // Compare surface normals
         if (dbl_eq(angle_vec_vec(pln_1_norm, pln_2_norm), 0.0))
            *orien = AGT_SAME_DIR;
         else
            *orien = AGT_OPP_DIR;
      }      
      
   }
   
   else {
      
      if (status)
         *status = 0;
      
      if (side)
         *side = AGT_CROSS;
      
      distance = 0.0;
      
   }
   
   return (distance);
}



//***************************************************************************
// Intersections
//***************************************************************************
CubitStatus AnalyticGeometryTool::int_ln_pln( double ln_orig[3],
                                             double ln_vec[3],
                                             double pln_orig[3],
                                             double pln_norm[3],
                                             double int_pnt[3] )
{   
   double denom;
   double t;
   
   // Set parametric eqns of line equal to parametric eqn of plane & solve
   // for t 
   denom = pln_norm[0]*ln_vec[0] + pln_norm[1]*ln_vec[1] + 
           pln_norm[2]*ln_vec[2];
   
   if (dbl_eq(denom, 0.0))
      return CUBIT_FAILURE;
   
   t = (pln_norm[0]*(pln_orig[0]-ln_orig[0]) + 
      pln_norm[1]*(pln_orig[1]-ln_orig[1]) + 
      pln_norm[2]*(pln_orig[2]-ln_orig[2]))/denom;
   
   // Substitute t back into equations of line to get xyz 
   int_pnt[0] = ln_orig[0] + ln_vec[0]*t;
   int_pnt[1] = ln_orig[1] + ln_vec[1]*t;
   int_pnt[2] = ln_orig[2] + ln_vec[2]*t;
      
   return CUBIT_SUCCESS;   
}

int AnalyticGeometryTool::int_ln_ln( double p1[3], double v1[3],
                                    double p2[3], double v2[3], 
                                    double int_pnt1[3], double int_pnt2[3] )
{   
   double norm[3];
   double pln1_norm[3];
   double pln2_norm[3];
   
   // Cross the two vectors to get a normal vector 
   cross_vec(v1, v2, norm);
   
   if (dbl_eq(mag_vec(norm), 0.0))
      return 0;
   
   // Cross v2 & normal to get normal to plane 2 
   cross_vec(v2, norm, pln2_norm);
   
   // Find intersection of pln2_norm and vector 1 - this is int_pnt1 
   int_ln_pln(p1, v1, p2, pln2_norm, int_pnt1);
   
   // Cross v1 & normal to get normal to plane 1 
   cross_vec(v1, norm, pln1_norm);
   
   // Find intersection of pln2_norm and vector 1 - this is int_pnt2 
   int_ln_pln(p2, v2, p1, pln1_norm, int_pnt2);

   // Check to see if the intersection points are the same & return 
   if (pnt_eq(int_pnt1, int_pnt2)) 
      return 1;
   else
      return 2;   
}


int AnalyticGeometryTool::int_pnt_pln( double pnt[3],
                                      double pln_orig[3], 
                                      double pln_norm[3], 
                                      double pln_int[3] )   
{
   // Calculate line plane intersection w/plane normal as line vector 
   int_ln_pln(pnt, pln_norm, pln_orig, pln_norm, pln_int);
   
   // Check to see if point is on the plane 
   if (pnt_eq(pln_int, pnt))
      return 1;
   else
      return 0; 
}



//***************************************************************************
// Comparison/Containment Tests
//***************************************************************************
CubitBoolean AnalyticGeometryTool::is_vec_par( double vec_1[3],
                                              double vec_2[3] )
{   
   double cross[3];
   
   // Get cross product & see if its magnitude is zero
   cross_vec(vec_1, vec_2, cross);
   
   if (dbl_eq(mag_vec(cross), 0.0))
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;
}

CubitBoolean AnalyticGeometryTool::is_vec_perp( double vec_1[3],double vec_2[3])
{  
   // Check angle between vectors
   if (dbl_eq(angle_vec_vec(vec_1, vec_2), AGT_PI_DIV_2))
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;   
}

CubitBoolean AnalyticGeometryTool::is_vecs_same_dir( double vec_1[3], 
                                                     double vec_2[3] )
{         
   // Check to see if angle between vectors can be considered zero   
   if (dbl_eq(angle_vec_vec(vec_1, vec_2), 0.0))
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;   
}


CubitBoolean AnalyticGeometryTool::is_pnt_on_ln( double pnt[3],
                                                double ln_orig[3],
                                                double ln_vec[3] )
{  
   double vec[3];
   
   // Compare pnt and line origin
   if (pnt_eq(pnt, ln_orig))
      return CUBIT_TRUE;   
   
   // Get a vector from line origin to the point      
   get_vec(ln_orig, pnt, vec);
   
   // If this vector is parallel with line vector, point is on the line
   if (is_vec_par(vec, ln_vec))
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;
}

CubitBoolean AnalyticGeometryTool::is_pnt_on_ln_seg( double pnt[3],
                                                    double end1[3],
                                                    double end2[3] )
{ 
//   METHOD:
//    o Use parametric equations of line
//    
//         x = x1 + t(x2 - x1)
//         y = y1 + t(y2 - y1)
//         z = z1 + t(z2 - z1)
//       
//    o Note:  two other method's were considered: 
//       1) Comparing sum of distance of point to both end points to the
//          line length.
//       2) Checking to see if area of a triangle with the vertices is zero
//       
//     Using parametric equations is more efficient in many cases.
  double t1 = 0.0,
    t2 = 0.0,
    t3 = 0.0,
    neg_range,
    pos_range;
  
  unsigned short flg1 = 0,
    flg2 = 0,
    flg3 = 0;
  
  neg_range = 0.0 - agtEpsilon;
  pos_range = 1.0 + agtEpsilon;
  
  if (fabs(end2[0] - end1[0]) < agtEpsilon)
  {
    if (fabs(pnt[0] - end1[0]) < agtEpsilon)
      flg1 = 1;
    else
      return CUBIT_FALSE;
  }
  else
  {
    t1 = (pnt[0] - end1[0])/(end2[0] - end1[0]);
    
    if (t1<neg_range || t1>pos_range)
      return CUBIT_FALSE;
  }
  
  if (fabs(end2[1] - end1[1]) < agtEpsilon)
  {
    if (fabs(pnt[1] - end1[1]) < agtEpsilon)
      flg2 = 1;
    else
      return CUBIT_FALSE;
  }
  else
  {
    t2 = (pnt[1] - end1[1])/(end2[1] - end1[1]);
    
    if (t2<neg_range || t2>pos_range)
      return CUBIT_FALSE;
  }
   
  if (fabs(end2[2] - end1[2]) < agtEpsilon)
  {
    if (fabs(pnt[2] - end1[2]) < agtEpsilon)
      flg3 = 1;
    else
      return CUBIT_FALSE;
  }
  else
  {
    t3 = (pnt[2] - end1[2])/(end2[2] - end1[2]);
    
    if (t3<neg_range || t3>pos_range)
      return CUBIT_FALSE;
  }
  
    // If any 2 flags are 1, point is on the line,
    // otherwise, check remaining T's for equality
  
  if (flg1)
  {
      // Here, flg1 = 1
    
    if (flg2)
    {
        // Here, flg1 = 1
        // Here, flg2 = 1
      return CUBIT_TRUE;
    }
    else
    {
        // Here, flg1 = 1
        // Here, flg2 = 0
      
      if (flg3)
          // Here, flg1 = 1
          // Here, flg2 = 0
          // Here, flg3 = 1
        return CUBIT_TRUE;
      else
      {
          // Here, flg1 = 1
          // Here, flg2 = 0
          // Here, flg3 = 0
        if (dbl_eq(t2, t3))
          return CUBIT_TRUE;
        else
          return CUBIT_FALSE;
      }
    }
  }
  else
  {
      // Here, flg1 = 0
    if (flg2)
    {
        // Here, flg1 = 0
        // Here, flg2 = 1
      if (flg3)
        return CUBIT_TRUE;
          // Here, flg1 = 0
          // Here, flg2 = 1
          // Here, flg3 = 1
      else
      {
          // Here, flg1 = 0
          // Here, flg2 = 1
          // Here, flg3 = 0
        if (dbl_eq(t1, t3))
          return CUBIT_TRUE;
        else
          return CUBIT_FALSE;
      }
    }
    else
    {
        // Here, flg1 = 0
        // Here, flg2 = 0
      if (flg3)
      {
          // Here, flg1 = 0
          // Here, flg2 = 0
          // Here, flg3 = 1
        if (dbl_eq(t1, t2))
          return CUBIT_TRUE;
        else
          return CUBIT_FALSE;
      }
      else
      {
          // Here, flg1 = 0
          // Here, flg2 = 0
          // Here, flg3 = 0
        if (dbl_eq(t1, t2) && dbl_eq(t1, t3))
          return CUBIT_TRUE;
        else
          return CUBIT_FALSE;
      }
    }
  }
  
    // This would be a programmer's error if we got to this point
//  return CUBIT_FALSE;
}

CubitBoolean AnalyticGeometryTool::is_pnt_on_pln( double pnt[3],
                                                 double pln_orig[3],
                                                 double pln_norm[3] )
{  
   double result;
   
   // See if point satisfies parametric equation of plane
   
   result = pln_norm[0] * (pnt[0] - pln_orig[0]) +
            pln_norm[1] * (pnt[1] - pln_orig[1]) +
            pln_norm[2] * (pnt[2] - pln_orig[2]);
   
   if (dbl_eq(result, 0.0))
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;
}

CubitBoolean AnalyticGeometryTool::is_ln_on_pln( double ln_orig[3],
                                                double ln_vec[3],
                                                double pln_orig[3],
                                                double pln_norm[3] )
{     
   
   // Check to see if line origin is on the plane.  If so, check to see if
   // line vector is perpendicular to plane normal.
   
   if (is_pnt_on_pln(ln_orig, pln_orig, pln_norm) &&
       is_vec_perp(ln_vec, pln_norm))      
      return CUBIT_TRUE;      
   else      
      return CUBIT_FALSE;
}



CubitBoolean AnalyticGeometryTool::is_pln_on_pln( double pln_orig1[3],
                                                 double pln_norm1[3],
                                                 double pln_orig2[3],
                                                 double pln_norm2[3] )
{  
   // If 1st plane origin is on the 2nd plane and the normals are
   // parallel they are coincident
   if( is_vec_par( pln_norm1, pln_norm2 ) &&
      is_pnt_on_pln( pln_orig1, pln_orig2, pln_norm2 ) )
      return CUBIT_TRUE;
   else
      return CUBIT_FALSE;
}

//***************************************************************************
// Arcs/Circles
//***************************************************************************
void AnalyticGeometryTool::setup_arc( double vec_1[3], double vec_2[3], 
                                     double origin[3], double start_angle, 
                                     double end_angle, double radius,
                                     AGT_3D_Arc &arc )
{
   copy_pnt( vec_1, arc.Vec1 );
   copy_pnt( vec_2, arc.Vec2 );
   copy_pnt( origin, arc.Origin );
   arc.StartAngle = start_angle;
   arc.EndAngle = end_angle;
   arc.Radius = radius;
}

void AnalyticGeometryTool::setup_arc( CubitVector& vec_1, CubitVector& vec_2, 
                                     CubitVector origin, double start_angle, 
                                     double end_angle, double radius,
                                     AGT_3D_Arc &arc )
{
   vec_1.get_xyz( arc.Vec1 );
   vec_2.get_xyz( arc.Vec2 );
   origin.get_xyz( arc.Origin );
   arc.StartAngle = start_angle;
   arc.EndAngle = end_angle;
   arc.Radius = radius;
}

void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, double pnt[3] )
{
   double Tp;
   
   // Un-normalized parameter
   Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
   
   // Solve for XYZ
   pnt[0] = arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
                           sin( Tp ) * arc.Vec2[0] ) + 
                           arc.Origin[0];
   
   pnt[1] = arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
                           sin( Tp ) * arc.Vec2[1] ) + 
                           arc.Origin[1];
   
   pnt[2] = arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
                           sin( Tp ) * arc.Vec2[2] ) + 
                           arc.Origin[2];
}

void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, CubitVector& pnt )
{
   double Tp;
   
   // Un-normalized parameter
   Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
   
   // Solve for XYZ
   pnt.x( arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
                         sin( Tp ) * arc.Vec2[0] ) + 
                         arc.Origin[0] );
   
   pnt.y( arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
                         sin( Tp ) * arc.Vec2[1] ) + 
                         arc.Origin[1] );
   
   pnt.z( arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
                         sin( Tp ) * arc.Vec2[2] ) + 
                         arc.Origin[2] );
}

int 
AnalyticGeometryTool::get_num_circle_tess_pnts( double radius, 
                                                double len_tol )
{
  double cmin, cmax;

  double c = 2*CUBIT_PI*radius; // Circumference

  // Find the number of points required for the given accuracy.  Use
  // a bisection method.
  int nmin = 8, nmax = 100;
  cmin = 2.0*nmin*radius*sin(CUBIT_PI/nmin); // Circumference of circle using segments
  cmax = 2.0*nmax*radius*sin(CUBIT_PI/nmax);

  if( dbl_eq( cmin, c ) )
    return nmin;

  double old_epsilon = set_epsilon( len_tol );

  // Find an n that is more than accurate enough
  while( !dbl_eq( cmax, c ) )
  {
    nmin = nmax;
    nmax = nmin * 10;
    cmin = 2.0*nmin*radius*sin(CUBIT_PI/nmin);
    cmax = 2.0*nmax*radius*sin(CUBIT_PI/nmax);
  }
  
  // Biscect until the minimum number of segments satisfying
  // the tolerance is found.
  int n;
  while( 1 )
  {
    n = (nmin + nmax)/2;
    double cn = 2.0*n*radius*sin(CUBIT_PI/n);
    if( dbl_eq( cn, c ) )
    {
      // Go lower
      nmax = n;
    }
    else
    {
      // Go higher
      nmin = n;
    }
    if( nmax-nmin < 2 )
      break;
  }
  set_epsilon( old_epsilon );

  return nmax;
}

//***************************************************************************
// Miscellaneous
//***************************************************************************
void AnalyticGeometryTool::get_pln_orig_norm( double A, double B, double C,
                                             double D, double pln_orig[3], 
                                             double pln_norm[3] )
{
   double x = 0.0, y = 0.0, z = 0.0;

   // Try to have origin aligned with one of the principal axes
   if( !dbl_eq( C, 0.0 ) )
      z = -D/C;
   else if (!dbl_eq( A, 0.0 ) )
      x = -D/A;
   else if (!dbl_eq( B, 0.0 ) )
      y = -D/B;

   pln_orig[0] = x;
   pln_orig[1] = y;
   pln_orig[2] = z;

   if( pln_norm )
   {
      pln_norm[0] = A;
      pln_norm[1] = B;
      pln_norm[2] = C;
   }
}

void AnalyticGeometryTool::get_box_corners( double box_min[3], 
                                           double box_max[3], 
                                           double c[8][3] )
{
   // Left-Bottom-Front       // Left-Top-Front
   c[0][0] = box_min[0];      c[1][0] = box_min[0];
   c[0][1] = box_min[1];      c[1][1] = box_max[1];
   c[0][2] = box_max[2];      c[1][2] = box_max[2];

   // Right-Top-Front         // Right-Bottom-Front
   c[2][0] = box_max[0];      c[3][0] = box_max[0];
   c[2][1] = box_max[1];      c[3][1] = box_min[1];
   c[2][2] = box_max[2];      c[3][2] = box_max[2];

   // Left-Bottom-Back        // Left-Top-Back
   c[4][0] = box_min[0];      c[5][0] = box_min[0];
   c[4][1] = box_min[1];      c[5][1] = box_max[1];
   c[4][2] = box_min[2];      c[5][2] = box_min[2];

   // Right-Top-Back          // Right-Bottom-Back
   c[6][0] = box_max[0];      c[7][0] = box_max[0];
   c[6][1] = box_max[1];      c[7][1] = box_min[1];
   c[6][2] = box_min[2];      c[7][2] = box_min[2];
   
}

CubitStatus 
AnalyticGeometryTool::min_pln_box_int_corners( const CubitPlane& plane,
                                              const CubitBox& box,
                                              int extension_type,
                                              double extension,
                                              CubitVector& p1, CubitVector& p2,
                                              CubitVector& p3, CubitVector& p4,
                                              CubitBoolean silent )
{
   CubitVector box_min = box.minimum();
   CubitVector box_max = box.maximum();
   
   CubitVector plane_norm = plane.normal();
   
   double box_min_pnt[3], box_max_pnt[3], pln_norm[3];
   box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
   plane_norm.get_xyz( pln_norm );
   
   double pnt1[3], pnt2[3], pnt3[3], pnt4[3];
   
   if( min_pln_box_int_corners( pln_norm, plane.coefficient(),
      box_min_pnt, box_max_pnt, 
      extension_type, extension, 
      pnt1, pnt2, pnt3, pnt4, silent ) == CUBIT_FAILURE )
         return CUBIT_FAILURE;
   
   p1.set( pnt1 ); p2.set( pnt2 ); p3.set( pnt3 ); p4.set( pnt4 );
   
   return CUBIT_SUCCESS;
}

CubitStatus 
AnalyticGeometryTool::min_pln_box_int_corners( CubitVector& vec1,
                                              CubitVector& vec2,
                                              CubitVector& vec3,
                                              CubitVector& box_min,
                                              CubitVector& box_max,
                                              int extension_type,
                                              double extension,
                                              CubitVector& p1, CubitVector& p2, 
                                              CubitVector& p3, CubitVector& p4,
                                              CubitBoolean silent )
{
   CubitPlane plane;
   if( plane.mk_plane_with_points( vec1, vec2, vec3 ) == CUBIT_FAILURE )
      return CUBIT_FAILURE;

   CubitVector plane_norm = plane.normal();
   double coefficient = plane.coefficient();

   double plane_norm3[3];
   double box_min3[3];
   double box_max3[3];

   box_min.get_xyz( box_min3 );
   box_max.get_xyz( box_max3 );
   plane_norm.get_xyz( plane_norm3 );

   double p1_3[3], p2_3[3], p3_3[3], p4_3[3];
   p1.get_xyz( p1_3 );
   p2.get_xyz( p2_3 );
   p3.get_xyz( p3_3 );
   p4.get_xyz( p4_3 );
   
   if( min_pln_box_int_corners( plane_norm3, coefficient, box_min3, box_max3, 
      extension_type, extension, p1_3, p2_3, p3_3, p4_3, silent ) == CUBIT_FAILURE )
      return CUBIT_FAILURE;

   p1.set( p1_3 );
   p2.set( p2_3 );
   p3.set( p3_3 );
   p4.set( p4_3 );

   return CUBIT_SUCCESS;
}

CubitStatus 
AnalyticGeometryTool::min_pln_box_int_corners( double pln_norm[3], 
                                              double pln_coeff,
                                              double box_min[3],
                                              double box_max[3],
                                              int extension_type,
                                              double extension,
                                              double p1[3], double p2[3],
                                              double p3[3], double p4[3],
                                              CubitBoolean silent )
{
   int i;
   double cubit2pln_mtx[4][4],
      pln2cubit_mtx[4][4];
   double pln_orig[3];

   double A = pln_norm[0];
   double B = pln_norm[1];
   double C = pln_norm[2];
   double D = pln_coeff;

   //PRINT_INFO( "A=%0.4lf, B=%0.4lf, C=%0.4lf, D=%0.4lf\n", A, B, C, D );

   get_pln_orig_norm( A, B, C, D, pln_orig );

//   PRINT_INFO( "Plane Orig = %0.4lf, %0.4lf, %0.4lf\n", pln_orig[0],
//      pln_orig[1], pln_orig[2] );

   // Find intersections of edges with plane.  Add to unique
   // array.  At most there are 6 intersections...
   double int_array[6][3];
   int num_int = 0;
   num_int = get_plane_bbox_intersections( box_min, box_max, pln_orig, pln_norm, int_array );

   //attempt to adjust bounding box to x,y,z intercepts of plane
   if( num_int == 0 )
   {
     //Stretch bounding box so that plane will fit, for sure
     //get x,y,z intercepts
     double x_intercept = 0;
     double y_intercept = 0;
     double z_intercept = 0;
     if( !dbl_eq( A, 0.0 ) )
       x_intercept = -D/A;
     if( !dbl_eq( B, 0.0 ) )
       y_intercept = -D/B;
     if( !dbl_eq( C, 0.0 ) )
       z_intercept = -D/C;
    
    //adjust box 
    if( x_intercept < box_min[0] )
      box_min[0] = x_intercept;
    else if( x_intercept > box_max[0] )
      box_max[0] = x_intercept;

    if( y_intercept < box_min[1] )
      box_min[1] = y_intercept;
    else if( y_intercept > box_max[1] )
      box_max[1] = y_intercept;
      
    if( z_intercept < box_min[2] )
      box_min[2] = z_intercept;
    else if( z_intercept > box_max[2] )
      box_max[2] = z_intercept;

     num_int = get_plane_bbox_intersections( box_min, box_max, pln_orig, pln_norm, int_array );
   }

   if( num_int == 0 )
   {
     if( silent == CUBIT_FALSE )
       PRINT_ERROR( "Plane does not intersect the bounding box\n"
       "       Can't find 4 corners of plane\n" );
     return CUBIT_FAILURE;
   }
   if( num_int < 3 )
   {
     if( silent == CUBIT_FALSE )
       PRINT_ERROR( "Plane intersects the bounding box at only %d locations\n"
       "      Can't calculate 4 corners of plane\n", num_int );
      return CUBIT_FAILURE;
   }

   // Transform pnts to plane coordinate system
   double pln_x[3], pln_y[3];
   orth_vecs( pln_norm, pln_x, pln_y );
   vecs_to_mtx( pln_x, pln_y, pln_norm, pln_orig, pln2cubit_mtx );
   inv_trans_mtx( pln2cubit_mtx, cubit2pln_mtx );

   double int_arr_pln[6][3];
   for( i=0; i<num_int; i++ )
      transform_pnt( cubit2pln_mtx, int_array[i], int_arr_pln[i] );

   // Place into format for mimimal box calculation
   Point2 pt[6];
   for ( i=0; i<num_int; i++ )
   {
      pt[i].x = int_arr_pln[i][0];
      pt[i].y = int_arr_pln[i][1];
      if( !dbl_eq( int_arr_pln[i][2], 0.0 ) )
      {
        if( silent == CUBIT_FALSE )
          PRINT_ERROR( "in AnalyticGeometryTool::min_box_pln_int_corners\n"
          "       Transform to plane wrong\n" );
         return CUBIT_FAILURE;
      }
   }
   
   // Find rectangle with minimal area to surround the points
   // (this is definitely overkill esp. for the simple cases.....)
   OBBox2 minimal = MinimalBox2( num_int, pt );
   
   // Strip out results
   double old_epsilon = set_epsilon( 1e-10 );
   double centroid[3];
   centroid[0] = minimal.center.x;
   centroid[1] = minimal.center.y;
   centroid[2] = 0.0;
   round_near_val( centroid[0] ); // Makes near -1, 0, 1 values -1, 0, 1
   round_near_val( centroid[1] );
   transform_pnt( pln2cubit_mtx, centroid, centroid );
   
   double x_axis[3];
   x_axis[0] = minimal.axis[0].x;
   x_axis[1] = minimal.axis[0].y;
   x_axis[2] = 0.0;
   round_near_val( x_axis[0] );
   round_near_val( x_axis[1] );
   transform_vec( pln2cubit_mtx, x_axis, x_axis );
   
   double y_axis[3];
   y_axis[0] = minimal.axis[1].x;
   y_axis[1] = minimal.axis[1].y;
   y_axis[2] = 0.0;
   round_near_val( y_axis[0] );
   round_near_val( y_axis[1] );
   transform_vec( pln2cubit_mtx, y_axis, y_axis );

   set_epsilon( old_epsilon );
   
   double dist_x;
   double dist_y;
   double extension_distance = 0.0;
   if( extension_type == 1 ) // Percentage (of 1/2 diagonal)
   {
      double diag_len = sqrt(  minimal.extent[0]*minimal.extent[0]
                             + minimal.extent[1]*minimal.extent[1] );
      extension_distance = diag_len*extension/100.0;
   }
   else if( extension_type == 2 ) // Absolute distance in x and y
      extension_distance = extension;

   dist_x = minimal.extent[0] + extension_distance;
   dist_y = minimal.extent[1] + extension_distance;
   
   next_pnt( centroid, x_axis, -dist_x, p1 );
   next_pnt( p1, y_axis, -dist_y, p1 );
   
   next_pnt( centroid, x_axis, -dist_x, p2 );
   next_pnt( p2, y_axis, dist_y, p2 );
   
   next_pnt( centroid, x_axis, dist_x, p3 );
   next_pnt( p3, y_axis, dist_y, p3 );
   
   next_pnt( centroid, x_axis, dist_x, p4 );
   next_pnt( p4, y_axis, -dist_y, p4 );
   
   return CUBIT_SUCCESS;
}

int AnalyticGeometryTool::get_plane_bbox_intersections( double box_min[3],
                                                           double box_max[3],
                                                           double pln_orig[3],
                                                           double pln_norm[3],
                                                           double int_array[6][3])
{

   // Fill in an array with all 8 box corners
   double corner[8][3];
   get_box_corners( box_min, box_max, corner );
 
   // Get 12 edges of the box
   double ln_start[12][3], ln_end[12][3];
   copy_pnt( corner[0], ln_start[0] );  copy_pnt( corner[1], ln_end[0] );
   copy_pnt( corner[1], ln_start[1] );  copy_pnt( corner[2], ln_end[1] );
   copy_pnt( corner[2], ln_start[2] );  copy_pnt( corner[3], ln_end[2] );
   copy_pnt( corner[3], ln_start[3] );  copy_pnt( corner[0], ln_end[3] ); 
   copy_pnt( corner[4], ln_start[4] );  copy_pnt( corner[5], ln_end[4] );
   copy_pnt( corner[5], ln_start[5] );  copy_pnt( corner[6], ln_end[5] );
   copy_pnt( corner[6], ln_start[6] );  copy_pnt( corner[7], ln_end[6] );
   copy_pnt( corner[7], ln_start[7] );  copy_pnt( corner[4], ln_end[7] );
   copy_pnt( corner[0], ln_start[8] );  copy_pnt( corner[4], ln_end[8] );
   copy_pnt( corner[1], ln_start[9] );  copy_pnt( corner[5], ln_end[9] );
   copy_pnt( corner[2], ln_start[10] ); copy_pnt( corner[6], ln_end[10] );
   copy_pnt( corner[3], ln_start[11] ); copy_pnt( corner[7], ln_end[11] );
   
   double ln_vec[3];
   double int_pnt[3];
   int num_int = 0;
   int i, j, found;
   for( i=0; i<12; i++ )
   {
      get_vec_unit( ln_start[i], ln_end[i], ln_vec );
      if( int_ln_pln( ln_start[i], ln_vec, pln_orig, pln_norm, int_pnt ) )
      {
         // Only add if on the bounded line segment
         if( is_pnt_on_ln_seg( int_pnt, ln_start[i], ln_end[i] ) )
         {
            // Only add if unique
            found = 0;
            for( j=0; j<num_int; j++ )
            {
               if( pnt_eq( int_pnt, int_array[j] ) )
               {
                  found = 1;
                  break;
               }
            }
            if( !found )
            {
               copy_pnt( int_pnt, int_array[num_int] );
               num_int++;
            }
         }
      }
   }
  return num_int;
}

CubitStatus
AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,                                              
                                              CubitVector &center,
                                              CubitVector axes[3],
                                              CubitVector &extension )
{
   int num_pnts = point_list.size();
   if( num_pnts == 0 )
      return CUBIT_FAILURE;
   Point3 *pnt_arr = new Point3[num_pnts];

   int i;
   point_list.reset();
   CubitVector *vec;
   for( i=0; i<num_pnts; i++ )
   {
      vec = point_list.get_and_step();

      pnt_arr[i].x = vec->x();
      pnt_arr[i].y = vec->y();
      pnt_arr[i].z = vec->z();
   }

   OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);

   //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
   //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
   //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
   //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
   //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );

   center.set(minimal.center.x, minimal.center.y, minimal.center.z);
   axes[0].set(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
   axes[1].set(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
   axes[2].set(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
   extension.set(minimal.extent[0], minimal.extent[1], minimal.extent[2]);

   delete [] pnt_arr;

   return CUBIT_SUCCESS;
}

CubitStatus
AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,                                              
                                              CubitVector& p1, CubitVector& p2,
                                              CubitVector& p3, CubitVector& p4,
                                              CubitVector& p5, CubitVector& p6,
                                              CubitVector& p7, CubitVector& p8)
{
   int num_pnts = point_list.size();
   if( num_pnts == 0 )
      return CUBIT_FAILURE;
   Point3 *pnt_arr = new Point3[num_pnts];

   int i;
   point_list.reset();
   CubitVector *vec;
   for( i=0; i<num_pnts; i++ )
   {
      vec = point_list.get_and_step();

      pnt_arr[i].x = vec->x();
      pnt_arr[i].y = vec->y();
      pnt_arr[i].z = vec->z();
   }

   OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);

   //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
   //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
   //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
   //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
   //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );

   CubitVector center(minimal.center.x, minimal.center.y, minimal.center.z);
   CubitVector x(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
   CubitVector y(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
   CubitVector z(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
   CubitVector extent(minimal.extent[0], minimal.extent[1], minimal.extent[2]);

   center.next_point( -x, extent.x(), p1 ); p1.next_point( -y, extent.y(), p1 );
   p1.next_point( z, extent.z(), p1 );

   center.next_point( -x, extent.x(), p2 ); p2.next_point( y, extent.y(), p2 );
   p2.next_point( z, extent.z(), p2 );

   center.next_point( x, extent.x(), p3 ); p3.next_point( y, extent.y(), p3 );
   p3.next_point( z, extent.z(), p3 );

   center.next_point( x, extent.x(), p4 ); p4.next_point( -y, extent.y(), p4 );
   p4.next_point( z, extent.z(), p4 );

   center.next_point( -x, extent.x(), p5 ); p5.next_point( -y, extent.y(), p5 );
   p5.next_point( -z, extent.z(), p5 );

   center.next_point( -x, extent.x(), p6 ); p6.next_point( y, extent.y(), p6 );
   p6.next_point( -z, extent.z(), p6 );

   center.next_point( x, extent.x(), p7 ); p7.next_point( y, extent.y(), p7 );
   p7.next_point( -z, extent.z(), p7 );

   center.next_point( x, extent.x(), p8 ); p8.next_point( -y, extent.y(), p8 );
   p8.next_point( -z, extent.z(), p8 );

   delete pnt_arr;

   return CUBIT_SUCCESS;
}

CubitStatus 
AnalyticGeometryTool::min_cyl_box_int( double radius,
                                       CubitVector& axis,
                                       CubitVector& center,
                                       CubitBox& box,
                                       int extension_type,
                                       double extension,
                                       CubitVector &start,
                                       CubitVector &end,
                                       int num_tess_pnts )
                                       
{
   CubitVector box_min = box.minimum();
   CubitVector box_max = box.maximum();
   
   double box_min_pnt[3], box_max_pnt[3], axis_vec[3], center_pnt[3];
   box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
   axis.get_xyz( axis_vec ); center.get_xyz( center_pnt );
   
   double start_pnt[3], end_pnt[3];
   
   if( min_cyl_box_int( radius, axis_vec, center_pnt, 
                        box_min_pnt, box_max_pnt, 
                        extension_type, extension, 
                        start_pnt, end_pnt, num_tess_pnts )
                        == CUBIT_FAILURE )
     return CUBIT_FAILURE;
   
   start.set( start_pnt ); end.set( end_pnt );
   
   return CUBIT_SUCCESS;
}

CubitStatus 
AnalyticGeometryTool::min_cyl_box_int( double radius, double axis[3], 
                                      double center[3], 
                                      double box_min[3], double box_max[3], 
                                      int extension_type, double extension, 
                                      double start[3], double end[3], 
                                      int num_tess_pnts )
{
  double cyl_z[3];
  unit_vec( axis, cyl_z );

  //PRINT_INFO( "Axis = %f, %f, %f\n", cyl_z[0], cyl_z[1], cyl_z[2] );
  //PRINT_INFO( "Center = %f, %f, %f\n", center[0], center[1], center[2] );

  // Find transformation matrix to take a point into cylinder's
  // coordinate system
  double cubit2cyl_mtx[4][4], cyl2cubit_mtx[4][4];
  double cyl_x[3], cyl_y[3];
  orth_vecs( cyl_z, cyl_x, cyl_y );
  vecs_to_mtx( cyl_x, cyl_y, cyl_z, center, cyl2cubit_mtx );
  inv_trans_mtx( cyl2cubit_mtx, cubit2cyl_mtx );

  // Setup the circle
  double vec_1[3], vec_2[3];
  orth_vecs( cyl_z, vec_1, vec_2 );
  AGT_3D_Arc arc;
  setup_arc( vec_1, vec_2, center, 0.0, 2.0 * CUBIT_PI, radius, arc );

  // Setup the planes of the box
  double pln_norm[6][3], pln_orig[6][3];
  // Front
  pln_orig[0][0] = 0.0; pln_orig[0][1] = 0.0; pln_orig[0][2] = box_max[2];
  pln_norm[0][0] = 0.0; pln_norm[0][1] = 0.0; pln_norm[0][2] = 1.0;
  // Left
  pln_orig[1][0] = box_min[0]; pln_orig[1][1] = 0.0; pln_orig[1][2] = 0.0;
  pln_norm[1][0] = -1.0; pln_norm[1][1] = 0.0; pln_norm[1][2] = 0.0;
  // Top
  pln_orig[2][0] = 0.0; pln_orig[2][1] = box_max[1]; pln_orig[2][2] = 0.0;
  pln_norm[2][0] = 0.0; pln_norm[2][1] = 1.0; pln_norm[2][2] = 0.0;
  // Right
  pln_orig[3][0] = box_max[0]; pln_orig[3][1] = 0.0; pln_orig[3][2] = 0.0;
  pln_norm[3][0] = 1.0; pln_norm[3][1] = 0.0; pln_norm[3][2] = 0.0;
  // Bottom
  pln_orig[4][0] = 0.0; pln_orig[4][1] = box_min[1]; pln_orig[4][2] = 0.0;
  pln_norm[4][0] = 0.0; pln_norm[4][1] = -1.0; pln_norm[4][2] = 0.0;
  // Back
  pln_orig[5][0] = 0.0; pln_orig[5][1] = 0.0; pln_orig[5][2] = box_min[2];
  pln_norm[5][0] = 0.0; pln_norm[5][1] = 0.0; pln_norm[5][2] = -1.0;

  double z; // Intersection along cylinder's axis
  double min_z = CUBIT_DBL_MAX, max_z = -CUBIT_DBL_MAX;
  
  double t = 0.0, dt;
  dt = 1.0/(double)num_tess_pnts;
  double pnt[3];
  double int_pnt[3];
  double box_tol = 1e-14;
  double box_min_0 = box_min[0]-box_tol;
  double box_min_1 = box_min[1]-box_tol;
  double box_min_2 = box_min[2]-box_tol;
  double box_max_0 = box_max[0]+box_tol;
  double box_max_1 = box_max[1]+box_tol;
  double box_max_2 = box_max[2]+box_tol;

  int i,j;
  for( i=0; i<num_tess_pnts; i++ )
  {
    get_arc_xyz( arc, t, pnt );

    for( j=0; j<6; j++ )
    {
      // Evaluate the intersection at this point
      if( int_ln_pln( pnt, cyl_z, pln_orig[j], pln_norm[j], int_pnt )
        == CUBIT_FAILURE )
        continue;

      // Throw-out if intersection not on physical box
      if( int_pnt[0] < box_min_0 || int_pnt[1] < box_min_1 ||
          int_pnt[2] < box_min_2 || int_pnt[0] > box_max_0 ||
          int_pnt[1] > box_max_1 || int_pnt[2] > box_max_2 ) 
        continue;

      // Find min/max cylinder z on box so far
      // z-distance (in cylinder coordinate system)
      z = cubit2cyl_mtx[0][2]*int_pnt[0] + cubit2cyl_mtx[1][2]*int_pnt[1] + 
        cubit2cyl_mtx[2][2]*int_pnt[2] + cubit2cyl_mtx[3][2];

      if( z < min_z ) min_z = z;
      if( z > max_z ) max_z = z;
      
    }

    t += dt;

  }

  // Check the 8 corners of the box - they are likely min/max's.
  double box_corners[8][3];
  get_box_corners( box_min, box_max, box_corners );
  for( i=0; i<8; i++ )
  {
    // Get the corner in the cylinder csys
    transform_pnt( cubit2cyl_mtx, box_corners[i], pnt );
    // If the pnt is within the circle's radius, check it's z-coord
    // (distance from center)
    if(  sqrt( pnt[0]*pnt[0] + pnt[1]*pnt[1] ) <= radius+box_tol )
    {
      if( pnt[2] < min_z ) min_z = pnt[2];
      if( pnt[2] > max_z ) max_z = pnt[2];
    }
  }

  if( min_z == CUBIT_DBL_MAX || max_z == -CUBIT_DBL_MAX )
  {
    PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
    return CUBIT_FAILURE;
  }

  if( min_z == max_z )
  {
    PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
    return CUBIT_FAILURE;
  }

  //PRINT_INFO( "Min dist = %f\n", min_z );
  //PRINT_INFO( "Max dist = %f\n", max_z );

  // Find the start and end of the cylinder
  next_pnt( center, cyl_z, min_z, start );
  next_pnt( center, cyl_z, max_z, end );

  //PRINT_INFO( "Start = %f, %f, %f\n", start[0], start[1], start[2] );
  //PRINT_INFO( "End = %f, %f, %f\n", end[0], end[1], end[2] );

  // Extend start and end, if necessary
  if( extension_type > 0 )
  {
    double ext_distance = 0.0;
    if( extension_type == 1 ) // percentage
    {
      double cyl_length = dist_pnt_pnt( start, end );
      ext_distance = extension/100.0 * cyl_length;
    }
    else
      ext_distance = extension;

    next_pnt( end, cyl_z, ext_distance, end );
    reverse_vec( cyl_z, cyl_z );
    next_pnt( start, cyl_z, ext_distance, start );
  }

  return CUBIT_SUCCESS;
}

// MAGIC SOFTWARE - see .hpp file
// FILE: minbox2.cpp
//---------------------------------------------------------------------------
double AnalyticGeometryTool::Area (int N, Point2* pt, double angle)
{
    double cs = cos(angle), sn = sin(angle);

    double umin = +cs*pt[0].x+sn*pt[0].y, umax = umin;
    double vmin = -sn*pt[0].x+cs*pt[0].y, vmax = vmin;
    for (int i = 1; i < N; i++)
    {
        double u = +cs*pt[i].x+sn*pt[i].y;
        if ( u < umin )
            umin = u;
        else if ( u > umax )
            umax = u;

        double v = -sn*pt[i].x+cs*pt[i].y;
        if ( v < vmin )
            vmin = v;
        else if ( v > vmax )
            vmax = v;
    }

    double area = (umax-umin)*(vmax-vmin);
    return area;
}
//---------------------------------------------------------------------------
void AnalyticGeometryTool::MinimalBoxForAngle (int N, Point2