cgma
AnalyticGeometryTool.cpp
Go to the documentation of this file.
```00001 //- Class:       AnalyticGeometryTool
00002 //- Description: This class performs calculations on analytic geometry
00003 //               (points, lines, arcs, planes, polygons).  Capabilities
00004 //               include vector and point math, matrix operations,
00005 //               measurements, intersections and comparison/containment checks.
00006 //
00007 //- Owner: Steve Storm, Caterpillar Inc.
00008 //- Checked by:
00009
00010
00011 #include <float.h>
00012 #include "AnalyticGeometryTool.hpp"
00013 #include "CubitMessage.hpp"
00014 #include "CubitBox.hpp"
00015 #include "CubitPlane.hpp"
00016 #include "CubitVector.hpp"
00017 #include "DLIList.hpp"
00018 #include <math.h>
00019 #include "CastTo.hpp"
00020 #include "GfxPreview.hpp"
00021 #include "GMem.hpp"
00022
00023 #include <fstream>
00024 using std::cout;
00025 using std::endl;
00026 using std::ofstream;
00027 using std::ios;
00028 using std::ostream;
00029
00030 static double AGT_IDENTITY_MTX_4X4[4][4] = { {1.0, 0.0, 0.0, 0.0},
00031                                              {0.0, 1.0, 0.0, 0.0},
00032                                              {0.0, 0.0, 1.0, 0.0},
00033                                              {0.0, 0.0, 0.0, 1.0} };
00034
00035 static double AGT_IDENTITY_MTX_3X3[3][3] = { {1.0, 0.0, 0.0},
00036                                              {0.0, 1.0, 0.0},
00037                                              {0.0, 0.0, 1.0} };
00038
00039 AnalyticGeometryTool* AnalyticGeometryTool::instance_ = 0;
00040
00041 Point2 operator+ (const Point2& p, const Point2& q)
00042 {
00044     add.x = p.x + q.x;
00045     add.y = p.y + q.y;
00047 }
00048
00049 Point2 operator- (const Point2& p, const Point2& q)
00050 {
00051     Point2 sub;
00052     sub.x = p.x - q.x;
00053     sub.y = p.y - q.y;
00054     return sub;
00055 }
00056
00057 Point2 operator* (double t, const Point2& p)
00058 {
00059     Point2 prod;
00060     prod.x = t*p.x;
00061     prod.y = t*p.y;
00062     return prod;
00063 }
00064
00065 Point2 operator* (const Point2& p, double t)
00066 {
00067     Point2 prod;
00068     prod.x = t*p.x;
00069     prod.y = t*p.y;
00070     return prod;
00071 }
00072
00073 Point2 operator- (const Point2& p)
00074 {
00075     Point2 neg;
00076     neg.x = -p.x;
00077     neg.y = -p.y;
00078     return neg;
00079 }
00080
00081 inline double AnalyticGeometryTool::Dot (const Point2& p, const Point2& q)
00082 {
00083     return double(p.x*q.x + p.y*q.y);
00084 }
00085
00086 Point3 operator+ (const Point3& p, const Point3& q)
00087 {
00089     add.x = p.x + q.x;
00090     add.y = p.y + q.y;
00091     add.z = p.z + q.z;
00093 }
00094
00095 Point3 operator- (const Point3& p, const Point3& q)
00096 {
00097     Point3 sub;
00098     sub.x = p.x - q.x;
00099     sub.y = p.y - q.y;
00100     sub.z = p.z - q.z;
00101     return sub;
00102 }
00103
00104 Point3 operator* (double t, const Point3& p)
00105 {
00106     Point3 prod;
00107     prod.x = t*p.x;
00108     prod.y = t*p.y;
00109     prod.z = t*p.z;
00110     return prod;
00111 }
00112
00113 Point3 operator* (const Point3& p, double t)
00114 {
00115     Point3 prod;
00116     prod.x = t*p.x;
00117     prod.y = t*p.y;
00118     prod.z = t*p.z;
00119     return prod;
00120 }
00121
00122 Point3 operator- (const Point3& p)
00123 {
00124     Point3 neg;
00125     neg.x = -p.x;
00126     neg.y = -p.y;
00127     neg.z = -p.z;
00128     return neg;
00129 }
00130
00131 // Method: instance
00133 // sets up this instance on first access
00134 AnalyticGeometryTool* AnalyticGeometryTool::instance()
00135 {
00136   if (instance_ == 0) {
00137     instance_ = new AnalyticGeometryTool;
00138   }
00139   return instance_;
00140 }
00141
00142 AnalyticGeometryTool::AnalyticGeometryTool()
00143 {
00144    agtEpsilon = 1e-8;
00145 }
00146
00147 AnalyticGeometryTool::~AnalyticGeometryTool()
00148 {
00149   instance_ = NULL;
00150 }
00151
00152 //***************************************************************************
00153 // Double numbers
00154 //***************************************************************************
00155
00156
00157 void AnalyticGeometryTool::round_near_val( double &val )
00158 {
00159    if( dbl_eq( val, 0.0 ) )
00160       val = 0.0;
00161    else if( dbl_eq( val, 1.0 ) )
00162       val  = 1.0;
00163    else if( dbl_eq( val, -1.0 ) )
00164       val = -1.0;
00165 }
00166
00167 //***************************************************************************
00168 // Matrices & Transforms
00169 //***************************************************************************
00170 void AnalyticGeometryTool::transform_pnt( double m[4][4],
00171                                          double pin[3],
00172                                          double pout[3] )
00173 {
00174     double p[3];  // working buffer
00175
00176     // Check if transformation can occur
00177     if (!m) {
00178        if (pin && pout)
00179       copy_pnt(pin, pout);
00180        return;
00181     }
00182
00183     // Perform transformation
00184     p[0] = m[0][0] * pin[0] + m[1][0] * pin[1] + m[2][0] * pin[2] + m[3][0];
00185     p[1] = m[0][1] * pin[0] + m[1][1] * pin[1] + m[2][1] * pin[2] + m[3][1];
00186     p[2] = m[0][2] * pin[0] + m[1][2] * pin[1] + m[2][2] * pin[2] + m[3][2];
00187
00188     // Copy work buffer to out point
00189     copy_pnt(p,pout);
00190 }
00191
00192 void AnalyticGeometryTool::transform_vec( double m3[3][3],
00193                                          double vin[3],
00194                                          double vout[3] )
00195 {
00196    double v[3];    // working buffer
00197
00198    // Determine if transformation can occur
00199    if (!m3) {
00200       if (vin && vout)
00201          copy_vec(vin, vout);
00202       return;
00203    }
00204
00205    // Perform transformation
00206    v[0] = m3[0][0] * vin[0] + m3[1][0] * vin[1] + m3[2][0] * vin[2];
00207    v[1] = m3[0][1] * vin[0] + m3[1][1] * vin[1] + m3[2][1] * vin[2];
00208    v[2] = m3[0][2] * vin[0] + m3[1][2] * vin[1] + m3[2][2] * vin[2];
00209
00210    // Copy work buffer to vector out
00211    copy_pnt(v,vout);
00212 }
00213
00214 void AnalyticGeometryTool::transform_vec( double m4[4][4],
00215                                          double vin[3],
00216                                          double vout[3] )
00217 {
00218    double v[3];    // working buffer
00219
00220    // Determine if transformation can occur
00221    if (!m4) {
00222       if (vin && vout)
00223          copy_vec(vin, vout);
00224       return;
00225    }
00226
00227    // Perform transformation
00228
00229    v[0] = m4[0][0] * vin[0] + m4[1][0] * vin[1] + m4[2][0] * vin[2];
00230    v[1] = m4[0][1] * vin[0] + m4[1][1] * vin[1] + m4[2][1] * vin[2];
00231    v[2] = m4[0][2] * vin[0] + m4[1][2] * vin[1] + m4[2][2] * vin[2];
00232
00233    // Copy work buffer to vector out
00234    copy_pnt(v,vout);
00235 }
00236
00237 void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
00238                                            double origin[3], double axis[3] )
00239 {
00240    double end_pnt[3]; // Find arbitrary end point on line
00241    next_pnt( origin, axis, 10.0, end_pnt );
00242
00243    transform_pnt( rot_mtx, origin, origin );
00244    transform_pnt( rot_mtx, end_pnt, end_pnt );
00245
00246    axis[0] = end_pnt[0] - origin[0];
00247    axis[1] = end_pnt[1] - origin[1];
00248    axis[2] = end_pnt[2] - origin[2];
00249 }
00250
00251 void AnalyticGeometryTool::transform_line( double rot_mtx[4][4],
00252                                           CubitVector &origin, CubitVector &axis )
00253 {
00254    CubitVector end_point; // Find arbitrary end point on line
00255    origin.next_point( axis, 10.0, end_point );
00256
00257    double start_pnt[3], end_pnt[3];
00258    copy_pnt( origin, start_pnt );
00259    copy_pnt( end_point, end_pnt );
00260
00261    transform_pnt( rot_mtx, start_pnt, start_pnt );
00262    transform_pnt( rot_mtx, end_pnt, end_pnt );
00263
00264    axis.x( end_pnt[0] - start_pnt[0] );
00265    axis.y( end_pnt[1] - start_pnt[1] );
00266    axis.z( end_pnt[2] - start_pnt[2] );
00267
00268    origin.set( start_pnt );
00269 }
00270
00271 void AnalyticGeometryTool::copy_mtx( double from[3][3],double to[3][3] )
00272 {
00273    // Determine if identity matrix needed
00274    if (!from) // copy in the identity matrix
00275       memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
00276    else // Copy from to to
00277       memcpy(to, from, sizeof(double)*9);
00278 }
00279
00280 void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[4][4] )
00281 {
00282    // determine if identity matrix needed
00283    if (!from) // copy in the identity matrix
00284       memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
00285    else // copy from to to
00286       memcpy(to, from, sizeof(double)*16);
00287 }
00288
00289 void AnalyticGeometryTool::copy_mtx( double from[4][4], double to[3][3] )
00290 {
00291    size_t dbl3;
00292
00293    dbl3 = sizeof(double) * 3;
00294
00295    // Determine if identity matrix needed
00296    if (!from) // Copy in the identity matrix
00297       memcpy(to, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
00298    else { // Copy each upper left element of from to to
00299       memcpy(to[0], from[0], dbl3);
00300       memcpy(to[1], from[1], dbl3);
00301       memcpy(to[2], from[2], dbl3);
00302    }
00303 }
00304
00305 void AnalyticGeometryTool::copy_mtx(double from[3][3], double to[4][4],
00306                                     double* origin )
00307 {
00308    size_t dbl3;
00309
00310    dbl3 = sizeof(double) * 3;
00311
00312    // Determine if identity matrix needed
00313    if (!from) { // Copy in the identity matrix
00314
00315       memcpy(to, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
00316
00317       if (origin)
00318          memcpy(to[3], origin, dbl3);
00319    }
00320
00321    else { // Copy each upper element of from to to
00322
00323       memcpy(to[0], from[0], dbl3);
00324       memcpy(to[1], from[1], dbl3);
00325       memcpy(to[2], from[2], dbl3);
00326
00327       to[0][3] = 0.0;
00328       to[1][3] = 0.0;
00329       to[2][3] = 0.0;
00330       to[3][3] = 1.0;
00331
00332       if (origin)
00333       {
00334          memcpy(to[3], origin, dbl3);
00335 //         to[3][0] = origin[0];
00336 //         to[3][1] = origin[1];
00337 //         to[3][2] = origin[2];
00338       }
00339       else
00340          memcpy(to[3], AGT_IDENTITY_MTX_4X4[3], sizeof(double)*3);
00341    }
00342 }
00343
00344 void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
00345                                                double mtx3x3[3][3] )
00346 {
00347    double coeff1;
00348    double coeff2;
00349    double coeff3;
00350    double v_unit[3];
00351
00352    if (!mtx3x3)
00353       return;
00354
00355    coeff1 = cos(theta);
00356    coeff2 = (1.0l - coeff1);
00357    coeff3 = sin(theta);
00358
00359    unit_vec(v, v_unit);
00360
00361    mtx3x3[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
00362    mtx3x3[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
00363    mtx3x3[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
00364
00365    mtx3x3[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
00366    mtx3x3[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
00367    mtx3x3[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
00368
00369    mtx3x3[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
00370    mtx3x3[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
00371    mtx3x3[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
00372 }
00373
00374 void AnalyticGeometryTool::create_rotation_mtx( double theta, double v[3],
00375                                                double mtx4x4[4][4] )
00376 {
00377    double coeff1;
00378    double coeff2;
00379    double coeff3;
00380    double v_unit[3];
00381
00382    if (!mtx4x4)
00383       return;
00384
00385    coeff1 = cos(theta);
00386    coeff2 = (1.0l - coeff1);
00387    coeff3 = sin(theta);
00388
00389    unit_vec(v, v_unit);
00390
00391    mtx4x4[0][0] = coeff1 + coeff2 * (v_unit[0] * v_unit[0]);
00392    mtx4x4[0][1] = coeff2 * v_unit[1] * v_unit[0] + coeff3 * v_unit[2];
00393    mtx4x4[0][2] = coeff2 * v_unit[2] * v_unit[0] - coeff3 * v_unit[1];
00394
00395    mtx4x4[1][0] = coeff2 * v_unit[1] * v_unit[0] - coeff3 * v_unit[2];
00396    mtx4x4[1][1] = coeff1 + coeff2 * (v_unit[1] * v_unit[1]);
00397    mtx4x4[1][2] = coeff2 * v_unit[1] * v_unit[2] + coeff3 * v_unit[0];
00398
00399    mtx4x4[2][0] = coeff2 * v_unit[2] * v_unit[0] + coeff3 * v_unit[1];
00400    mtx4x4[2][1] = coeff2 * v_unit[2] * v_unit[1] - coeff3 * v_unit[0];
00401    mtx4x4[2][2] = coeff1 + coeff2 * (v_unit[2] * v_unit[2]);
00402
00403    mtx4x4[0][3] = 0.0;
00404    mtx4x4[1][3] = 0.0;
00405    mtx4x4[2][3] = 0.0;
00406    mtx4x4[3][3] = 1.0;
00407    mtx4x4[3][0] = 0.0;
00408    mtx4x4[3][1] = 0.0;
00409    mtx4x4[3][2] = 0.0;
00410 }
00411
00413                                                       double origin[3] )
00414 {
00415    double tmp_mtx[4][4];
00416
00417    // Translate to origin
00418    double t[4][4];
00419    memcpy(t, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
00420    //PRINT_INFO( "Rotation matrix, before origin: \n" );
00421    //print_mtx( rot_mtx );
00422    t[3][0]=-origin[0]; t[3][1]=-origin[1]; t[3][2]=-origin[2];
00423    mult_mtx( t, rot_mtx, tmp_mtx );
00424
00425    //PRINT_INFO( "Origin times rotation: \n" );
00426    //print_mtx( tmp_mtx );
00427
00428    // Translate back
00429    t[3][0]=origin[0]; t[3][1]=origin[1]; t[3][2]=origin[2];
00430    mult_mtx( tmp_mtx, t, rot_mtx );
00431
00432    //PRINT_INFO( "Rotation x -origin: \n" );
00433    //print_mtx( rot_mtx );
00434 }
00435
00436 void AnalyticGeometryTool::identity_mtx( double mtx3x3[3][3] )
00437 {
00438    memcpy(mtx3x3, AGT_IDENTITY_MTX_3X3, sizeof(double)*9);
00439 }
00440
00441 void AnalyticGeometryTool::identity_mtx( double mtx4x4[4][4] )
00442 {
00443    memcpy(mtx4x4, AGT_IDENTITY_MTX_4X4, sizeof(double)*16);
00444 }
00445
00446 void AnalyticGeometryTool::mtx_to_angs( double mtx3x3[3][3],
00447                                        double &ax, double &ay,
00448                                        double &az )
00449 {
00450 //   METHOD:
00451 //   o Rotate x-vector onto xz plane
00452 //   o  Check xp dotted into y
00453 //   o   If xp dot y is zero  ==> az = 0 (x-vector already in xz plane)
00454 //   o   Otherwise, compute rotation of vector into xz plane to acquire *az
00455 //   o    Use atan2 (on x-vector) to get *az
00456 //   o    Rotate the system about z
00457 //   o Use atan2 function (on x-vector in xz plane) to determine *ay
00458 //   o Rotate the system about y
00459 //   o Compute ax using y-vector
00460 //   o Resultant angles are negated (to reverse above procedure)
00461
00462    double x[3];                    // x-axis vector
00463    double y[3];                    // y-axis vector
00464    double z[3];                    // z-axis vector
00465    double ar[3][3];                // Rotation matrix
00466    double sinr,cosr;               // Used for atan2 function
00467    double work_sys[3][3];          // Temporary holder for system
00468    double *xp = work_sys[0];       // x-axis vector of system: x-primed
00469    double *yp = work_sys[1];       // y-axis vector of system: y-primed
00470
00471    x[0] = 1.0; x[1] = 0.0; x[2] = 0.0;
00472    y[0] = 0.0; y[1] = 1.0; y[2] = 0.0;
00473    z[0] = 0.0; z[1] = 0.0; z[2] = 1.0;
00474
00475    if (!mtx3x3)
00476       return;
00477
00478    // Copy matrix over to work csys
00479    copy_mtx(mtx3x3,work_sys);
00480
00481    // Check xp dotted into y
00482    //   If xp dot y is zero  ==> az = 0
00483
00484    // Otherwise, compute rotation of vector into xz plane to acquire *az
00485
00486    if (dbl_eq(dot_vec(xp,y), 0.0))
00487
00488       az = 0.0;
00489
00490    else {
00491
00492       /*
00493       Compute *az - rotate xp to xz-plane about z-axis
00494                y    xp
00495                |  /
00496                | /  negative angle about z (negative of atan2)
00497                 -----x              (use RH rule about z)
00498                  \
00499                   \ positive angle about z (negative of atan2)
00500                    xp
00501       */
00502
00503       sinr = dot_vec(xp,y);
00504       cosr = dot_vec(xp,x);
00505       az = - atan2(sinr, cosr);
00506
00507       // Rotate the system about z
00508       create_rotation_mtx(az,z,ar);
00509       rotate_system(ar,work_sys,work_sys);
00510
00511    }
00512
00513       /*
00514       Compute *ay - rotate xp to x-axis about y-axis
00515               z    xp
00516               |  /
00517               | /  positive angle about y (positive of atan2)
00518                -----x     (use RH rule about y)
00519                 \
00520                  \ negative angle about y (positive of atan2)
00521                   xp
00522       */
00523
00524    sinr = dot_vec(xp,z);
00525    cosr = dot_vec(xp,x);
00526    ay = atan2(sinr, cosr);
00527
00528    // Rotate the system about y
00529    create_rotation_mtx(ay,y,ar);
00530    rotate_system(ar,work_sys,work_sys);
00531
00532    /*
00533       Compute *ax - rotate yp to y-axis about x-axis
00534               z    yp
00535               |  /
00536               | /  negative angle about x (negative of atan2)
00537                -----y     (use RH rule about x)
00538                 \
00539                  \ positive angle about x (negative of atan2)
00540                   yp
00541    */
00542
00543    sinr = dot_vec(yp,z);
00544    cosr = dot_vec(yp,y);
00545    ax = atan2(sinr,cosr);     // Negative of negative - see below
00546
00547    // Negate above angles for rotation of the system back to original
00548    az = -(az);
00549    ay = -(ay);
00550
00551    // Make sure near zero angles are actually zero
00552    if (dbl_eq(ax, 0.0))
00553       ax = 0.0;
00554
00555    if (dbl_eq(ay, 0.0))
00556       ay = 0.0;
00557 }
00558
00559 void AnalyticGeometryTool::mtx_to_angs( double mtx4x4[4][4],
00560                                        double &ax, double &ay,
00561                                        double &az )
00562 {
00563    double work_sys[3][3];
00564
00565    if(!mtx4x4)
00566       return;
00567
00568    copy_mtx(mtx4x4,work_sys);
00569    mtx_to_angs( work_sys, ax, ay, az );
00570 }
00571
00572 void AnalyticGeometryTool::rotate_system( double mtx[3][3],
00573                                          double sys_in[3][3],
00574                                          double sys_out[3][3] )
00575 {
00576    double sys_tmp[3][3];
00577    double *p_sys_tmp;
00578
00579    // Check to see if rotating in place
00580    if (sys_in == sys_out) {
00581       copy_mtx( sys_in, sys_tmp );
00582       p_sys_tmp = (double *)sys_tmp;
00583    }
00584    else
00585       // Have sys_tmp point at outgoing memory location
00586       p_sys_tmp = (double *)sys_out;
00587
00588
00589    // X-vector
00590    p_sys_tmp[0] =   mtx[0][0] * sys_in[0][0]
00591                   + mtx[1][0] * sys_in[0][1]
00592                   + mtx[2][0] * sys_in[0][2];
00593    p_sys_tmp[1] =   mtx[0][1] * sys_in[0][0]
00594                   + mtx[1][1] * sys_in[0][1]
00595                   + mtx[2][1] * sys_in[0][2];
00596    p_sys_tmp[2] =   mtx[0][2] * sys_in[0][0]
00597                   + mtx[1][2] * sys_in[0][1]
00598                   + mtx[2][2] * sys_in[0][2];
00599
00600    // Y-vector
00601    p_sys_tmp[3] =   mtx[0][0] * sys_in[1][0]
00602                   + mtx[1][0] * sys_in[1][1]
00603                   + mtx[2][0] * sys_in[1][2];
00604    p_sys_tmp[4] =   mtx[0][1] * sys_in[1][0]
00605                   + mtx[1][1] * sys_in[1][1]
00606                   + mtx[2][1] * sys_in[1][2];
00607    p_sys_tmp[5] =   mtx[0][2] * sys_in[1][0]
00608                   + mtx[1][2] * sys_in[1][1]
00609                   + mtx[2][2] * sys_in[1][2];
00610
00611    // Z-vector
00612    p_sys_tmp[6] =   mtx[0][0] * sys_in[2][0]
00613                   + mtx[1][0] * sys_in[2][1]
00614                   + mtx[2][0] * sys_in[2][2];
00615    p_sys_tmp[7] =   mtx[0][1] * sys_in[2][0]
00616                   + mtx[1][1] * sys_in[2][1]
00617                   + mtx[2][1] * sys_in[2][2];
00618    p_sys_tmp[8] =   mtx[0][2] * sys_in[2][0]
00619                   + mtx[1][2] * sys_in[2][1]
00620                   + mtx[2][2] * sys_in[2][2];
00621
00622     // Copy sys_tmp to sys_out if rotating in place
00623    if (sys_in == sys_out) {
00624       memcpy(sys_out, sys_tmp, sizeof(double)*9);
00625    }
00626 }
00627
00628 void AnalyticGeometryTool::rotate_system( double mtx[4][4],
00629                                          double sys_in[4][4],
00630                                          double sys_out[4][4] )
00631 {
00632    double sys_tmp[4][4];
00633    double* p_sys_tmp;
00634
00635    // Check to see if rotating in place
00636    if (sys_in == sys_out) {
00637       copy_mtx( sys_in, sys_tmp );
00638       p_sys_tmp = (double *)sys_tmp;
00639    }
00640    else
00641       // Have p_sys_tmp point at outgoing memory location
00642       p_sys_tmp = (double *)sys_out;
00643
00644
00645    // X-vector
00646    p_sys_tmp[0] =   mtx[0][0] * sys_in[0][0]
00647                   + mtx[1][0] * sys_in[0][1]
00648                   + mtx[2][0] * sys_in[0][2];
00649    p_sys_tmp[1] =   mtx[0][1] * sys_in[0][0]
00650                   + mtx[1][1] * sys_in[0][1]
00651                   + mtx[2][1] * sys_in[0][2];
00652    p_sys_tmp[2] =   mtx[0][2] * sys_in[0][0]
00653                   + mtx[1][2] * sys_in[0][1]
00654                   + mtx[2][2] * sys_in[0][2];
00655
00656    // Y-vector
00657    p_sys_tmp[4] =   mtx[0][0] * sys_in[1][0]
00658                   + mtx[1][0] * sys_in[1][1]
00659                   + mtx[2][0] * sys_in[1][2];
00660    p_sys_tmp[5] =   mtx[0][1] * sys_in[1][0]
00661                   + mtx[1][1] * sys_in[1][1]
00662                   + mtx[2][1] * sys_in[1][2];
00663    p_sys_tmp[6] =   mtx[0][2] * sys_in[1][0]
00664                   + mtx[1][2] * sys_in[1][1]
00665                   + mtx[2][2] * sys_in[1][2];
00666
00667    // Z-vector
00668    p_sys_tmp[8] =   mtx[0][0] * sys_in[2][0]
00669                   + mtx[1][0] * sys_in[2][1]
00670                   + mtx[2][0] * sys_in[2][2];
00671    p_sys_tmp[9] =   mtx[0][1] * sys_in[2][0]
00672                   + mtx[1][1] * sys_in[2][1]
00673                   + mtx[2][1] * sys_in[2][2];
00674    p_sys_tmp[10] =  mtx[0][2] * sys_in[2][0]
00675                   + mtx[1][2] * sys_in[2][1]
00676                   + mtx[2][2] * sys_in[2][2];
00677
00678    // Maintain the origin
00679    p_sys_tmp[3] = sys_in[0][3];
00680    p_sys_tmp[7] = sys_in[1][3];
00681    p_sys_tmp[11] = sys_in[2][3];
00682    p_sys_tmp[15] = sys_in[3][3];
00683    p_sys_tmp[12] = sys_in[3][0];
00684    p_sys_tmp[13] = sys_in[3][1];
00685    p_sys_tmp[14] = sys_in[3][2];
00686
00687     // Copy sys_tmp to sys_out if rotating in place
00688    if (sys_in == sys_out) {
00689       memcpy(sys_out, sys_tmp, sizeof(double)*16);
00690    }
00691 }
00692
00693 double AnalyticGeometryTool::det_mtx( double m[3][3] )
00694 {
00695    double determinant;
00696
00697    if (!m)
00698       return (0.0);
00699
00700    determinant = m[0][0]*(m[1][1]*m[2][2]-m[1][2]*m[2][1])
00701                + m[0][1]*(m[1][2]*m[2][0]-m[1][0]*m[2][2])
00702                + m[0][2]*(m[1][0]*m[2][1]-m[1][1]*m[2][0]);
00703
00704    return (determinant);
00705 }
00706
00707 void AnalyticGeometryTool::mult_mtx( double a[3][3],double b[3][3],
00708                                     double d[3][3] )
00709 {
00710    double c[3][3];    // working buffer
00711
00712    if( a != 0 && b != 0 ) {   // a & b are valid
00713
00714       c[0][0] = (  a[0][0] * b[0][0] + a[0][1] * b[1][0]
00715                  + a[0][2] * b[2][0]);
00716       c[1][0] = (  a[1][0] * b[0][0] + a[1][1] * b[1][0]
00717                  + a[1][2] * b[2][0]);
00718       c[2][0] = (  a[2][0] * b[0][0] + a[2][1] * b[1][0]
00719                  + a[2][2] * b[2][0]);
00720
00721       c[0][1] = (  a[0][0] * b[0][1] + a[0][1] * b[1][1]
00722                  + a[0][2] * b[2][1]);
00723       c[1][1] = (  a[1][0] * b[0][1] + a[1][1] * b[1][1]
00724                  + a[1][2] * b[2][1]);
00725       c[2][1] = (  a[2][0] * b[0][1] + a[2][1] * b[1][1]
00726                  + a[2][2] * b[2][1]);
00727
00728       c[0][2] = (  a[0][0] * b[0][2] + a[0][1] * b[1][2]
00729                  + a[0][2] * b[2][2]);
00730       c[1][2] = (  a[1][0] * b[0][2] + a[1][1] * b[1][2]
00731                  + a[1][2] * b[2][2]);
00732       c[2][2] = (  a[2][0] * b[0][2] + a[2][1] * b[1][2]
00733                  + a[2][2] * b[2][2]);
00734
00735       copy_mtx(c, d);
00736
00737    }
00738    else if (a) {                 // b equals 0
00739       copy_mtx(a, d);
00740    }
00741    else if (b) {                  // a equals 0
00742       copy_mtx(b, d);
00743    }
00744    else {                         // a & b equal 0
00745
00746       copy_mtx(AGT_IDENTITY_MTX_3X3, d);
00747    }
00748 }
00749
00750 void AnalyticGeometryTool::mult_mtx( double a[4][4],
00751                                     double b[4][4],
00752                                     double d[4][4] )
00753 {
00754    double c[4][4];    // working buffer
00755
00756    if( a != 0 && b != 0 ) {   // a & b are valid
00757
00758       c[0][0] = (  a[0][0] * b[0][0] + a[0][1] * b[1][0]
00759                  + a[0][2] * b[2][0] + a[0][3] * b[3][0]);
00760       c[1][0] = (  a[1][0] * b[0][0] + a[1][1] * b[1][0]
00761                  + a[1][2] * b[2][0] + a[1][3] * b[3][0]);
00762       c[2][0] = (  a[2][0] * b[0][0] + a[2][1] * b[1][0]
00763                  + a[2][2] * b[2][0] + a[2][3] * b[3][0]);
00764       c[3][0] = (  a[3][0] * b[0][0] + a[3][1] * b[1][0]
00765                  + a[3][2] * b[2][0] + a[3][3] * b[3][0]);
00766
00767       c[0][1] = (  a[0][0] * b[0][1] + a[0][1] * b[1][1]
00768                  + a[0][2] * b[2][1] + a[0][3] * b[3][1]);
00769       c[1][1] = (  a[1][0] * b[0][1] + a[1][1] * b[1][1]
00770                  + a[1][2] * b[2][1] + a[1][3] * b[3][1]);
00771       c[2][1] = (  a[2][0] * b[0][1] + a[2][1] * b[1][1]
00772                  + a[2][2] * b[2][1] + a[2][3] * b[3][1]);
00773       c[3][1] = (  a[3][0] * b[0][1] + a[3][1] * b[1][1]
00774                  + a[3][2] * b[2][1] + a[3][3] * b[3][1]);
00775
00776       c[0][2] = (  a[0][0] * b[0][2] + a[0][1] * b[1][2]
00777                  + a[0][2] * b[2][2] + a[0][3] * b[3][2]);
00778       c[1][2] = (  a[1][0] * b[0][2] + a[1][1] * b[1][2]
00779                  + a[1][2] * b[2][2] + a[1][3] * b[3][2]);
00780       c[2][2] = (  a[2][0] * b[0][2] + a[2][1] * b[1][2]
00781                  + a[2][2] * b[2][2] + a[2][3] * b[3][2]);
00782       c[3][2] = (  a[3][0] * b[0][2] + a[3][1] * b[1][2]
00783                  + a[3][2] * b[2][2] + a[3][3] * b[3][2]);
00784
00785       c[0][3] = (  a[0][0] * b[0][3] + a[0][1] * b[1][3]
00786                  + a[0][2] * b[2][3] + a[0][3] * b[3][3]);
00787       c[1][3] = (  a[1][0] * b[0][3] + a[1][1] * b[1][3]
00788                  + a[1][2] * b[2][3] + a[1][3] * b[3][3]);
00789       c[2][3] = (  a[2][0] * b[0][3] + a[2][1] * b[1][3]
00790                  + a[2][2] * b[2][3] + a[2][3] * b[3][3]);
00791       c[3][3] = (  a[3][0] * b[0][3] + a[3][1] * b[1][3]
00792                  + a[3][2] * b[2][3] + a[3][3] * b[3][3]);
00793
00794       copy_mtx(c, d);
00795    }
00796    else if (a) {                 // b equals 0
00797       copy_mtx(a, d);
00798    }
00799    else if (b) {                  // a equals 0
00800       copy_mtx(b, d);
00801    }
00802    else {                         // a & b equal 0
00803
00804       copy_mtx(AGT_IDENTITY_MTX_4X4, d);
00805    }
00806 }
00807
00809                                               double inv_mtx[3][3] )
00810 {
00811    int i,i1,i2,j,j1,j2;
00812    double work_mtx[3][3];
00813    double determinant;
00814
00815    // Check for null input
00816    if (!mtx) {
00817       copy_mtx(AGT_IDENTITY_MTX_3X3, inv_mtx);
00818       return CUBIT_SUCCESS;
00819    }
00820
00821    // Calculate determinant
00822    determinant = det_mtx(mtx);
00823
00824    // Check for singularity
00825    if (dbl_eq(determinant,0.0))
00826       return CUBIT_FAILURE;
00827
00828    // Get work matrix (allow inverting in place)
00829    copy_mtx(mtx, work_mtx);
00830
00831    // Inverse is adjoint matrix divided by determinant
00832    for (i=1; i<4; i++) {
00833
00834       i1 = (i % 3) + 1;  i2 = (i1 % 3) + 1;
00835
00836       for (j=1; j<4; j++) {
00837
00838          j1 = (j % 3) + 1;  j2 = (j1 % 3) + 1;
00839
00840          inv_mtx[j-1][i-1] = (work_mtx[i1-1][j1-1]*work_mtx[i2-1][j2-1] -
00841             work_mtx[i1-1][j2-1]*work_mtx[i2-1][j1-1])
00842             / determinant;
00843
00844       }
00845    }
00846    return CUBIT_SUCCESS;
00847 }
00848
00849 CubitStatus AnalyticGeometryTool::inv_trans_mtx( double transf[4][4],
00850                                                 double inv_transf[4][4] )
00851 {
00852    double scale_sq;
00853    double inv_sq_scale;
00854    double tmp_transf[4][4]; // For temporary storage of incoming matrix
00855    double *p_tmp_transf = NULL;
00856
00857    // If input transform is 0 set output to identity matrix
00858    if (!transf) {
00859       copy_mtx( AGT_IDENTITY_MTX_4X4, inv_transf );
00860       return CUBIT_SUCCESS;
00861    }
00862
00863    // Obtain the matrix scale
00864    scale_sq = transf[0][0]*transf[0][0] + transf[0][1]*transf[0][1] +
00865               transf[0][2]*transf[0][2];
00866
00867    // Check for singular matrix
00868    if (scale_sq < (.000000001 * .000000001))
00869       return CUBIT_FAILURE;
00870
00871    // Need the inverse scale squared
00872    inv_sq_scale = 1.0 / scale_sq;
00873
00874    // Check to see if inverting in place
00875    if (transf == inv_transf) {
00876       copy_mtx( transf, tmp_transf );
00877       p_tmp_transf = (double *)tmp_transf;
00878    }
00879    else
00880       p_tmp_transf = (double *)transf;
00881
00882    // The X vector
00883    inv_transf[0][0] = p_tmp_transf[0] * inv_sq_scale;
00884    inv_transf[1][0] = p_tmp_transf[1] * inv_sq_scale;
00885    inv_transf[2][0] = p_tmp_transf[2] * inv_sq_scale;
00886
00887    // The Y vector
00888    inv_transf[0][1] = p_tmp_transf[4] * inv_sq_scale;
00889    inv_transf[1][1] = p_tmp_transf[5] * inv_sq_scale;
00890    inv_transf[2][1] = p_tmp_transf[6] * inv_sq_scale;
00891
00892    // The Z vector
00893    inv_transf[0][2] = p_tmp_transf[8] * inv_sq_scale;
00894    inv_transf[1][2] = p_tmp_transf[9] * inv_sq_scale;
00895    inv_transf[2][2] = p_tmp_transf[10] * inv_sq_scale;
00896
00897    // Column 4
00898    inv_transf[0][3] = 0.0;
00899    inv_transf[1][3] = 0.0;
00900    inv_transf[2][3] = 0.0;
00901
00902    // The X origin
00903    inv_transf[3][0] = -inv_sq_scale * (  p_tmp_transf[0] * p_tmp_transf[12]
00904                        + p_tmp_transf[1] * p_tmp_transf[13]
00905                        + p_tmp_transf[2] * p_tmp_transf[14]);
00906
00907    // The Y origin
00908    inv_transf[3][1] = -inv_sq_scale * (  p_tmp_transf[4] * p_tmp_transf[12]
00909                        + p_tmp_transf[5] * p_tmp_transf[13]
00910                        + p_tmp_transf[6] * p_tmp_transf[14]);
00911
00912    // The Z origin
00913    inv_transf[3][2] = -inv_sq_scale * (  p_tmp_transf[8] * p_tmp_transf[12]
00914                        + p_tmp_transf[9] * p_tmp_transf[13]
00915                        + p_tmp_transf[10] * p_tmp_transf[14]);
00916
00917    // This is always one
00918    inv_transf[3][3] = 1.0;
00919
00920    return CUBIT_SUCCESS;
00921 }
00922
00923 void AnalyticGeometryTool::vecs_to_mtx( double xvec[3],
00924                                        double yvec[3],
00925                                        double zvec[3],
00926                                        double matrix[3][3] )
00927 {
00928    if (xvec)
00929       copy_pnt(xvec, matrix[0]);
00930    else
00931       copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
00932
00933    if (yvec)
00934       copy_pnt(yvec, matrix[1]);
00935    else
00936       copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
00937
00938    if (zvec)
00939       copy_pnt(zvec, matrix[2]);
00940    else
00941       copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
00942 }
00943
00944 void AnalyticGeometryTool::vecs_to_mtx( double xvec[3],
00945                                        double yvec[3],
00946                                        double zvec[3],
00947                                        double origin[3],
00948                                        double matrix[4][4] )
00949 {
00950    if (xvec)
00951       copy_pnt(xvec, matrix[0]);
00952    else
00953       copy_pnt(AGT_IDENTITY_MTX_3X3[0], matrix[0]);
00954
00955    if (yvec)
00956       copy_pnt(yvec, matrix[1]);
00957    else
00958       copy_pnt(AGT_IDENTITY_MTX_3X3[1], matrix[1]);
00959
00960    if (zvec)
00961       copy_pnt(zvec, matrix[2]);
00962    else
00963       copy_pnt(AGT_IDENTITY_MTX_3X3[2], matrix[2]);
00964
00965    if( origin )
00966       copy_pnt(origin, matrix[3]);
00967    else
00968    {
00969       matrix[3][0] = 0.0;
00970       matrix[3][1] = 0.0;
00971       matrix[3][2] = 0.0;
00972    }
00973
00974    matrix[0][3] = 0.0;
00975    matrix[1][3] = 0.0;
00976    matrix[2][3] = 0.0;
00977    matrix[3][3] = 1.0;
00978 }
00979
00980 void AnalyticGeometryTool::print_mtx( double mtx[3][3] )
00981 {
00982    PRINT_INFO( "%f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2] );
00983    PRINT_INFO( "%f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2] );
00984    PRINT_INFO( "%f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2] );
00985 }
00986
00987 void AnalyticGeometryTool::print_mtx( double mtx[4][4] )
00988 {
00989    PRINT_INFO( "%f %f %f %f\n", mtx[0][0], mtx[0][1], mtx[0][2], mtx[0][3] );
00990    PRINT_INFO( "%f %f %f %f\n", mtx[1][0], mtx[1][1], mtx[1][2], mtx[1][3] );
00991    PRINT_INFO( "%f %f %f %f\n", mtx[2][0], mtx[2][1], mtx[2][2], mtx[2][3] );
00992    PRINT_INFO( "%f %f %f %f\n", mtx[3][0], mtx[3][1], mtx[3][2], mtx[3][3] );
00993 }
00994
00995 //***************************************************************************
00996 // 3D Points
00997 //***************************************************************************
00998 void AnalyticGeometryTool::copy_pnt( double pnt_in[3], double pnt_out[3] )
00999 {
01000    if (pnt_in == pnt_out)
01001       return;
01002
01003    if (pnt_out == NULL)
01004       return;
01005
01006    if (pnt_in == NULL) {
01007       pnt_out[0] = 0.0;
01008       pnt_out[1] = 0.0;
01009       pnt_out[2] = 0.0;
01010       return;
01011    }
01012
01013    // Simply copy first point into second point
01014    memcpy(pnt_out, pnt_in, sizeof(double)*3);
01015 }
01016
01017 void AnalyticGeometryTool::copy_pnt( double pnt_in[3], CubitVector &cubit_vec )
01018 {
01019    cubit_vec.set( pnt_in );
01020 }
01021
01022 void AnalyticGeometryTool::copy_pnt( CubitVector &cubit_vec, double pnt_out[3] )
01023 {
01024    pnt_out[0] = cubit_vec.x();
01025    pnt_out[1] = cubit_vec.y();
01026    pnt_out[2] = cubit_vec.z();
01027 }
01028
01029
01030 CubitBoolean AnalyticGeometryTool::pnt_eq( double pnt1[3],double pnt2[3] )
01031 {
01032    double x = pnt2[0] - pnt1[0];  // difference in the x direction
01033    double y = pnt2[1] - pnt1[1];  // difference in the y direction
01034    double z = pnt2[2] - pnt1[2];  // difference in the z direction
01035
01036    return (dbl_eq(sqrt(x*x + y*y + z*z), 0.0));
01037 }
01038
01039
01040 CubitStatus AnalyticGeometryTool::mirror_pnt( double pnt[3],
01041                                              double pln_orig[3],
01042                                              double pln_norm[3],
01043                                              double m_pnt[3])
01044 {
01045    double int_pnt[3],
01046       vec[3];
01047
01048    // Find intersection of point and plane
01049    if (int_pnt_pln(pnt, pln_orig, pln_norm, int_pnt)) {
01050       // If intersection is on the plane, return
01051       copy_pnt(pnt, m_pnt);
01052       return CUBIT_FAILURE;
01053    }
01054
01055    // Find vector from pnt to int_pnt
01056    get_vec(pnt, int_pnt, vec);
01057
01058    // Traverse twice the length of vec in vec direction
01059    m_pnt[0] = pnt[0] + 2.0 * vec[0];
01060    m_pnt[1] = pnt[1] + 2.0 * vec[1];
01061    m_pnt[2] = pnt[2] + 2.0 * vec[2];
01062
01063    return CUBIT_SUCCESS;
01064 }
01065
01066
01067 CubitStatus AnalyticGeometryTool::next_pnt( double str_pnt[3],
01068                                            double vec_dir[3],
01069                                            double len,
01070                                            double new_pnt[3])
01071 {
01072    double uv[3];  // unit vector representation of vector direction
01073
01074    // unitize specified direction
01075    if (!unit_vec(vec_dir,uv)) {
01076       copy_pnt(str_pnt, new_pnt);
01077       return CUBIT_FAILURE;
01078    }
01079
01080    // determine next point in space
01081
01082    new_pnt[0] = str_pnt[0] + (len * uv[0]);
01083    new_pnt[1] = str_pnt[1] + (len * uv[1]);
01084    new_pnt[2] = str_pnt[2] + (len * uv[2]);
01085
01086    return CUBIT_SUCCESS;
01087 }
01088
01089
01090 //***************************************************************************
01091 // 3D Vectors
01092 //***************************************************************************
01093 CubitStatus AnalyticGeometryTool::unit_vec( double vin[3], double vout[3] )
01094 {
01095    double rmag; // holds magnitude of vector
01096
01097    // Calculate vector magnitude
01098    rmag = sqrt(vin[0]*vin[0] + vin[1]*vin[1] + vin[2]*vin[2]);
01099
01100    // check if vector has a magnitude - catch division by zero
01101
01102    if (dbl_eq(rmag, 0.0)) {
01103       if (vin != vout)
01104      copy_pnt(vin, vout);
01105       return CUBIT_FAILURE;
01106    }
01107
01108    // divide each element of the vector by the magnitude
01109
01110    vout[0] = vin[0] / rmag;
01111    vout[1] = vin[1] / rmag;
01112    vout[2] = vin[2] / rmag;
01113
01114    return CUBIT_SUCCESS;
01115 }
01116
01117 double AnalyticGeometryTool::dot_vec( double uval[3], double vval[3] )
01118 {
01119    double dot_val;
01120
01121    // perform dot calculation = v[0]*u[0] + v[1]*u[1] + v[1]*u[1]
01122
01123    dot_val = uval[0]*vval[0] + uval[1]*vval[1] + uval[2]*vval[2];
01124
01125    return(dot_val);
01126 }
01127
01128 void AnalyticGeometryTool::cross_vec( double uval[3], double vval[3],
01129                                      double cross[3] )
01130 {
01131    // determine cross product of the two vectors
01132
01133    cross[0] = uval[1] * vval[2] - uval[2] * vval[1];
01134    cross[1] = uval[2] * vval[0] - uval[0] * vval[2];
01135    cross[2] = uval[0] * vval[1] - uval[1] * vval[0];
01136 }
01137
01138 void AnalyticGeometryTool::cross_vec_unit( double uval[3], double vval[3],
01139                                           double cross[3] )
01140 {
01141    // determine cross product of the two vectors
01142    cross_vec(uval,vval,cross);
01143
01144    // convert to unit vector
01145    unit_vec(cross,cross);
01146 }
01147
01148 void AnalyticGeometryTool::orth_vecs( double uvect[3], double vvect[3],
01149                                      double wvect[3] )
01150 {
01151    double x[3];
01152    unsigned short i = 0;
01153    unsigned short imin = 3;
01154    double rmin = 1.0E20;
01155    unsigned short iperm1[3];
01156    unsigned short iperm2[3];
01157    unsigned short cont_flag = 1;
01158    double vec[3];
01159
01160    // Initialize perm flags
01161    iperm1[0] = 1; iperm1[1] = 2; iperm1[2] = 0;
01162    iperm2[0] = 2; iperm2[1] = 0; iperm2[2] = 1;
01163
01164    // unitize vector
01165
01166    unit_vec(uvect,vec);
01167
01168    while (i<3 && cont_flag ) {
01169       if (dbl_eq(vec[i], 0.0)) {
01170          vvect[i] = 1.0;
01171          vvect[iperm1[i]] = 0.0;
01172          vvect[iperm2[i]] = 0.0;
01173          cont_flag = 0;
01174       }
01175
01176       if (fabs(vec[i]) < rmin) {
01177          imin = i;
01178          rmin = fabs(vec[i]);
01179       }
01180       ++i;
01181    }
01182
01183    if (cont_flag) {
01184       x[imin] = 1.0;
01185       x[iperm1[imin]] = 0.0;
01186       x[iperm2[imin]] = 0.0;
01187
01188       // determine cross product
01189       cross_vec_unit(vec,x,vvect);
01190    }
01191
01192    // cross vector to determine last orthogonal vector
01193    cross_vec(vvect,vec,wvect);
01194 }
01195
01196 double AnalyticGeometryTool::mag_vec( double vec[3] )
01197 {
01198    return (sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]));
01199 }
01200
01201 CubitStatus AnalyticGeometryTool::get_vec ( double str_pnt[3],
01202                                            double stp_pnt[3],
01203                                            double vector_out[3] )
01204 {
01205    // Make sure we can create a vector
01206    if (pnt_eq(str_pnt, stp_pnt)) {
01207       vector_out[0] = 0.0;
01208       vector_out[1] = 0.0;
01209       vector_out[2] = 0.0;
01210       return CUBIT_FAILURE;
01211    }
01212
01213    // determine vector by subtracting starting point from stopping point
01214
01215   vector_out[0] = stp_pnt[0] - str_pnt[0];
01216   vector_out[1] = stp_pnt[1] - str_pnt[1];
01217   vector_out[2] = stp_pnt[2] - str_pnt[2];
01218
01219   return CUBIT_SUCCESS;
01220 }
01221
01222 CubitStatus AnalyticGeometryTool::get_vec_unit( double str_pnt[3],
01223                                                double stp_pnt[3],
01224                                                double uv_out[3] )
01225 {
01226   // determine vector between points
01227    if (!get_vec(str_pnt,stp_pnt,uv_out))
01228       return CUBIT_FAILURE;
01229
01230   // unitize vector
01231   if (!unit_vec(uv_out,uv_out))
01232      return CUBIT_FAILURE;
01233
01234   return CUBIT_SUCCESS;
01235 }
01236
01237 void AnalyticGeometryTool::mult_vecxconst( double constant,
01238                                           double vec[3],
01239                                           double vec_out[3] )
01240 {
01241    // multiply each element of the vector by the constant
01242    vec_out[0] = constant * vec[0];
01243    vec_out[1] = constant * vec[1];
01244    vec_out[2] = constant * vec[2];
01245 }
01246
01247
01248 void AnalyticGeometryTool::reverse_vec( double vin[3],double vout[3] )
01249 {
01250    // Multiply the vector components by a -1.0
01251    mult_vecxconst(-1.0, vin, vout);
01252 }
01253
01254 double AnalyticGeometryTool::angle_vec_vec( double v1[3],double v2[3] )
01255 {
01256   double denom, dot, cosang, sinang, acrsb, angle;
01257   double crossed_vec[3];
01258
01259   // For accuracy, use cosine for large angles, sine for small angles
01260   denom = sqrt((v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2])
01261               *(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]));
01262
01263   // Check for a zero length vector
01264   if (dbl_eq(denom, 0.0))
01265      return (0.0);
01266
01267   dot = dot_vec(v1, v2);
01268
01269   cosang = dot/denom;
01270
01271   if (1.0 - fabs(cosang) < 0.01) {
01272      cross_vec(v1, v2, crossed_vec);
01273      acrsb = mag_vec(crossed_vec);
01274      sinang = acrsb/denom;
01275      if (cosang > 0.0)
01276         angle = asin(sinang);
01277      else
01278         angle = AGT_PI - asin(sinang);
01279   }
01280   else
01281      angle = acos(cosang);
01282
01283   return (angle);
01284 }
01285
01286 //***************************************************************************
01287 // Distances
01288 //***************************************************************************
01289 double AnalyticGeometryTool::dist_pnt_pnt( double pnt1[3], double pnt2[3] )
01290 {
01291   double x = pnt2[0] - pnt1[0];  // difference in the x direction
01292   double y = pnt2[1] - pnt1[1];  // difference in the y direction
01293   double z = pnt2[2] - pnt1[2];  // difference in the z direction
01294
01295   // return the distance
01296   return(sqrt(x*x + y*y + z*z));
01297 }
01298
01299
01300
01301 double AnalyticGeometryTool::dist_pln_pln( double pln_1_orig[3],
01302                                           double pln_1_norm[3],
01303                                           double pln_2_orig[3],
01304                                           double pln_2_norm[3],
01305                                           AgtSide *side,
01306                                           AgtOrientation *orien,
01307                                           unsigned short *status )
01308 {
01309    double distance;
01310    double int_pnt[3];
01311    double vec[3];
01312
01313    // Check to see if planes are parallel
01314    if (is_vec_par(pln_1_norm, pln_2_norm)) {
01315
01316       // Set successful status
01317       if (status)
01318          *status = 1;
01319
01320       // Calculate perpendicular line plane intersection on plane_2 from
01321       // pln_1_origin
01322       int_ln_pln(pln_1_orig, pln_1_norm, pln_2_orig, pln_2_norm,
01323                  int_pnt);
01324
01325       // Find distance between pln_1_origin and this intersection pnt
01326       distance = dist_pnt_pnt(pln_1_orig, int_pnt);
01327
01328       // Get side if required
01329       if (side) {
01330
01331          if (dbl_eq(distance, 0.0))
01332
01333             *side = AGT_ON_PLANE;
01334
01335          else {
01336
01337             // Get vector to intersection point
01338             get_vec(pln_1_orig, int_pnt, vec);
01339
01340             // Compare angles
01341             if (dbl_eq(angle_vec_vec(vec, pln_1_norm), 0.0))
01342                *side = AGT_POS_SIDE;
01343             else
01344                *side = AGT_NEG_SIDE;
01345
01346          }
01347       }
01348
01349       // Get orientation if required
01350       if (orien) {
01351
01352          // Compare surface normals
01353          if (dbl_eq(angle_vec_vec(pln_1_norm, pln_2_norm), 0.0))
01354             *orien = AGT_SAME_DIR;
01355          else
01356             *orien = AGT_OPP_DIR;
01357       }
01358
01359    }
01360
01361    else {
01362
01363       if (status)
01364          *status = 0;
01365
01366       if (side)
01367          *side = AGT_CROSS;
01368
01369       distance = 0.0;
01370
01371    }
01372
01373    return (distance);
01374 }
01375
01376
01377
01378 //***************************************************************************
01379 // Intersections
01380 //***************************************************************************
01381 CubitStatus AnalyticGeometryTool::int_ln_pln( double ln_orig[3],
01382                                              double ln_vec[3],
01383                                              double pln_orig[3],
01384                                              double pln_norm[3],
01385                                              double int_pnt[3] )
01386 {
01387    double denom;
01388    double t;
01389
01390    // Set parametric eqns of line equal to parametric eqn of plane & solve
01391    // for t
01392    denom = pln_norm[0]*ln_vec[0] + pln_norm[1]*ln_vec[1] +
01393            pln_norm[2]*ln_vec[2];
01394
01395    if (dbl_eq(denom, 0.0))
01396       return CUBIT_FAILURE;
01397
01398    t = (pln_norm[0]*(pln_orig[0]-ln_orig[0]) +
01399       pln_norm[1]*(pln_orig[1]-ln_orig[1]) +
01400       pln_norm[2]*(pln_orig[2]-ln_orig[2]))/denom;
01401
01402    // Substitute t back into equations of line to get xyz
01403    int_pnt[0] = ln_orig[0] + ln_vec[0]*t;
01404    int_pnt[1] = ln_orig[1] + ln_vec[1]*t;
01405    int_pnt[2] = ln_orig[2] + ln_vec[2]*t;
01406
01407    return CUBIT_SUCCESS;
01408 }
01409
01410 int AnalyticGeometryTool::int_ln_ln( double p1[3], double v1[3],
01411                                     double p2[3], double v2[3],
01412                                     double int_pnt1[3], double int_pnt2[3] )
01413 {
01414    double norm[3];
01415    double pln1_norm[3];
01416    double pln2_norm[3];
01417
01418    // Cross the two vectors to get a normal vector
01419    cross_vec(v1, v2, norm);
01420
01421    if (dbl_eq(mag_vec(norm), 0.0))
01422       return 0;
01423
01424    // Cross v2 & normal to get normal to plane 2
01425    cross_vec(v2, norm, pln2_norm);
01426
01427    // Find intersection of pln2_norm and vector 1 - this is int_pnt1
01428    int_ln_pln(p1, v1, p2, pln2_norm, int_pnt1);
01429
01430    // Cross v1 & normal to get normal to plane 1
01431    cross_vec(v1, norm, pln1_norm);
01432
01433    // Find intersection of pln2_norm and vector 1 - this is int_pnt2
01434    int_ln_pln(p2, v2, p1, pln1_norm, int_pnt2);
01435
01436    // Check to see if the intersection points are the same & return
01437    if (pnt_eq(int_pnt1, int_pnt2))
01438       return 1;
01439    else
01440       return 2;
01441 }
01442
01443
01444 int AnalyticGeometryTool::int_pnt_pln( double pnt[3],
01445                                       double pln_orig[3],
01446                                       double pln_norm[3],
01447                                       double pln_int[3] )
01448 {
01449    // Calculate line plane intersection w/plane normal as line vector
01450    int_ln_pln(pnt, pln_norm, pln_orig, pln_norm, pln_int);
01451
01452    // Check to see if point is on the plane
01453    if (pnt_eq(pln_int, pnt))
01454       return 1;
01455    else
01456       return 0;
01457 }
01458
01459
01460
01461 //***************************************************************************
01462 // Comparison/Containment Tests
01463 //***************************************************************************
01464 CubitBoolean AnalyticGeometryTool::is_vec_par( double vec_1[3],
01465                                               double vec_2[3] )
01466 {
01467    double cross[3];
01468
01469    // Get cross product & see if its magnitude is zero
01470    cross_vec(vec_1, vec_2, cross);
01471
01472    if (dbl_eq(mag_vec(cross), 0.0))
01473       return CUBIT_TRUE;
01474    else
01475       return CUBIT_FALSE;
01476 }
01477
01478 CubitBoolean AnalyticGeometryTool::is_vec_perp( double vec_1[3],double vec_2[3])
01479 {
01480    // Check angle between vectors
01481    if (dbl_eq(angle_vec_vec(vec_1, vec_2), AGT_PI_DIV_2))
01482       return CUBIT_TRUE;
01483    else
01484       return CUBIT_FALSE;
01485 }
01486
01487 CubitBoolean AnalyticGeometryTool::is_vecs_same_dir( double vec_1[3],
01488                                                      double vec_2[3] )
01489 {
01490    // Check to see if angle between vectors can be considered zero
01491    if (dbl_eq(angle_vec_vec(vec_1, vec_2), 0.0))
01492       return CUBIT_TRUE;
01493    else
01494       return CUBIT_FALSE;
01495 }
01496
01497
01498 CubitBoolean AnalyticGeometryTool::is_pnt_on_ln( double pnt[3],
01499                                                 double ln_orig[3],
01500                                                 double ln_vec[3] )
01501 {
01502    double vec[3];
01503
01504    // Compare pnt and line origin
01505    if (pnt_eq(pnt, ln_orig))
01506       return CUBIT_TRUE;
01507
01508    // Get a vector from line origin to the point
01509    get_vec(ln_orig, pnt, vec);
01510
01511    // If this vector is parallel with line vector, point is on the line
01512    if (is_vec_par(vec, ln_vec))
01513       return CUBIT_TRUE;
01514    else
01515       return CUBIT_FALSE;
01516 }
01517
01518 CubitBoolean AnalyticGeometryTool::is_pnt_on_ln_seg( double pnt[3],
01519                                                     double end1[3],
01520                                                     double end2[3] )
01521 {
01522 //   METHOD:
01523 //    o Use parametric equations of line
01524 //
01525 //         x = x1 + t(x2 - x1)
01526 //         y = y1 + t(y2 - y1)
01527 //         z = z1 + t(z2 - z1)
01528 //
01529 //    o Note:  two other method's were considered:
01530 //       1) Comparing sum of distance of point to both end points to the
01531 //          line length.
01532 //       2) Checking to see if area of a triangle with the vertices is zero
01533 //
01534 //     Using parametric equations is more efficient in many cases.
01535   double t1 = 0.0,
01536     t2 = 0.0,
01537     t3 = 0.0,
01538     neg_range,
01539     pos_range;
01540
01541   unsigned short flg1 = 0,
01542     flg2 = 0,
01543     flg3 = 0;
01544
01545   neg_range = 0.0 - agtEpsilon;
01546   pos_range = 1.0 + agtEpsilon;
01547
01548   if (fabs(end2[0] - end1[0]) < agtEpsilon)
01549   {
01550     if (fabs(pnt[0] - end1[0]) < agtEpsilon)
01551       flg1 = 1;
01552     else
01553       return CUBIT_FALSE;
01554   }
01555   else
01556   {
01557     t1 = (pnt[0] - end1[0])/(end2[0] - end1[0]);
01558
01559     if (t1<neg_range || t1>pos_range)
01560       return CUBIT_FALSE;
01561   }
01562
01563   if (fabs(end2[1] - end1[1]) < agtEpsilon)
01564   {
01565     if (fabs(pnt[1] - end1[1]) < agtEpsilon)
01566       flg2 = 1;
01567     else
01568       return CUBIT_FALSE;
01569   }
01570   else
01571   {
01572     t2 = (pnt[1] - end1[1])/(end2[1] - end1[1]);
01573
01574     if (t2<neg_range || t2>pos_range)
01575       return CUBIT_FALSE;
01576   }
01577
01578   if (fabs(end2[2] - end1[2]) < agtEpsilon)
01579   {
01580     if (fabs(pnt[2] - end1[2]) < agtEpsilon)
01581       flg3 = 1;
01582     else
01583       return CUBIT_FALSE;
01584   }
01585   else
01586   {
01587     t3 = (pnt[2] - end1[2])/(end2[2] - end1[2]);
01588
01589     if (t3<neg_range || t3>pos_range)
01590       return CUBIT_FALSE;
01591   }
01592
01593     // If any 2 flags are 1, point is on the line,
01594     // otherwise, check remaining T's for equality
01595
01596   if (flg1)
01597   {
01598       // Here, flg1 = 1
01599
01600     if (flg2)
01601     {
01602         // Here, flg1 = 1
01603         // Here, flg2 = 1
01604       return CUBIT_TRUE;
01605     }
01606     else
01607     {
01608         // Here, flg1 = 1
01609         // Here, flg2 = 0
01610
01611       if (flg3)
01612           // Here, flg1 = 1
01613           // Here, flg2 = 0
01614           // Here, flg3 = 1
01615         return CUBIT_TRUE;
01616       else
01617       {
01618           // Here, flg1 = 1
01619           // Here, flg2 = 0
01620           // Here, flg3 = 0
01621         if (dbl_eq(t2, t3))
01622           return CUBIT_TRUE;
01623         else
01624           return CUBIT_FALSE;
01625       }
01626     }
01627   }
01628   else
01629   {
01630       // Here, flg1 = 0
01631     if (flg2)
01632     {
01633         // Here, flg1 = 0
01634         // Here, flg2 = 1
01635       if (flg3)
01636         return CUBIT_TRUE;
01637           // Here, flg1 = 0
01638           // Here, flg2 = 1
01639           // Here, flg3 = 1
01640       else
01641       {
01642           // Here, flg1 = 0
01643           // Here, flg2 = 1
01644           // Here, flg3 = 0
01645         if (dbl_eq(t1, t3))
01646           return CUBIT_TRUE;
01647         else
01648           return CUBIT_FALSE;
01649       }
01650     }
01651     else
01652     {
01653         // Here, flg1 = 0
01654         // Here, flg2 = 0
01655       if (flg3)
01656       {
01657           // Here, flg1 = 0
01658           // Here, flg2 = 0
01659           // Here, flg3 = 1
01660         if (dbl_eq(t1, t2))
01661           return CUBIT_TRUE;
01662         else
01663           return CUBIT_FALSE;
01664       }
01665       else
01666       {
01667           // Here, flg1 = 0
01668           // Here, flg2 = 0
01669           // Here, flg3 = 0
01670         if (dbl_eq(t1, t2) && dbl_eq(t1, t3))
01671           return CUBIT_TRUE;
01672         else
01673           return CUBIT_FALSE;
01674       }
01675     }
01676   }
01677
01678     // This would be a programmer's error if we got to this point
01679 //  return CUBIT_FALSE;
01680 }
01681
01682 CubitBoolean AnalyticGeometryTool::is_pnt_on_pln( double pnt[3],
01683                                                  double pln_orig[3],
01684                                                  double pln_norm[3] )
01685 {
01686    double result;
01687
01688    // See if point satisfies parametric equation of plane
01689
01690    result = pln_norm[0] * (pnt[0] - pln_orig[0]) +
01691             pln_norm[1] * (pnt[1] - pln_orig[1]) +
01692             pln_norm[2] * (pnt[2] - pln_orig[2]);
01693
01694    if (dbl_eq(result, 0.0))
01695       return CUBIT_TRUE;
01696    else
01697       return CUBIT_FALSE;
01698 }
01699
01700 CubitBoolean AnalyticGeometryTool::is_ln_on_pln( double ln_orig[3],
01701                                                 double ln_vec[3],
01702                                                 double pln_orig[3],
01703                                                 double pln_norm[3] )
01704 {
01705
01706    // Check to see if line origin is on the plane.  If so, check to see if
01707    // line vector is perpendicular to plane normal.
01708
01709    if (is_pnt_on_pln(ln_orig, pln_orig, pln_norm) &&
01710        is_vec_perp(ln_vec, pln_norm))
01711       return CUBIT_TRUE;
01712    else
01713       return CUBIT_FALSE;
01714 }
01715
01716
01717
01718 CubitBoolean AnalyticGeometryTool::is_pln_on_pln( double pln_orig1[3],
01719                                                  double pln_norm1[3],
01720                                                  double pln_orig2[3],
01721                                                  double pln_norm2[3] )
01722 {
01723    // If 1st plane origin is on the 2nd plane and the normals are
01724    // parallel they are coincident
01725    if( is_vec_par( pln_norm1, pln_norm2 ) &&
01726       is_pnt_on_pln( pln_orig1, pln_orig2, pln_norm2 ) )
01727       return CUBIT_TRUE;
01728    else
01729       return CUBIT_FALSE;
01730 }
01731
01732 //***************************************************************************
01733 // Arcs/Circles
01734 //***************************************************************************
01735 void AnalyticGeometryTool::setup_arc( double vec_1[3], double vec_2[3],
01736                                      double origin[3], double start_angle,
01738                                      AGT_3D_Arc &arc )
01739 {
01740    copy_pnt( vec_1, arc.Vec1 );
01741    copy_pnt( vec_2, arc.Vec2 );
01742    copy_pnt( origin, arc.Origin );
01743    arc.StartAngle = start_angle;
01744    arc.EndAngle = end_angle;
01746 }
01747
01748 void AnalyticGeometryTool::setup_arc( CubitVector& vec_1, CubitVector& vec_2,
01749                                      CubitVector origin, double start_angle,
01751                                      AGT_3D_Arc &arc )
01752 {
01753    vec_1.get_xyz( arc.Vec1 );
01754    vec_2.get_xyz( arc.Vec2 );
01755    origin.get_xyz( arc.Origin );
01756    arc.StartAngle = start_angle;
01757    arc.EndAngle = end_angle;
01759 }
01760
01761 void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, double pnt[3] )
01762 {
01763    double Tp;
01764
01765    // Un-normalized parameter
01766    Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
01767
01768    // Solve for XYZ
01769    pnt[0] = arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
01770                            sin( Tp ) * arc.Vec2[0] ) +
01771                            arc.Origin[0];
01772
01773    pnt[1] = arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
01774                            sin( Tp ) * arc.Vec2[1] ) +
01775                            arc.Origin[1];
01776
01777    pnt[2] = arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
01778                            sin( Tp ) * arc.Vec2[2] ) +
01779                            arc.Origin[2];
01780 }
01781
01782 void AnalyticGeometryTool::get_arc_xyz( AGT_3D_Arc &arc, double param, CubitVector& pnt )
01783 {
01784    double Tp;
01785
01786    // Un-normalized parameter
01787    Tp = arc.StartAngle * ( 1.0 - param ) + arc.EndAngle * param;
01788
01789    // Solve for XYZ
01790    pnt.x( arc.Radius * ( cos( Tp ) * arc.Vec1[0] +
01791                          sin( Tp ) * arc.Vec2[0] ) +
01792                          arc.Origin[0] );
01793
01794    pnt.y( arc.Radius * ( cos( Tp ) * arc.Vec1[1] +
01795                          sin( Tp ) * arc.Vec2[1] ) +
01796                          arc.Origin[1] );
01797
01798    pnt.z( arc.Radius * ( cos( Tp ) * arc.Vec1[2] +
01799                          sin( Tp ) * arc.Vec2[2] ) +
01800                          arc.Origin[2] );
01801 }
01802
01803 int
01805                                                 double len_tol )
01806 {
01807   double cmin, cmax;
01808
01809   double c = 2*CUBIT_PI*radius; // Circumference
01810
01811   // Find the number of points required for the given accuracy.  Use
01812   // a bisection method.
01813   int nmin = 8, nmax = 100;
01814   cmin = 2.0*nmin*radius*sin(CUBIT_PI/nmin); // Circumference of circle using segments
01816
01817   if( dbl_eq( cmin, c ) )
01818     return nmin;
01819
01820   double old_epsilon = set_epsilon( len_tol );
01821
01822   // Find an n that is more than accurate enough
01823   while( !dbl_eq( cmax, c ) )
01824   {
01825     nmin = nmax;
01826     nmax = nmin * 10;
01829   }
01830
01831   // Biscect until the minimum number of segments satisfying
01832   // the tolerance is found.
01833   int n;
01834   while( 1 )
01835   {
01836     n = (nmin + nmax)/2;
01838     if( dbl_eq( cn, c ) )
01839     {
01840       // Go lower
01841       nmax = n;
01842     }
01843     else
01844     {
01845       // Go higher
01846       nmin = n;
01847     }
01848     if( nmax-nmin < 2 )
01849       break;
01850   }
01851   set_epsilon( old_epsilon );
01852
01853   return nmax;
01854 }
01855
01856 //***************************************************************************
01857 // Miscellaneous
01858 //***************************************************************************
01859 void AnalyticGeometryTool::get_pln_orig_norm( double A, double B, double C,
01860                                              double D, double pln_orig[3],
01861                                              double pln_norm[3] )
01862 {
01863    double x = 0.0, y = 0.0, z = 0.0;
01864
01865    // Try to have origin aligned with one of the principal axes
01866    if( !dbl_eq( C, 0.0 ) )
01867       z = -D/C;
01868    else if (!dbl_eq( A, 0.0 ) )
01869       x = -D/A;
01870    else if (!dbl_eq( B, 0.0 ) )
01871       y = -D/B;
01872
01873    pln_orig[0] = x;
01874    pln_orig[1] = y;
01875    pln_orig[2] = z;
01876
01877    if( pln_norm )
01878    {
01879       pln_norm[0] = A;
01880       pln_norm[1] = B;
01881       pln_norm[2] = C;
01882    }
01883 }
01884
01885 void AnalyticGeometryTool::get_box_corners( double box_min[3],
01886                                            double box_max[3],
01887                                            double c[8][3] )
01888 {
01889    // Left-Bottom-Front       // Left-Top-Front
01890    c[0][0] = box_min[0];      c[1][0] = box_min[0];
01891    c[0][1] = box_min[1];      c[1][1] = box_max[1];
01892    c[0][2] = box_max[2];      c[1][2] = box_max[2];
01893
01894    // Right-Top-Front         // Right-Bottom-Front
01895    c[2][0] = box_max[0];      c[3][0] = box_max[0];
01896    c[2][1] = box_max[1];      c[3][1] = box_min[1];
01897    c[2][2] = box_max[2];      c[3][2] = box_max[2];
01898
01899    // Left-Bottom-Back        // Left-Top-Back
01900    c[4][0] = box_min[0];      c[5][0] = box_min[0];
01901    c[4][1] = box_min[1];      c[5][1] = box_max[1];
01902    c[4][2] = box_min[2];      c[5][2] = box_min[2];
01903
01904    // Right-Top-Back          // Right-Bottom-Back
01905    c[6][0] = box_max[0];      c[7][0] = box_max[0];
01906    c[6][1] = box_max[1];      c[7][1] = box_min[1];
01907    c[6][2] = box_min[2];      c[7][2] = box_min[2];
01908
01909 }
01910
01911 CubitStatus
01912 AnalyticGeometryTool::min_pln_box_int_corners( const CubitPlane& plane,
01913                                               const CubitBox& box,
01914                                               int extension_type,
01915                                               double extension,
01916                                               CubitVector& p1, CubitVector& p2,
01917                                               CubitVector& p3, CubitVector& p4,
01918                                               CubitBoolean silent )
01919 {
01920    CubitVector box_min = box.minimum();
01921    CubitVector box_max = box.maximum();
01922
01923    CubitVector plane_norm = plane.normal();
01924
01925    double box_min_pnt[3], box_max_pnt[3], pln_norm[3];
01926    box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
01927    plane_norm.get_xyz( pln_norm );
01928
01929    double pnt1[3], pnt2[3], pnt3[3], pnt4[3];
01930
01931    if( min_pln_box_int_corners( pln_norm, plane.coefficient(),
01932       box_min_pnt, box_max_pnt,
01933       extension_type, extension,
01934       pnt1, pnt2, pnt3, pnt4, silent ) == CUBIT_FAILURE )
01935          return CUBIT_FAILURE;
01936
01937    p1.set( pnt1 ); p2.set( pnt2 ); p3.set( pnt3 ); p4.set( pnt4 );
01938
01939    return CUBIT_SUCCESS;
01940 }
01941
01942 CubitStatus
01943 AnalyticGeometryTool::min_pln_box_int_corners( CubitVector& vec1,
01944                                               CubitVector& vec2,
01945                                               CubitVector& vec3,
01946                                               CubitVector& box_min,
01947                                               CubitVector& box_max,
01948                                               int extension_type,
01949                                               double extension,
01950                                               CubitVector& p1, CubitVector& p2,
01951                                               CubitVector& p3, CubitVector& p4,
01952                                               CubitBoolean silent )
01953 {
01954    CubitPlane plane;
01955    if( plane.mk_plane_with_points( vec1, vec2, vec3 ) == CUBIT_FAILURE )
01956       return CUBIT_FAILURE;
01957
01958    CubitVector plane_norm = plane.normal();
01959    double coefficient = plane.coefficient();
01960
01961    double plane_norm3[3];
01962    double box_min3[3];
01963    double box_max3[3];
01964
01965    box_min.get_xyz( box_min3 );
01966    box_max.get_xyz( box_max3 );
01967    plane_norm.get_xyz( plane_norm3 );
01968
01969    double p1_3[3], p2_3[3], p3_3[3], p4_3[3];
01970    p1.get_xyz( p1_3 );
01971    p2.get_xyz( p2_3 );
01972    p3.get_xyz( p3_3 );
01973    p4.get_xyz( p4_3 );
01974
01975    if( min_pln_box_int_corners( plane_norm3, coefficient, box_min3, box_max3,
01976       extension_type, extension, p1_3, p2_3, p3_3, p4_3, silent ) == CUBIT_FAILURE )
01977       return CUBIT_FAILURE;
01978
01979    p1.set( p1_3 );
01980    p2.set( p2_3 );
01981    p3.set( p3_3 );
01982    p4.set( p4_3 );
01983
01984    return CUBIT_SUCCESS;
01985 }
01986
01987 CubitStatus
01988 AnalyticGeometryTool::min_pln_box_int_corners( double pln_norm[3],
01989                                               double pln_coeff,
01990                                               double box_min[3],
01991                                               double box_max[3],
01992                                               int extension_type,
01993                                               double extension,
01994                                               double p1[3], double p2[3],
01995                                               double p3[3], double p4[3],
01996                                               CubitBoolean silent )
01997 {
01998    int i;
01999    double cubit2pln_mtx[4][4],
02000       pln2cubit_mtx[4][4];
02001    double pln_orig[3];
02002    double box_tol = 1e-6;
02003
02004    // Adjust bounding box if it is zero in any direction
02005    double abox_min[3], abox_max[3];
02006    copy_pnt( box_min, abox_min );
02007    copy_pnt( box_max, abox_max );
02008
02009    double x_range = fabs(box_max[0]-box_min[0]);
02010    double y_range = fabs(box_max[1]-box_min[1]);
02011    double z_range = fabs(box_max[2]-box_min[2]);
02012
02013    if( x_range < box_tol || y_range < box_tol || z_range < box_tol )
02014    {
02015      // Adjust zero length side(s) to maximum range
02017
02018      // Arbitrarily expand box if it is zero in size
02019      if( adj < box_tol )
02021
02022      if( x_range < box_tol )
02023      {
02026      }
02027      if( y_range < box_tol )
02028      {
02031      }
02032      if( z_range < box_tol )
02033      {
02036      }
02037    }
02038
02039    // Get plane origin
02040    double A = pln_norm[0];
02041    double B = pln_norm[1];
02042    double C = pln_norm[2];
02043    double D = pln_coeff;
02044    get_pln_orig_norm( A, B, C, D, pln_orig );
02045
02046    // Find intersections of edges with plane.  Add to unique array.  At most
02047    // there are 6 intersections...
02048    double int_array[6][3];
02049    int num_int = 0;
02050    num_int = get_plane_bbox_intersections( abox_min, abox_max, pln_orig, pln_norm,
02051      int_array );
02052
02053    // Adjust so plane intercepts bounding box
02054    if( num_int < 3 )
02055    {
02056      // Move the plane to the center of the bounding box
02057      double box_cent[3];
02058      box_cent[0] = (abox_min[0]+abox_max[0])/2.0;
02059      box_cent[1] = (abox_min[1]+abox_max[1])/2.0;
02060      box_cent[2] = (abox_min[2]+abox_max[2])/2.0;
02061
02062      // Get the intersections
02063      num_int = get_plane_bbox_intersections( abox_min, abox_max, box_cent, pln_norm, int_array );
02064
02065      // Project the points back to the original plane
02066      switch (num_int)
02067      {
02068      case 6:
02069        int_pnt_pln( int_array[5], pln_orig, pln_norm, int_array[5] );
02070      case 5:
02071        int_pnt_pln( int_array[4], pln_orig, pln_norm, int_array[4] );
02072      case 4:
02073        int_pnt_pln( int_array[3], pln_orig, pln_norm, int_array[3] );
02074      case 3:
02075        int_pnt_pln( int_array[2], pln_orig, pln_norm, int_array[2] );
02076      case 2:
02077        int_pnt_pln( int_array[1], pln_orig, pln_norm, int_array[1] );
02078      case 1:
02079        int_pnt_pln( int_array[0], pln_orig, pln_norm, int_array[0] );
02080      }
02081    }
02082
02083    if( num_int == 0 )
02084    {
02085      if( silent == CUBIT_FALSE )
02086        PRINT_ERROR( "Plane does not intersect the bounding box\n"
02087        "       Can't find 4 corners of plane\n" );
02088      return CUBIT_FAILURE;
02089    }
02090    if( num_int < 3 )
02091    {
02092      if( silent == CUBIT_FALSE )
02093        PRINT_ERROR( "Plane intersects the bounding box at only %d locations\n"
02094        "      Can't calculate 4 corners of plane\n", num_int );
02095       return CUBIT_FAILURE;
02096    }
02097
02098    // Transform pnts to plane coordinate system
02099    double pln_x[3], pln_y[3];
02100    orth_vecs( pln_norm, pln_x, pln_y );
02101    vecs_to_mtx( pln_x, pln_y, pln_norm, pln_orig, pln2cubit_mtx );
02102    inv_trans_mtx( pln2cubit_mtx, cubit2pln_mtx );
02103
02104    double int_arr_pln[6][3];
02105    for( i=0; i<num_int; i++ )
02106       transform_pnt( cubit2pln_mtx, int_array[i], int_arr_pln[i] );
02107
02108    for(i=0; i<num_int; i++)
02109    {
02110      if( !dbl_eq( int_arr_pln[i][2], 0.0 ) )
02111      {
02112        if( silent == CUBIT_FALSE )
02113          PRINT_ERROR( "in AnalyticGeometryTool::min_box_pln_int_corners\n"
02114          "       Transform to plane wrong\n" );
02115         return CUBIT_FAILURE;
02116      }
02117    }
02118
02119    // Place into format for mimimal box calculation
02120    Point2 pt[6];
02121    for ( i=0; i<num_int; i++ )
02122    {
02123       pt[i].x = int_arr_pln[i][0];
02124       pt[i].y = int_arr_pln[i][1];
02125    }
02126
02127    // Find rectangle with minimal area to surround the points
02128    // (this is definitely overkill esp. for the simple cases.....)
02129    OBBox2 minimal = MinimalBox2( num_int, pt );
02130
02131    // Strip out results
02132    double old_epsilon = set_epsilon( 1e-10 );
02133    double centroid[3];
02134    centroid[0] = minimal.center.x;
02135    centroid[1] = minimal.center.y;
02136    centroid[2] = 0.0;
02137    round_near_val( centroid[0] ); // Makes near -1, 0, 1 values -1, 0, 1
02138    round_near_val( centroid[1] );
02139    transform_pnt( pln2cubit_mtx, centroid, centroid );
02140
02141    double x_axis[3];
02142    x_axis[0] = minimal.axis[0].x;
02143    x_axis[1] = minimal.axis[0].y;
02144    x_axis[2] = 0.0;
02145    round_near_val( x_axis[0] );
02146    round_near_val( x_axis[1] );
02147    transform_vec( pln2cubit_mtx, x_axis, x_axis );
02148
02149    double y_axis[3];
02150    y_axis[0] = minimal.axis[1].x;
02151    y_axis[1] = minimal.axis[1].y;
02152    y_axis[2] = 0.0;
02153    round_near_val( y_axis[0] );
02154    round_near_val( y_axis[1] );
02155    transform_vec( pln2cubit_mtx, y_axis, y_axis );
02156
02157    set_epsilon( old_epsilon );
02158
02159    double dist_x;
02160    double dist_y;
02161    double extension_distance = 0.0;
02162    if( extension_type == 1 ) // Percentage (of 1/2 diagonal)
02163    {
02164       double diag_len = sqrt(  minimal.extent[0]*minimal.extent[0]
02165                              + minimal.extent[1]*minimal.extent[1] );
02166       extension_distance = diag_len*extension/100.0;
02167    }
02168    else if( extension_type == 2 ) // Absolute distance in x and y
02169       extension_distance = extension;
02170
02171    dist_x = minimal.extent[0] + extension_distance;
02172    dist_y = minimal.extent[1] + extension_distance;
02173
02174    next_pnt( centroid, x_axis, -dist_x, p1 );
02175    next_pnt( p1, y_axis, -dist_y, p1 );
02176
02177    next_pnt( centroid, x_axis, -dist_x, p2 );
02178    next_pnt( p2, y_axis, dist_y, p2 );
02179
02180    next_pnt( centroid, x_axis, dist_x, p3 );
02181    next_pnt( p3, y_axis, dist_y, p3 );
02182
02183    next_pnt( centroid, x_axis, dist_x, p4 );
02184    next_pnt( p4, y_axis, -dist_y, p4 );
02185
02186    return CUBIT_SUCCESS;
02187 }
02188
02189 int AnalyticGeometryTool::get_plane_bbox_intersections( double box_min[3],
02190                                                         double box_max[3],
02191                                                         double pln_orig[3],
02192                                                         double pln_norm[3],
02193                                                         double int_array[6][3])
02194 {
02195    // Fill in an array with all 8 box corners
02196    double corner[8][3];
02197    get_box_corners( box_min, box_max, corner );
02198
02199    // Get 12 edges of the box
02200    double ln_start[12][3], ln_end[12][3];
02201    copy_pnt( corner[0], ln_start[0] );  copy_pnt( corner[1], ln_end[0] );
02202    copy_pnt( corner[1], ln_start[1] );  copy_pnt( corner[2], ln_end[1] );
02203    copy_pnt( corner[2], ln_start[2] );  copy_pnt( corner[3], ln_end[2] );
02204    copy_pnt( corner[3], ln_start[3] );  copy_pnt( corner[0], ln_end[3] );
02205    copy_pnt( corner[4], ln_start[4] );  copy_pnt( corner[5], ln_end[4] );
02206    copy_pnt( corner[5], ln_start[5] );  copy_pnt( corner[6], ln_end[5] );
02207    copy_pnt( corner[6], ln_start[6] );  copy_pnt( corner[7], ln_end[6] );
02208    copy_pnt( corner[7], ln_start[7] );  copy_pnt( corner[4], ln_end[7] );
02209    copy_pnt( corner[0], ln_start[8] );  copy_pnt( corner[4], ln_end[8] );
02210    copy_pnt( corner[1], ln_start[9] );  copy_pnt( corner[5], ln_end[9] );
02211    copy_pnt( corner[2], ln_start[10] ); copy_pnt( corner[6], ln_end[10] );
02212    copy_pnt( corner[3], ln_start[11] ); copy_pnt( corner[7], ln_end[11] );
02213
02214    double ln_vec[3];
02215    double int_pnt[3];
02216    int num_int = 0;
02217    int i, j, found;
02218    for( i=0; i<12; i++ )
02219    {
02220       get_vec_unit( ln_start[i], ln_end[i], ln_vec );
02221       if( int_ln_pln( ln_start[i], ln_vec, pln_orig, pln_norm, int_pnt ) )
02222       {
02223          // Only add if on the bounded line segment
02224          if( is_pnt_on_ln_seg( int_pnt, ln_start[i], ln_end[i] ) )
02225          {
02226             // Only add if unique
02227             found = 0;
02228             for( j=0; j<num_int; j++ )
02229             {
02230                if( pnt_eq( int_pnt, int_array[j] ) )
02231                {
02232                   found = 1;
02233                   break;
02234                }
02235             }
02236             if( !found )
02237             {
02238                copy_pnt( int_pnt, int_array[num_int] );
02239                num_int++;
02240             }
02241          }
02242       }
02243    }
02244   return num_int;
02245 }
02246
02247 CubitStatus
02248 AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,
02249                                               CubitVector &center,
02250                                               CubitVector axes[3],
02251                                               CubitVector &extension )
02252 {
02253    int num_pnts = point_list.size();
02254    if( num_pnts == 0 )
02255       return CUBIT_FAILURE;
02256    Point3 *pnt_arr = new Point3[num_pnts];
02257
02258    int i;
02259    point_list.reset();
02260    CubitVector *vec;
02261    for( i=0; i<num_pnts; i++ )
02262    {
02263       vec = point_list.get_and_step();
02264       //GfxPreview::draw_point( vec, 3 );
02265
02266       pnt_arr[i].x = vec->x();
02267       pnt_arr[i].y = vec->y();
02268       pnt_arr[i].z = vec->z();
02269    }
02270
02271    OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);
02272
02273    //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
02274    //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
02275    //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
02276    //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
02277    //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );
02278
02279    center.set(minimal.center.x, minimal.center.y, minimal.center.z);
02280    axes[0].set(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
02281    axes[1].set(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
02282    axes[2].set(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
02283    extension.set(minimal.extent[0], minimal.extent[1], minimal.extent[2]);
02284
02285    delete [] pnt_arr;
02286
02287    return CUBIT_SUCCESS;
02288 }
02289
02290 CubitStatus
02291 AnalyticGeometryTool::get_tight_bounding_box( DLIList<CubitVector*> &point_list,
02292                                               CubitVector& p1, CubitVector& p2,
02293                                               CubitVector& p3, CubitVector& p4,
02294                                               CubitVector& p5, CubitVector& p6,
02295                                               CubitVector& p7, CubitVector& p8)
02296 {
02297    int num_pnts = point_list.size();
02298    if( num_pnts == 0 )
02299       return CUBIT_FAILURE;
02300    Point3 *pnt_arr = new Point3[num_pnts];
02301
02302    int i;
02303    point_list.reset();
02304    CubitVector *vec;
02305    for( i=0; i<num_pnts; i++ )
02306    {
02307       vec = point_list.get_and_step();
02308
02309       pnt_arr[i].x = vec->x();
02310       pnt_arr[i].y = vec->y();
02311       pnt_arr[i].z = vec->z();
02312    }
02313
02314    OBBox3 minimal = MinimalBox3 (point_list.size(), pnt_arr);
02315
02316    //PRINT_INFO( "MinBox center = %lf, %lf, %lf\n", minimal.center.x, minimal.center.y, minimal.center.z );
02317    //PRINT_INFO( "MinBox axis 1 = %lf, %lf, %lf\n", minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z );
02318    //PRINT_INFO( "MinBox axis 2 = %lf, %lf, %lf\n", minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z );
02319    //PRINT_INFO( "MinBox axis 3 = %lf, %lf, %lf\n", minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z );
02320    //PRINT_INFO( "MinBox extent = %lf, %lf, %lf\n", minimal.extent[0], minimal.extent[1], minimal.extent[2] );
02321
02322    CubitVector center(minimal.center.x, minimal.center.y, minimal.center.z);
02323    CubitVector x(minimal.axis[0].x, minimal.axis[0].y, minimal.axis[0].z);
02324    CubitVector y(minimal.axis[1].x, minimal.axis[1].y, minimal.axis[1].z);
02325    CubitVector z(minimal.axis[2].x, minimal.axis[2].y, minimal.axis[2].z);
02326    CubitVector extent(minimal.extent[0], minimal.extent[1], minimal.extent[2]);
02327
02328    center.next_point( -x, extent.x(), p1 ); p1.next_point( -y, extent.y(), p1 );
02329    p1.next_point( z, extent.z(), p1 );
02330
02331    center.next_point( -x, extent.x(), p2 ); p2.next_point( y, extent.y(), p2 );
02332    p2.next_point( z, extent.z(), p2 );
02333
02334    center.next_point( x, extent.x(), p3 ); p3.next_point( y, extent.y(), p3 );
02335    p3.next_point( z, extent.z(), p3 );
02336
02337    center.next_point( x, extent.x(), p4 ); p4.next_point( -y, extent.y(), p4 );
02338    p4.next_point( z, extent.z(), p4 );
02339
02340    center.next_point( -x, extent.x(), p5 ); p5.next_point( -y, extent.y(), p5 );
02341    p5.next_point( -z, extent.z(), p5 );
02342
02343    center.next_point( -x, extent.x(), p6 ); p6.next_point( y, extent.y(), p6 );
02344    p6.next_point( -z, extent.z(), p6 );
02345
02346    center.next_point( x, extent.x(), p7 ); p7.next_point( y, extent.y(), p7 );
02347    p7.next_point( -z, extent.z(), p7 );
02348
02349    center.next_point( x, extent.x(), p8 ); p8.next_point( -y, extent.y(), p8 );
02350    p8.next_point( -z, extent.z(), p8 );
02351
02352    delete [] pnt_arr;
02353
02354    return CUBIT_SUCCESS;
02355 }
02356
02357 CubitStatus
02359                                        CubitVector& axis,
02360                                        CubitVector& center,
02361                                        CubitBox& box,
02362                                        int extension_type,
02363                                        double extension,
02364                                        CubitVector &start,
02365                                        CubitVector &end,
02366                                        int num_tess_pnts )
02367
02368 {
02369    CubitVector box_min = box.minimum();
02370    CubitVector box_max = box.maximum();
02371
02372    double box_min_pnt[3], box_max_pnt[3], axis_vec[3], center_pnt[3];
02373    box_min.get_xyz( box_min_pnt ); box_max.get_xyz( box_max_pnt );
02374    axis.get_xyz( axis_vec ); center.get_xyz( center_pnt );
02375
02376    double start_pnt[3], end_pnt[3];
02377
02378    if( min_cyl_box_int( radius, axis_vec, center_pnt,
02379                         box_min_pnt, box_max_pnt,
02380                         extension_type, extension,
02381                         start_pnt, end_pnt, num_tess_pnts )
02382                         == CUBIT_FAILURE )
02383      return CUBIT_FAILURE;
02384
02385    start.set( start_pnt ); end.set( end_pnt );
02386
02387    return CUBIT_SUCCESS;
02388 }
02389
02390 CubitStatus
02391 AnalyticGeometryTool::min_cyl_box_int( double radius, double axis[3],
02392                                       double center[3],
02393                                       double box_min[3], double box_max[3],
02394                                       int extension_type, double extension,
02395                                       double start[3], double end[3],
02396                                       int num_tess_pnts )
02397 {
02398   double cyl_z[3];
02399   unit_vec( axis, cyl_z );
02400
02401   //PRINT_INFO( "Axis = %f, %f, %f\n", cyl_z[0], cyl_z[1], cyl_z[2] );
02402   //PRINT_INFO( "Center = %f, %f, %f\n", center[0], center[1], center[2] );
02403
02404   // Find transformation matrix to take a point into cylinder's
02405   // coordinate system
02406   double cubit2cyl_mtx[4][4], cyl2cubit_mtx[4][4];
02407   double cyl_x[3], cyl_y[3];
02408   orth_vecs( cyl_z, cyl_x, cyl_y );
02409   vecs_to_mtx( cyl_x, cyl_y, cyl_z, center, cyl2cubit_mtx );
02410   inv_trans_mtx( cyl2cubit_mtx, cubit2cyl_mtx );
02411
02412   // Setup the circle
02413   double vec_1[3], vec_2[3];
02414   orth_vecs( cyl_z, vec_1, vec_2 );
02415   AGT_3D_Arc arc;
02416   setup_arc( vec_1, vec_2, center, 0.0, 2.0 * CUBIT_PI, radius, arc );
02417
02418   // Setup the planes of the box
02419   double pln_norm[6][3], pln_orig[6][3];
02420   // Front
02421   pln_orig[0][0] = 0.0; pln_orig[0][1] = 0.0; pln_orig[0][2] = box_max[2];
02422   pln_norm[0][0] = 0.0; pln_norm[0][1] = 0.0; pln_norm[0][2] = 1.0;
02423   // Left
02424   pln_orig[1][0] = box_min[0]; pln_orig[1][1] = 0.0; pln_orig[1][2] = 0.0;
02425   pln_norm[1][0] = -1.0; pln_norm[1][1] = 0.0; pln_norm[1][2] = 0.0;
02426   // Top
02427   pln_orig[2][0] = 0.0; pln_orig[2][1] = box_max[1]; pln_orig[2][2] = 0.0;
02428   pln_norm[2][0] = 0.0; pln_norm[2][1] = 1.0; pln_norm[2][2] = 0.0;
02429   // Right
02430   pln_orig[3][0] = box_max[0]; pln_orig[3][1] = 0.0; pln_orig[3][2] = 0.0;
02431   pln_norm[3][0] = 1.0; pln_norm[3][1] = 0.0; pln_norm[3][2] = 0.0;
02432   // Bottom
02433   pln_orig[4][0] = 0.0; pln_orig[4][1] = box_min[1]; pln_orig[4][2] = 0.0;
02434   pln_norm[4][0] = 0.0; pln_norm[4][1] = -1.0; pln_norm[4][2] = 0.0;
02435   // Back
02436   pln_orig[5][0] = 0.0; pln_orig[5][1] = 0.0; pln_orig[5][2] = box_min[2];
02437   pln_norm[5][0] = 0.0; pln_norm[5][1] = 0.0; pln_norm[5][2] = -1.0;
02438
02439   double z; // Intersection along cylinder's axis
02440   double min_z = CUBIT_DBL_MAX, max_z = -CUBIT_DBL_MAX;
02441
02442   double t = 0.0, dt;
02443   dt = 1.0/(double)num_tess_pnts;
02444   double pnt[3];
02445   double int_pnt[3];
02446   double box_tol = 1e-14;
02447   double box_min_0 = box_min[0]-box_tol;
02448   double box_min_1 = box_min[1]-box_tol;
02449   double box_min_2 = box_min[2]-box_tol;
02450   double box_max_0 = box_max[0]+box_tol;
02451   double box_max_1 = box_max[1]+box_tol;
02452   double box_max_2 = box_max[2]+box_tol;
02453
02454   int i,j;
02455   for( i=0; i<num_tess_pnts; i++ )
02456   {
02457     get_arc_xyz( arc, t, pnt );
02458
02459     for( j=0; j<6; j++ )
02460     {
02461       // Evaluate the intersection at this point
02462       if( int_ln_pln( pnt, cyl_z, pln_orig[j], pln_norm[j], int_pnt )
02463         == CUBIT_FAILURE )
02464         continue;
02465
02466       // Throw-out if intersection not on physical box
02467       if( int_pnt[0] < box_min_0 || int_pnt[1] < box_min_1 ||
02468           int_pnt[2] < box_min_2 || int_pnt[0] > box_max_0 ||
02469           int_pnt[1] > box_max_1 || int_pnt[2] > box_max_2 )
02470         continue;
02471
02472       // Find min/max cylinder z on box so far
02473       // z-distance (in cylinder coordinate system)
02474       z = cubit2cyl_mtx[0][2]*int_pnt[0] + cubit2cyl_mtx[1][2]*int_pnt[1] +
02475         cubit2cyl_mtx[2][2]*int_pnt[2] + cubit2cyl_mtx[3][2];
02476
02477       if( z < min_z ) min_z = z;
02478       if( z > max_z ) max_z = z;
02479
02480     }
02481
02482     t += dt;
02483
02484   }
02485
02486   // Check the 8 corners of the box - they are likely min/max's.
02487   double box_corners[8][3];
02488   get_box_corners( box_min, box_max, box_corners );
02489   for( i=0; i<8; i++ )
02490   {
02491     // Get the corner in the cylinder csys
02492     transform_pnt( cubit2cyl_mtx, box_corners[i], pnt );
02493     // If the pnt is within the circle's radius, check it's z-coord
02494     // (distance from center)
02495     if(  sqrt( pnt[0]*pnt[0] + pnt[1]*pnt[1] ) <= radius+box_tol )
02496     {
02497       if( pnt[2] < min_z ) min_z = pnt[2];
02498       if( pnt[2] > max_z ) max_z = pnt[2];
02499     }
02500   }
02501
02502   if( min_z == CUBIT_DBL_MAX || max_z == -CUBIT_DBL_MAX )
02503   {
02504     PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
02505     return CUBIT_FAILURE;
02506   }
02507
02508   if( min_z == max_z )
02509   {
02510     PRINT_ERROR( "Unable to find cylinder/box intersection\n" );
02511     return CUBIT_FAILURE;
02512   }
02513
02514   //PRINT_INFO( "Min dist = %f\n", min_z );
02515   //PRINT_INFO( "Max dist = %f\n", max_z );
02516
02517   // Find the start and end of the cylinder
02518   next_pnt( center, cyl_z, min_z, start );
02519   next_pnt( center, cyl_z, max_z, end );
02520
02521   //PRINT_INFO( "Start = %f, %f, %f\n", start[0], start[1], start[2] );
02522   //PRINT_INFO( "End = %f, %f, %f\n", end[0], end[1], end[2] );
02523
02524   // Extend start and end, if necessary
02525   if( extension_type > 0 )
02526   {
02527     double ext_distance = 0.0;
02528     if( extension_type == 1 ) // percentage
02529     {
02530       double cyl_length = dist_pnt_pnt( start, end );
02531       ext_distance = extension/100.0 * cyl_length;
02532     }
02533     else
02534       ext_distance = extension;
02535
02536     next_pnt( end, cyl_z, ext_distance, end );
02537     reverse_vec( cyl_z, cyl_z );
02538     next_pnt( start, cyl_z, ext_distance, start );
02539   }
02540
02541   return CUBIT_SUCCESS;
02542 }
02543
02544 // MAGIC SOFTWARE - see .hpp file
02545 // FILE: minbox2.cpp
02546 //---------------------------------------------------------------------------
02547 double AnalyticGeometryTool::Area (int N, Point2* pt, double angle)
02548 {
02549     double cs = cos(angle), sn = sin(angle);
02550
02551     double umin = +cs*pt[0].x+sn*pt[0].y, umax = umin;
02552     double vmin = -sn*pt[0].x+cs*pt[0].y, vmax = vmin;
02553     for (int i = 1; i < N; i++)
02554     {
02555         double u = +cs*pt[i].x+sn*pt[i].y;
02556         if ( u < umin )
02557             umin = u;
02558         else if ( u > umax )
02559             umax = u;
02560
02561         double v = -sn*pt[i].x+cs*pt[i].y;
02562         if ( v < vmin )
02563             vmin = v;
02564         else if ( v > vmax )
02565             vmax = v;
02566     }
02567
02568     double area = (umax-umin)*(vmax-vmin);
02569     return area;
02570 }
02571 //---------------------------------------------------------------------------
02572 void AnalyticGeometryTool::MinimalBoxForAngle (int N, Point2* pt, double angle,
02573                                                OBBox2& box)
02574 {
02575     double cs = cos(angle), sn = sin(angle);
02576
02577
02578     double umin = +cs*pt[0].x+sn*pt[0].y, umax = umin;
02579     double vmin = -sn*pt[0].x+cs*pt[0].y, vmax = vmin;
02580     for (int i = 1; i < N; i++)
02581     {
02582         double u = +cs*pt[i].x+sn*pt[i].y;
02583         if ( u < umin )
02584             umin = u;
02585         else if ( u > umax )
02586             umax = u;
02587
02588         double v = -sn*pt[i].x+cs*pt[i].y;
02589         if ( v < vmin )
02590             vmin = v;
02591         else if ( v > vmax )
02592             vmax = v;
02593     }
02594
02595     double umid = 0.5*(umax+umin);
02596     double vmid = 0.5*(vmax+vmin);
02597     box.center.x = umid*cs-vmid*sn;
02598     box.center.y = umid*sn+vmid*cs;
02599     box.axis[0].x = cs;
02600     box.axis[0].y = sn;
02601     box.axis[1].x = -sn;
02602     box.axis[1].y = cs;
02603     box.extent[0] = 0.5*(umax-umin);
02604     box.extent[1] = 0.5*(vmax-vmin);
02605 }
02606 //---------------------------------------------------------------------------
02607 OBBox2 AnalyticGeometryTool::MinimalBox2 (int N, Point2* pt)
02608 {
02609     OBBox2 box;
02610
02611     // bracket a minimum for angles in [-pi,pi]
02612     double angle, area;
02613     int imin = 0;
02614     double areaMin = Area(N,pt,angleMin);
02615
02616     int i;
02617     for (i = 1; i <= maxPartition; i++)
02618     {
02619         angle = angleMin+i*angleRange/maxPartition;
02620         area = Area(N,pt,angle);
02621         if ( area < areaMin )
02622         {
02623             imin = i;
02624             areaMin = area;
02625         }
02626     }
02627
02628     double angle0 = angleMin+(imin-1)*angleRange/maxPartition;
02629     double area0 = Area(N,pt,angle0);
02630     double angle1 = angleMin+(imin+1)*angleRange/maxPartition;
02631     double area1 = Area(N,pt,angle1);
02632
02633     // use inverse parabolic interpolation to find the minimum
02634     for (i = 0; i <= invInterp; i++)
02635     {
02636         double angleMid, areaMid;
02637
02638         // test for convergence (do not change these parameters)
02639         const double epsilon = 1e-08, tol = 1e-04;
02640 //        const double omtol = 1.0-tol;
02641         if ( fabs(angle1-angle0) <= 2*tol*fabs(angle)+epsilon )
02642             break;
02643
02644         // compute vertex of interpolating parabola
02645         double dangle0 = angle0-angle, dangle1 = angle1-angle;
02646         double darea0 = area0-area, darea1 = area1-area;
02647         double temp0 = dangle0*darea1, temp1 = dangle1*darea0;
02648         double delta = temp1-temp0;
02649         if ( fabs(delta) < epsilon )
02650            break;
02651
02652         angleMid = angle+0.5*(dangle1*temp1-dangle0*temp0)/(temp1-temp0);
02653
02654         // update bracket
02655         if ( angleMid < angle )
02656         {
02657             areaMid = Area(N,pt,angleMid);
02658             if ( areaMid <= area )
02659             {
02660                 angle1 = angle;
02661                 area1 = area;
02662                 angle = angleMid;
02663                 area = areaMid;
02664             }
02665             else
02666             {
02667                 angle0 = angleMid;
02668                 area0 = areaMid;
02669             }
02670         }
02671         else if ( angleMid > angle )
02672         {
02673             areaMid = Area(N,pt,angleMid);
02674             if ( areaMid <= area )
02675             {
02676                 angle0 = angle;
02677                 area0 = area;
02678                 angle = angleMid;
02679                 area = areaMid;
02680             }
02681             else
02682             {
02683                 angle1 = angleMid;
02684                 area1 = areaMid;
02685             }
02686         }
02687         else
02688         {
02689             // bracket middle already vertex of parabola
02690             break;
02691         }
02692     }
02693
02694     MinimalBoxForAngle(N,pt,angle,box);
02695     return box;
02696 }
02697 //---------------------------------------------------------------------------
02698
02699 #ifdef MINBOX2_TEST
02700
02701 #include <stdlib.h>
02702
02703 void main ()
02704 {
02705     const int N = 128;
02706     Point2 pt[N];
02707
02708     for (int i = 0; i < N; i++)
02709     {
02710         pt[i].x = rand()/double(RAND_MAX);
02711         pt[i].y = rand()/double(RAND_MAX);
02712     }
02713
02714     OBBox2 minimal = MinimalBox2(N,pt);
02715 }
02716
02717 #endif
02718
02719 #if 1
02720 // FILE: minbox3.cpp
02721 //---------------------------------------------------------------------------
02722 void AnalyticGeometryTool::MatrixToAngleAxis (double** R, double& angle, double axis[3])
02723 {
02724     // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
02725     // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
02726     // I is the identity and
02727     //
02728     //       +-        -+
02729     //   P = |  0 +z -y |
02730     //       | -z  0 +x |
02731     //       | +y -x  0 |
02732     //       +-        -+
02733     //
02734     // Some algebra will show that
02735     //
02736     //   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
02737     //
02738     // In the event that A = pi, R-R^t = 0 which prevents us from extracting
02739     // the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
02740     // P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and z^2-1.
02741     // We can solve these for axis (x,y,z).  Because the angle is pi, it does
02742     // not matter which sign you choose on the square roots.
02743
02744     double trace = R[0][0]+R[1][1]+R[2][2];
02745     double cs = 0.5*(trace-1.0);
02746     if ( -1 < cs )
02747     {
02748         if ( cs < 1 )
02749             angle = acos(cs);
02750         else
02751             angle = 0;
02752     }
02753     else
02754     {
02755         angle = AGT_PI;
02756     }
02757
02758     axis[0] = R[1][2]-R[2][1];
02759     axis[1] = R[2][0]-R[0][2];
02760     axis[2] = R[0][1]-R[1][0];
02761     double length = sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
02762     const double epsilon = 1e-06;
02763     if ( length > epsilon )
02764     {
02765         axis[0] /= length;
02766         axis[1] /= length;
02767         axis[2] /= length;
02768     }
02769     else  // angle is 0 or pi
02770     {
02771         if ( angle > 1.0 )  // any number strictly between 0 and pi works
02772         {
02773             // angle must be pi
02774             axis[0] = sqrt(0.5*(1.0+R[0][0]));
02775             axis[1] = sqrt(0.5*(1.0+R[1][1]));
02776             axis[2] = sqrt(0.5*(1.0+R[2][2]));
02777
02778             // determine signs of axis components
02779             double test[3];
02780             test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
02781             test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
02782             test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
02783             length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
02784             if ( length < epsilon )
02785                 return;
02786
02787             axis[1] = -axis[1];
02788             test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
02789             test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
02790             test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
02791             length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
02792             if ( length < epsilon )
02793                 return;
02794
02795             axis[2] = -axis[2];
02796             test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
02797             test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
02798             test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
02799             length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
02800             if ( length < epsilon )
02801                 return;
02802
02803             axis[1] = -axis[1];
02804             test[0] = R[0][0]*axis[0]+R[0][1]*axis[1]+R[0][2]*axis[2]-axis[0];
02805             test[1] = R[1][0]*axis[0]+R[1][1]*axis[1]+R[1][2]*axis[2]-axis[1];
02806             test[2] = R[2][0]*axis[0]+R[2][1]*axis[1]+R[2][2]*axis[2]-axis[2];
02807             length = test[0]*test[0]+test[1]*test[1]+test[2]*test[2];
02808             if ( length < epsilon )
02809                 return;
02810         }
02811         else
02812         {
02813             // Angle is zero, matrix is the identity, no unique axis, so
02814             // return (1,0,0) for as good a guess as any.
02815             axis[0] = 1.0;
02816             axis[1] = 0.0;
02817             axis[2] = 0.0;
02818         }
02819     }
02820 }
02821 //---------------------------------------------------------------------------
02822 void AnalyticGeometryTool::AngleAxisToMatrix (double angle, double axis[3], double R[3][3])
02823 {
02824     double cs = cos(angle), sn = sin(angle);
02825     double length = sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
02826     double x = axis[0]/length;
02827     double y = axis[1]/length;
02828     double z = axis[2]/length;
02829     double omc = 1.0-cs;
02830     double x2 = x*x, y2 = y*y, z2 = z*z;
02831     double xy = x*y, xz = x*z, yz = y*z;
02832     double snx = sn*x, sny = sn*y, snz = sn*z;
02833
02834     R[0][0] = 1.0-omc*(y2+z2);
02835     R[0][1] = +snz+omc*xy;
02836     R[0][2] = -sny+omc*xz;
02837     R[1][0] = -snz+omc*xy;
02838     R[1][1] = 1.0-omc*(x2+z2);
02839     R[1][2] = +snx+omc*yz;
02840     R[2][0] = +sny+omc*xz;
02841     R[2][1] = -snx+omc*yz;
02842     R[2][2] = 1.0-omc*(x2+y2);
02843 }
02844 //---------------------------------------------------------------------------
02845 double AnalyticGeometryTool::Volume (int N, Point3* pt, double angle[3])
02846 {
02847     double cs0 = cos(angle[0]), sn0 = sin(angle[0]);
02848     double cs1 = cos(angle[1]), sn1 = sin(angle[1]);
02849     double axis[3] = { cs0*sn1, sn0*sn1, cs1 };
02850     double rot[3][3];
02851     AngleAxisToMatrix(angle[2],axis,rot);
02852
02853     double min[3] =
02854     {
02855         rot[0][0]*pt[0].x+rot[1][0]*pt[0].y+rot[2][0]*pt[0].z,
02856         rot[0][1]*pt[0].x+rot[1][1]*pt[0].y+rot[2][1]*pt[0].z,
02857         rot[0][2]*pt[0].x+rot[1][2]*pt[0].y+rot[2][2]*pt[0].z
02858     };
02859
02860     double max[3] = { min[0], min[1], min[2] };
02861
02862     for (int i = 1; i < N; i++)
02863     {
02864         double test[3] =
02865         {
02866             rot[0][0]*pt[i].x+rot[1][0]*pt[i].y+rot[2][0]*pt[i].z,
02867             rot[0][1]*pt[i].x+rot[1][1]*pt[i].y+rot[2][1]*pt[i].z,
02868             rot[0][2]*pt[i].x+rot[1][2]*pt[i].y+rot[2][2]*pt[i].z
02869         };
02870
02871         if ( test[0] < min[0] )
02872             min[0] = test[0];
02873         else if ( test[0] > max[0] )
02874             max[0] = test[0];
02875
02876         if ( test[1] < min[1] )
02877             min[1] = test[1];
02878         else if ( test[1] > max[1] )
02879             max[1] = test[1];
02880
02881         if ( test[2] < min[2] )
02882             min[2] = test[2];
02883         else if ( test[2] > max[2] )
02884             max[2] = test[2];
02885     }
02886
02887     double volume = (max[0]-min[0])*(max[1]-min[1])*(max[2]-min[2]);
02888     return volume;
02889 }
02890 //---------------------------------------------------------------------------
02891 void AnalyticGeometryTool::MinimalBoxForAngles (int N, Point3* pt, double angle[3],
02892                                  OBBox3& box)
02893 {
02894     double cs0 = cos(angle[0]), sn0 = sin(angle[0]);
02895     double cs1 = cos(angle[1]), sn1 = sin(angle[1]);
02896     double axis[3] = { cs0*sn1, sn0*sn1, cs1 };
02897     double rot[3][3];
02898     AngleAxisToMatrix(angle[2],axis,rot);
02899
02900     double min[3] =
02901     {
02902         rot[0][0]*pt[0].x+rot[1][0]*pt[0].y+rot[2][0]*pt[0].z,
02903         rot[0][1]*pt[0].x+rot[1][1]*pt[0].y+rot[2][1]*pt[0].z,
02904         rot[0][2]*pt[0].x+rot[1][2]*pt[0].y+rot[2][2]*pt[0].z
02905     };
02906
02907     double max[3] = { min[0], min[1], min[2] };
02908
02909     for (int i = 1; i < N; i++)
02910     {
02911         double test[3] =
02912         {
02913             rot[0][0]*pt[i].x+rot[1][0]*pt[i].y+rot[2][0]*pt[i].z,
02914             rot[0][1]*pt[i].x+rot[1][1]*pt[i].y+rot[2][1]*pt[i].z,
02915             rot[0][2]*pt[i].x+rot[1][2]*pt[i].y+rot[2][2]*pt[i].z
02916         };
02917
02918         if ( test[0] < min[0] )
02919             min[0] = test[0];
02920         else if ( test[0] > max[0] )
02921             max[0] = test[0];
02922
02923         if ( test[1] < min[1] )
02924             min[1] = test[1];
02925         else if ( test[1] > max[1] )
02926             max[1] = test[1];
02927
02928         if ( test[2] < min[2] )
02929             min[2] = test[2];
02930         else if ( test[2] > max[2] )
02931             max[2] = test[2];
02932     }
02933
02934     double mid[3] =
02935     {
02936         0.5*(max[0]+min[0]), 0.5*(max[1]+min[1]), 0.5*(max[2]+min[2])
02937     };
02938
02939     box.center.x = mid[0]*rot[0][0]+mid[1]*rot[0][1]+mid[2]*rot[0][2];
02940     box.center.y = mid[0]*rot[1][0]+mid[1]*rot[1][1]+mid[2]*rot[1][2];
02941     box.center.z = mid[0]*rot[2][0]+mid[1]*rot[2][1]+mid[2]*rot[2][2];
02942     box.axis[0].x = rot[0][0];
02943     box.axis[0].y = rot[1][0];
02944     box.axis[0].z = rot[2][0];
02945     box.axis[1].x = rot[0][1];
02946     box.axis[1].y = rot[1][1];
02947     box.axis[1].z = rot[2][1];
02948     box.axis[2].x = rot[0][2];
02949     box.axis[2].y = rot[1][2];
02950     box.axis[2].z = rot[2][2];
02951     box.extent[0] = 0.5*(max[0]-min[0]);
02952     box.extent[1] = 0.5*(max[1]-min[1]);
02953     box.extent[2] = 0.5*(max[2]-min[2]);
02954 }
02955 //---------------------------------------------------------------------------
02956 void AnalyticGeometryTool::GetInterval (double A[3], double D[3], double& tmin, double& tmax)
02957 {
02958     //static const double angle_min[3] = { -AGT_PI, 0.0, 0.0 };
02959     //static const double angle_max[3] = {  AGT_PI,  AGT_PI,  AGT_PI };
02960     //The pgCC compiler running on solars and cross-compiling for
02961     //janus has a bug such that it dies if the initialization
02962     //of angle_min is mixed symbolic and literal constants, so
02963     //define a symbolid constant for 0.0.
02964     // -- J.Kraftcheck, 06/05/2001
02965     static const double ZERO = 0.0;
02966     static const double angle_min[3] = { -AGT_PI, ZERO, ZERO };
02967     static const double angle_max[3] = {  AGT_PI, AGT_PI, AGT_PI };
02968
02969     tmin = -DBL_MAX;
02970     tmax = +DBL_MAX;
02971
02972     for (int i = 0; i < 3; i++)
02973     {
02974         const double epsilon = 1e-08;
02975         double b0 = angle_min[i]-A[i];
02976         double b1 = angle_max[i]-A[i];
02977
02978         double inv, tmp;
02979         if ( D[i] > epsilon )
02980         {
02981             inv = 1.0/D[i];
02982             tmp = inv*b0;
02983             if ( tmp > tmin )
02984                 tmin = tmp;
02985             tmp = inv*b1;
02986             if ( tmp < tmax )
02987                 tmax = tmp;
02988         }
02989         else if ( D[i] < -epsilon )
02990         {
02991             inv = 1.0/D[i];
02992             tmp = inv*b0;
02993             if ( tmp < tmax )
02994                 tmax = tmp;
02995             tmp = inv*b1;
02996             if ( tmp > tmin )
02997                 tmin = tmp;
02998         }
02999     }
03000
03001     if( tmin == -DBL_MAX || tmax == DBL_MAX ) // Added by SRS 10-6-2000
03002     {
03003        //PRINT_WARNING( "tmin/tmax not set\n" );
03004        tmin = -1.0;
03005        tmax = 1.0;
03006     }
03007 }
03008 //---------------------------------------------------------------------------
03009 void AnalyticGeometryTool::Combine (double result[3], double A[3], double t, double D[3])
03010 {
03011     for (int i = 0; i < 3; i++)
03012         result[i] = A[i]+t*D[i];
03013 }
03014 //---------------------------------------------------------------------------
03015 double AnalyticGeometryTool::MinimizeOnInterval (int N, Point3* pt, double A[3], double D[3])
03016 {
03017     // compute intersection of line A+t*D with domain of function
03018     double tmin, tmax;
03019     GetInterval(A,D,tmin,tmax);
03020     double tran = tmax-tmin;
03021     double angle[3];
03022
03023     if( tran == 0.0 ) // Added by SRS 10-6-2000
03024        tran = 1.0;
03025
03026     // bracket a minimum for angles in [A+tmin*D,A+tmax*D]
03027     double t = 0.0;
03028     double volumeMin = Volume(N,pt,A);
03029     double volume;
03030
03031     const int max_partition = 64;
03032     int i, imin;
03033     for (i = 0, imin = -1; i <= max_partition; i++)
03034     {
03035         t = tmin+i*tran/max_partition;
03036         Combine(angle,A,t,D);
03037
03038         volume = Volume(N,pt,angle);
03039         if ( volume < volumeMin )
03040         {
03041             imin = i;
03042             volumeMin = volume;
03043         }
03044     }
03045
03046     if ( imin != -1 )
03047     {
03048         t = tmin+imin*tran/max_partition;
03049     }
03050     else
03051     {
03052         t = 0.0;
03053
03054         // interval in which t=0 lies
03055         imin = int(-tmin*max_partition/tran+0.5);
03056     }
03057     volume = volumeMin;
03058
03059     double t0 = tmin+(imin-1)*tran/max_partition;
03060     Combine(angle,A,t0,D);
03061     double volume0 = Volume(N,pt,angle);
03062
03063     double t1 = tmin+(imin+1)*tran/max_partition;
03064     Combine(angle,A,t1,D);
03065     double volume1 = Volume(N,pt,angle);
03066
03067     // use inverse parabolic interpolation to find the minimum
03068     const int inv_interp = 64;
03069     for (i = 0; i <= inv_interp; i++)
03070     {
03071         double tMid, volumeMid;
03072
03073         // test for convergence (do not change these parameters)
03074         const double epsilon = 1e-08, tol = 1e-04;
03075 //        const double omtol = 1.0-tol;
03076         if ( fabs(t1-t0) <= 2*tol*fabs(t)+epsilon )
03077             break;
03078
03079         // compute vertex of interpolating parabola
03080         double dt0 = t0-t, dt1 = t1-t;
03081         double dvolume0 = volume0-volume, dvolume1 = volume1-volume;
03082         double temp0 = dt0*dvolume1, temp1 = dt1*dvolume0;
03083         double delta = temp1-temp0;
03084         if ( fabs(delta) < epsilon )
03085            break;
03086
03087         tMid = t+0.5*(dt1*temp1-dt0*temp0)/(temp1-temp0);
03088
03089         // update bracket
03090         if ( tMid < t )
03091         {
03092             Combine(angle,A,tMid,D);
03093             volumeMid = Volume(N,pt,angle);
03094             if ( volumeMid <= volume )
03095             {
03096                 t1 = t;
03097                 volume1 = volume;
03098                 t = tMid;
03099                 volume = volumeMid;
03100             }
03101             else
03102             {
03103                 t0 = tMid;
03104                 volume0 = volumeMid;
03105             }
03106         }
03107         else if ( tMid > t )
03108         {
03109             Combine(angle,A,tMid,D);
03110             volumeMid = Volume(N,pt,angle);
03111             if ( volumeMid <= volume )
03112             {
03113                 t0 = t;
03114                 volume0 = volume;
03115                 t = tMid;
03116                 volume = volumeMid;
03117             }
03118             else
03119             {
03120                 t1 = tMid;
03121                 volume1 = volumeMid;
03122             }
03123         }
03124         else
03125         {
03126             // bracket middle already vertex of parabola
03127             break;
03128         }
03129     }
03130
03131     Combine(A,A,t,D);
03132     return volume;
03133 }
03134 //---------------------------------------------------------------------------
03135 double AnalyticGeometryTool::MinimizeOnLattice (int N, Point3* pt, double A[3], int layers,
03136                                  double thickness)
03137 {
03138     int xmin = 0, ymin = 0, zmin = 0;
03139     double volume = Volume(N,pt,A);
03140
03141     double angle[3];
03142     for (int z = -layers; z <= layers; z++)
03143     {
03144         angle[2] = A[2]+thickness*z/layers;
03145         for (int y = -layers; y <= layers; y++)
03146         {
03147             angle[1] = A[1]+thickness*y/layers;
03148             for (int x = -layers; x <= layers; x++)
03149             {
03150                 angle[0] = A[0]+thickness*x/layers;
03151
03152                 double v = Volume(N,pt,angle);
03153                 if ( v < volume )
03154                 {
03155                     xmin = x;
03156                     ymin = y;
03157                     zmin = z;
03158                     volume = v;
03159                 }
03160             }
03161         }
03162     }
03163
03164     A[0] += thickness*xmin/layers;
03165     A[1] += thickness*ymin/layers;
03166     A[2] += thickness*zmin/layers;
03167
03168     return volume;
03169 }
03170 //---------------------------------------------------------------------------
03171 void AnalyticGeometryTool::InitialGuess (int N, Point3* pt, double angle[3])
03172 {
03173     int i;
03174
03175     // compute mean of points
03176     double xsum = 0.0f, ysum = 0.0f, zsum = 0.0f;;
03177     for (i = 0; i < N; i++)
03178     {
03179         xsum += pt[i].x;
03180         ysum += pt[i].y;
03181         zsum += pt[i].z;
03182     }
03183     double xmean = xsum/N;
03184     double ymean = ysum/N;
03185     double zmean = zsum/N;
03186
03187     // compute covariances of points
03188     double xxsum = 0.0f, xysum = 0.0f, xzsum = 0.0f;
03189     double yysum = 0.0f, yzsum = 0.0f, zzsum = 0.0f;
03190     for (i = 0; i < N; i++)
03191     {
03192         double dx = pt[i].x - xmean;
03193         double dy = pt[i].y - ymean;
03194         double dz = pt[i].z - zmean;
03195         xxsum += dx*dx;
03196         xysum += dx*dy;
03197         xzsum += dx*dz;
03198         yysum += dy*dy;
03199         yzsum += dy*dz;
03200         zzsum += dz*dz;
03201     }
03202     double xxcov = xxsum/N;
03203     double xycov = xysum/N;
03204     double xzcov = xzsum/N;
03205     double yycov = yysum/N;
03206     double yzcov = yzsum/N;
03207     double zzcov = zzsum/N;
03208
03209     // compute eigenvectors for covariance matrix
03210     mgcEigenD eig(3);
03211     eig.Matrix(0,0) = xxcov;
03212     eig.Matrix(0,1) = xycov;
03213     eig.Matrix(0,2) = xzcov;
03214     eig.Matrix(1,0) = xycov;
03215     eig.Matrix(1,1) = yycov;
03216     eig.Matrix(1,2) = yzcov;
03217     eig.Matrix(2,0) = xzcov;
03218     eig.Matrix(2,1) = yzcov;
03219     eig.Matrix(2,2) = zzcov;
03220     eig.EigenStuff3();
03221
03222     // Use eigenvectors as the box axes.  Eigenmatrix must not have a
03223     // reflection component, thus the check for negative determinant.
03224     const double epsilon = 1e-06;
03225     double** R = (double**)eig.Eigenvector();
03226     double det =
03227         +R[0][0]*R[1][1]*R[2][2]
03228         +R[0][1]*R[1][2]*R[2][0]
03229         +R[0][2]*R[1][0]*R[2][1]
03230         -R[0][2]*R[1][1]*R[2][0]
03231         -R[0][1]*R[1][0]*R[2][2]
03232         -R[0][0]*R[1][2]*R[2][1];
03233     if ( det < 0.0 )
03234     {
03235         R[0][0] = -R[0][0];
03236         R[1][0] = -R[1][0];
03237         R[2][0] = -R[2][0];
03238     }
03239
03240     // extract angles from rotation axis = (cos(u)sin(v),sin(u)sin(v),cos(v))
03241     double axis[3];
03242     MatrixToAngleAxis(R,angle[2],axis);
03243     if ( -1+epsilon < axis[2] )
03244     {
03245         if ( axis[2] < 1-epsilon )
03246         {
03247             angle[0] = atan2(axis[1],axis[0]);
03248             angle[1] = acos(axis[2]);
03249         }
03250         else
03251         {
03252             angle[0] = 0;
03253             angle[1] = 0;
03254         }
03255     }
03256     else
03257     {
03258         angle[0] = 0;
03259         angle[1] = AGT_PI;
03260     }
03261 }
03262 //---------------------------------------------------------------------------
03263 OBBox3 AnalyticGeometryTool::MinimalBox3 (int N, Point3* pt)
03264 {
03265     // compute a good initial guess for an oriented bounding box
03266     double angle[3];
03267     InitialGuess(N,pt,angle);
03268     double oldVolume = Volume(N,pt,angle);
03269
03270   //if initial guess gives angles that are zero, the rest of this code
03271   //can give bounding box that isn't tight.  This probably
03272   //isn't the best fix but it works.  CDE  4-20-2009
03273   if( angle[0] == 0 )
03274     angle[0] = 0.5;
03275   if( angle[1] == 0 )
03276     angle[1] = 0.5;
03277   if( angle[2] == 0 )
03278     angle[2] = 0.5;
03279
03280     double saveAngle[3] = { angle[0], angle[1], angle[2] };
03281
03282     // Powell's direction set method
03283     double U[3][3], volume;
03284     const int maxiters = 3*32;
03285     for (int iter = 0; iter < maxiters; iter++)
03286     {
03287         // reset directions to avoid linear dependence degeneration
03288         if ( iter % 3 == 0 )
03289         {
03290             U[0][0] = 1.0;  U[0][1] = 0.0;  U[0][2] = 0.0;
03291             U[1][0] = 0.0;  U[1][1] = 1.0;  U[1][2] = 0.0;
03292             U[2][0] = 0.0;  U[2][1] = 0.0;  U[2][2] = 1.0;
03293         }
03294
03295         // find minima in specified directions
03296         for (int d = 0; d < 3; d++)
03297             volume = MinimizeOnInterval(N,pt,angle,U[d]);
03298
03299         // estimate a conjugate direction
03300         double conj[3] =
03301         {
03302             angle[0]-saveAngle[0],
03303             angle[1]-saveAngle[1],
03304             angle[2]-saveAngle[2]
03305         };
03306         double length = sqrt(conj[0]*conj[0]+conj[1]*conj[1]+conj[2]*conj[2]);
03307         if ( length >= 1e-06 )
03308         {
03309             double invLen = 1.0/length;
03310             conj[0] *= invLen;
03311             conj[1] *= invLen;
03312             conj[2] *= invLen;
03313
03314             // minimize in conjugate direction
03315             volume = MinimizeOnInterval(N,pt,angle,conj);
03316         }
03317         else
03318         {
03319             // Possible local, but not global, minimum.  Search nearby for
03320             // a smaller volume.
03321             volume = MinimizeOnLattice(N,pt,angle,2,0.0001);
03322             volume = MinimizeOnLattice(N,pt,angle,2,0.0010);
03323             volume = MinimizeOnLattice(N,pt,angle,2,0.0100);
03324             volume = MinimizeOnLattice(N,pt,angle,2,0.1000);
03325         }
03326
03327         // test for convergence
03328         const double epsilon = 1e-04;
03329         double diff = fabs(volume-oldVolume);
03330         if ( diff <= epsilon )
03331         {
03332             // Possible local, but not global, minimum.  Search nearby for
03333             // a smaller volume.
03334             volume = MinimizeOnLattice(N,pt,angle,2,0.0001);
03335             volume = MinimizeOnLattice(N,pt,angle,2,0.0010);
03336             volume = MinimizeOnLattice(N,pt,angle,2,0.0100);
03337             volume = MinimizeOnLattice(N,pt,angle,2,0.1000);
03338             diff = fabs(volume-oldVolume);
03339             if ( diff <= epsilon )
03340                 break;
03341         }
03342
03343         // cycle the directions and add conjugate direction to list
03344         U[0][0] = U[1][0];  U[0][1] = U[1][1];  U[0][2] = U[1][2];
03345         U[1][0] = U[2][0];  U[1][1] = U[2][1];  U[1][2] = U[2][2];
03346         U[2][0] = conj[0];  U[2][1] = conj[1];  U[2][2] = conj[2];
03347
03348         // set parameters for next pass
03349         oldVolume = volume;
03350         saveAngle[0] = angle[0];
03351         saveAngle[1] = angle[1];
03352         saveAngle[2] = angle[2];
03353     }
03354
03355     OBBox3 box;
03356     MinimalBoxForAngles(N,pt,angle,box);
03357     return box;
03358 }
03359 //---------------------------------------------------------------------------
03360
03361 #ifdef MINBOX3_TEST
03362
03363 #define RAND (rand()/double(RAND_MAX))
03364
03365 void main ()
03366 {
03367     // build box with axes parallel to coordinate axes
03368     const int N = 16;
03369     const double ex = 1.0;
03370     const double ey = 2.0;
03371     const double ez = 3.0;
03372     Point3 pt[N];
03373     pt[0].x = -ex;  pt[0].y = -ey;  pt[0].z = -ez;
03374     pt[1].x = -ex;  pt[1].y = +ey;  pt[1].z = -ez;
03375     pt[2].x = +ex;  pt[2].y = +ey;  pt[2].z = -ez;
03376     pt[3].x = +ex;  pt[3].y = -ey;  pt[3].z = -ez;
03377     pt[4].x = -ex;  pt[4].y = -ey;  pt[4].z = +ez;
03378     pt[5].x = -ex;  pt[5].y = +ey;  pt[5].z = +ez;
03379     pt[6].x = +ex;  pt[6].y = +ey;  pt[6].z = +ez;
03380     pt[7].x = +ex;  pt[7].y = -ey;  pt[7].z = +ez;
03381
03382     for (int k = 8; k < N; k++)
03383     {
03384         // generate random points inside box to confound initial Gaussian fit
03385         pt[k].x = -ex+2.0*ex*RAND;
03386         pt[k].y = -ey+2.0*ey*RAND;
03387         pt[k].z = -ez+2.0*ez*RAND;
03388     }
03389
03390     double maxNorm = 0.0;
03391     int iMaxNorm = -1;
03392     for (int iter = 0; iter < 1024; iter++)
03393     {
03394         // build arbitrary rotation matrix
03395         double angle = RAND;
03396         double line[3] = { RAND, RAND, RAND };
03397         double rot[3][3];
03398         double length = sqrt(line[0]*line[0]+line[1]*line[1]+line[2]*line[2]);
03399         line[0] /= length;
03400         line[1] /= length;
03401         line[2] /= length;
03402         AngleAxisToMatrix(angle,line,rot);
03403
03404         // rotate box
03405         Point3 rpt[N];
03406         for (int i = 0; i < N; i++)
03407         {
03408             rpt[i].x = rot[0][0]*pt[i].x+rot[0][1]*pt[i].y+rot[0][2]*pt[i].z;
03409             rpt[i].y = rot[1][0]*pt[i].x+rot[1][1]*pt[i].y+rot[1][2]*pt[i].z;
03410             rpt[i].z = rot[2][0]*pt[i].x+rot[2][1]*pt[i].y+rot[2][2]*pt[i].z;
03411         }
03412
03413         OBBox3 minimal = MinimalBox3(N,rpt);
03414
03415         int index[3];
03416         if ( minimal.extent[0] <= minimal.extent[1] )
03417         {
03418             if ( minimal.extent[1] <= minimal.extent[2] )
03419             {
03420                 index[0] = 0;
03421                 index[1] = 1;
03422                 index[2] = 2;
03423             }
03424             else
03425             {
03426                 if ( minimal.extent[0] <= minimal.extent[2] )
03427                 {
03428                     index[0] = 0;
03429                     index[1] = 2;
03430                     index[2] = 1;
03431                 }
03432                 else
03433                 {
03434                     index[0] = 2;
03435                     index[1] = 0;
03436                     index[2] = 1;
03437                 }
03438             }
03439         }
03440         else
03441         {
03442             if ( minimal.extent[0] <= minimal.extent[2] )
03443             {
03444                 index[0] = 1;
03445                 index[1] = 0;
03446                 index[2] = 2;
03447             }
03448             else
03449             {
03450                 if ( minimal.extent[1] <= minimal.extent[2] )
03451                 {
03452                     index[0] = 1;
03453                     index[1] = 2;
03454                     index[2] = 0;
03455                 }
03456                 else
03457                 {
03458                     index[0] = 2;
03459                     index[1] = 1;
03460                     index[2] = 0;
03461                 }
03462             }
03463         }
03464
03465         double dx = ex-minimal.extent[index[0]];
03466         double dy = ey-minimal.extent[index[1]];
03467         double dz = ez-minimal.extent[index[2]];
03468         double norm = sqrt(dx*dx+dy*dy+dz*dz);
03469         if ( norm > maxNorm )
03470         {
03471             maxNorm = norm;
03472             iMaxNorm = iter;
03473         }
03474     }
03475 }
03476
03477 #endif // MINBOX3_TEST
03478
03479 // FILE: eigen.cpp
03480 //===========================================================================
03481 // error handling
03482 int mgcEigen::verbose1 = 0;
03483 unsigned mgcEigen::error = 0;
03484 const unsigned mgcEigen::invalid_size      = 0x00000001;
03485 const unsigned mgcEigen::allocation_failed = 0x00000002;
03486 const unsigned mgcEigen::ql_exceeded       = 0x00000004;
03487 const char* mgcEigen::message[3] = {
03488         "invalid matrix size",
03489         "allocation failed",
03490         "QL algorithm - exceeded maximum iterations"
03491 };
03492 //---------------------------------------------------------------------------
03493 void mgcEigen::
03494 Tridiagonal2 (float** pmat, float* pdiag, float* psubd)
03495 {
03496         // matrix is already tridiagonal
03497
03498         pdiag[0] = pmat[0][0];
03499         pdiag[1] = pmat[1][1];
03500         psubd[0] = pmat[0][1];
03501         psubd[1] = 0;
03502         pmat[0][0] = 1;  pmat[0][1] = 0;
03503         pmat[1][0] = 0;  pmat[1][1] = 1;
03504 }
03505 //---------------------------------------------------------------------------
03506 void mgcEigen::
03507 Tridiagonal3 (float** pmat, float* pdiag, float* psubd)
03508 {
03509         float a = pmat[0][0], b = pmat[0][1], c = pmat[0][2],
03510                                                  d = pmat[1][1], e = pmat[1][2],
03511                                                                                 f = pmat[2][2];
03512
03513         pdiag[0] = a;
03514         psubd[2] = 0;
03515         if ( c != 0 ) {
03516                 float ell = float(sqrt(b*b+c*c));
03517                 b /= ell;
03518                 c /= ell;
03519                 float q = 2*b*e+c*(f-d);
03520                 pdiag[1] = d+c*q;
03521                 pdiag[2] = f-c*q;
03522                 psubd[0] = ell;
03523                 psubd[1] = e-b*q;
03524                 pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
03525                 pmat[1][0] = 0; pmat[1][1] = b; pmat[1][2] = c;
03526                 pmat[2][0] = 0; pmat[2][1] = c; pmat[2][2] = -b;
03527         }
03528         else {
03529                 pdiag[1] = d;
03530                 pdiag[2] = f;
03531                 psubd[0] = b;
03532                 psubd[1] = e;
03533                 pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
03534                 pmat[1][0] = 0; pmat[1][1] = 1; pmat[1][2] = 0;
03535                 pmat[2][0] = 0; pmat[2][1] = 0; pmat[2][2] = 1;
03536         }
03537 }
03538 //---------------------------------------------------------------------------
03539 void mgcEigen::
03540 Tridiagonal4 (float** pmat, float* pdiag, float* psubd)
03541 {
03542         // save pmatrix M
03543         float
03544         a = pmat[0][0], b = pmat[0][1], c = pmat[0][2], d = pmat[0][3],
03545                                    e = pmat[1][1], f = pmat[1][2], g = pmat[1][3],
03546                                                                   h = pmat[2][2], i = pmat[2][3],
03547                                                                                                  j = pmat[3][3];
03548
03549         pdiag[0] = a;
03550         psubd[3] = 0;
03551
03552         pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0; pmat[0][3] = 0;
03553         pmat[1][0] = 0;
03554         pmat[2][0] = 0;
03555         pmat[3][0] = 0;
03556
03557         if ( c != 0 || d != 0 ) {
03558                 float q11, q12, q13;
03559                 float q21, q22, q23;
03560                 float q31, q32, q33;
03561
03562                 // build column Q1
03563                 float len = float(sqrt(b*b+c*c+d*d));
03564                 q11 = b/len;
03565                 q21 = c/len;
03566                 q31 = d/len;
03567
03568                 psubd[0] = len;
03569
03570                 // compute S*Q1
03571                 float v0 = e*q11+f*q21+g*q31;
03572                 float v1 = f*q11+h*q21+i*q31;
03573                 float v2 = g*q11+i*q21+j*q31;
03574
03575                 pdiag[1] = q11*v0+q21*v1+q31*v2;
03576
03577         // build column Q3 = Q1x(S*Q1)
03578                 q13 = q21*v2-q31*v1;
03579                 q23 = q31*v0-q11*v2;
03580                 q33 = q11*v1-q21*v0;
03581                 len = float(sqrt(q13*q13+q23*q23+q33*q33));
03582                 if ( len > 0 ) {
03583                         q13 /= len;
03584                         q23 /= len;
03585                         q33 /= len;
03586
03587                         // build column Q2 = Q3xQ1
03588                         q12 = q23*q31-q33*q21;
03589                         q22 = q33*q11-q13*q31;
03590                         q32 = q13*q21-q23*q11;
03591
03592                         v0 = q12*e+q22*f+q32*g;
03593                         v1 = q12*f+q22*h+q32*i;
03594                         v2 = q12*g+q22*i+q32*j;
03595                         psubd[1] = q11*v0+q21*v1+q31*v2;
03596                         pdiag[2] = q12*v0+q22*v1+q32*v2;
03597                         psubd[2] = q13*v0+q23*v1+q33*v2;
03598
03599                         v0 = q13*e+q23*f+q33*g;
03600                         v1 = q13*f+q23*h+q33*i;
03601                         v2 = q13*g+q23*i+q33*j;
03602                         pdiag[3] = q13*v0+q23*v1+q33*v2;
03603                 }
03604                 else {  // S*Q1 parallel to Q1, choose any valid Q2 and Q3
03605                         psubd[1] = 0;
03606
03607                         len = q21*q21+q31*q31;
03608                         if ( len > 0 ) {
03609                                 float tmp = q11-1;
03610                                 q12 = -q21;
03611                                 q22 = 1+tmp*q21*q21/len;
03612                 q32 = tmp*q21*q31/len;
03613
03614                                 q13 = -q31;
03615                                 q23 = q32;
03616                                 q33 = 1+tmp*q31*q31/len;
03617
03618                                 v0 = q12*e+q22*f+q32*g;
03619                                 v1 = q12*f+q22*h+q32*i;
03620                                 v2 = q12*g+q22*i+q32*j;
03621                                 pdiag[2] = q12*v0+q22*v1+q32*v2;
03622                                 psubd[2] = q13*v0+q23*v1+q33*v2;
03623
03624                                 v0 = q13*e+q23*f+q33*g;
03625                                 v1 = q13*f+q23*h+q33*i;
03626                                 v2 = q13*g+q23*i+q33*j;
03627                                 pdiag[3] = q13*v0+q23*v1+q33*v2;
03628                         }
03629                         else {  // Q1 = (+-1,0,0)
03630                                 q12 = 0; q22 = 1; q32 = 0;
03631                                 q13 = 0; q23 = 0; q33 = 1;
03632
03633                                 pdiag[2] = h;
03634                                 pdiag[3] = j;
03635                                 psubd[2] = i;
03636                         }
03637                 }
03638
03639                 pmat[1][1] = q11; pmat[1][2] = q12; pmat[1][3] = q13;
03640                 pmat[2][1] = q21; pmat[2][2] = q22; pmat[2][3] = q23;
03641                 pmat[3][1] = q31; pmat[3][2] = q32; pmat[3][3] = q33;
03642         }
03643         else {
03644                 pdiag[1] = e;
03645                 psubd[0] = b;
03646                 pmat[1][1] = 1;
03647                 pmat[2][1] = 0;
03648                 pmat[3][1] = 0;
03649
03650                 if ( g != 0 ) {
03651                         float ell = float(sqrt(f*f+g*g));
03652                         f /= ell;
03653                         g /= ell;
03654                         float Q = 2*f*i+g*(j-h);
03655
03656                         pdiag[2] = h+g*Q;
03657                         pdiag[3] = j-g*Q;
03658                         psubd[1] = ell;
03659                         psubd[2] = i-f*Q;
03660                         pmat[1][2] = 0;  pmat[1][3] = 0;
03661                         pmat[2][2] = f;  pmat[2][3] = g;
03662                         pmat[3][2] = g;  pmat[3][3] = -f;
03663                 }
03664                 else {
03665                         pdiag[2] = h;
03666                         pdiag[3] = j;
03667                         psubd[1] = f;
03668                         psubd[2] = i;
03669                         pmat[1][2] = 0;  pmat[1][3] = 0;
03670                         pmat[2][2] = 1;  pmat[2][3] = 0;
03671                         pmat[3][2] = 0;  pmat[3][3] = 1;
03672                 }
03673         }
03674 }
03675 //---------------------------------------------------------------------------
03676 void mgcEigen::
03677 TridiagonalN (int n, float** pmat, float* pdiag, float* psubd)
03678 {
03679         int i, j, k, ell;
03680
03681         for (i = n-1, ell = n-2; i >= 1; i--, ell--) {
03682                 float h = 0, scale = 0;
03683
03684                 if ( ell > 0 ) {
03685                         for (k = 0; k <= ell; k++)
03686                                 scale += float(fabs(pmat[i][k]));
03687                         if ( scale == 0 )
03688                                 psubd[i] = pmat[i][ell];
03689                         else {
03690                                 for (k = 0; k <= ell; k++) {
03691                                         pmat[i][k] /= scale;
03692                                         h += pmat[i][k]*pmat[i][k];
03693                                 }
03694                                 float f = pmat[i][ell];
03695                                 float g = ( f > 0 ? -float(sqrt(h)) : float(sqrt(h)) );
03696                                 psubd[i] = scale*g;
03697                                 h -= f*g;
03698                                 pmat[i][ell] = f-g;
03699                                 f = 0;
03700                                 for (j = 0; j <= ell; j++) {
03701                                         pmat[j][i] = pmat[i][j]/h;
03702                                         g = 0;
03703                                         for (k = 0; k <= j; k++)
03704                                                 g += pmat[j][k]*pmat[i][k];
03705                                         for (k = j+1; k <= ell; k++)
03706                                                 g += pmat[k][j]*pmat[i][k];
03707                                         psubd[j] = g/h;
03708                                         f += psubd[j]*pmat[i][j];
03709                                 }
03710                                 float hh = f/(h+h);
03711                                 for (j = 0; j <= ell; j++) {
03712                                         f = pmat[i][j];
03713                                         psubd[j] = g = psubd[j] - hh*f;
03714                                         for (k = 0; k <= j; k++)
03715                                                 pmat[j][k] -= f*psubd[k]+g*pmat[i][k];
03716                                 }
03717             }
03718                 }
03719                 else
03720                         psubd[i] = pmat[i][ell];
03721
03722                 pdiag[i] = h;
03723         }
03724
03725         pdiag[0] = psubd[0] = 0;
03726         for (i = 0, ell = -1; i <= n-1; i++, ell++) {
03727                 if ( pdiag[i] ) {
03728                         for (j = 0; j <= ell; j++) {
03729                                 float sum = 0;
03730                                 for (k = 0; k <= ell; k++)
03731                                         sum += pmat[i][k]*pmat[k][j];
03732                                 for (k = 0; k <= ell; k++)
03733                                         pmat[k][j] -= sum*pmat[k][i];
03734                         }
03735                 }
03736                 pdiag[i] = pmat[i][i];
03737                 pmat[i][i] = 1;
03738                 for (j = 0; j <= ell; j++)
03739                         pmat[j][i] = pmat[i][j] = 0;
03740         }
03741
03742         // re-ordering if mgcEigen::QLAlgorithm is used subsequently
03743         for (i = 1, ell = 0; i < n; i++, ell++)
03744                 psubd[ell] = psubd[i];
03745         psubd[n-1] = 0;
03746 }
03747 //---------------------------------------------------------------------------
03748 void mgcEigen::
03749 QLAlgorithm (int n, float* pdiag, float* psubd, float** pmat)
03750 {
03751         const int eigen_maxiter = 30;
03752
03753         for (int ell = 0; ell < n; ell++) {
03754                 int iter;
03755                 for (iter = 0; iter < eigen_maxiter; iter++) {
03756                         int m;
03757                         for (m = ell; m <= n-2; m++) {
03758                                 float dd = float(fabs(pdiag[m])+fabs(pdiag[m+1]));
03759                                 if ( (float)(fabs(psubd[m])+dd) == dd )
03760                                         break;
03761                         }
03762                         if ( m == ell )
03763                                 break;
03764
03765                         float g = (pdiag[ell+1]-pdiag[ell])/(2*psubd[ell]);
03766                         float r = float(sqrt(g*g+1));
03767                         if ( g < 0 )
03768                                 g = pdiag[m]-pdiag[ell]+psubd[ell]/(g-r);
03769                         else
03770                                 g = pdiag[m]-pdiag[ell]+psubd[ell]/(g+r);
03771                         float s = 1, c = 1, p = 0;
03772                         for (int i = m-1; i >= ell; i--) {
03773                                 float f = s*psubd[i], b = c*psubd[i];
03774                                 if ( fabs(f) >= fabs(g) ) {
03775                                         c = g/f;
03776                                         r = float(sqrt(c*c+1));
03777                                         psubd[i+1] = f*r;
03778                                         c *= (s = 1/r);
03779                                 }
03780                                 else {
03781                                         s = f/g;
03782                                         r = float(sqrt(s*s+1));
03783                                         psubd[i+1] = g*r;
03784                                         s *= (c = 1/r);
03785                                 }
03786                                 g = pdiag[i+1]-p;
03787                                 r = (pdiag[i]-g)*s+2*b*c;
03788                                 p = s*r;
03789                                 pdiag[i+1] = g+p;
03790                                 g = c*r-b;
03791
03792                                 for (int k = 0; k < n; k++) {
03793                                         f = pmat[k][i+1];
03794                                         pmat[k][i+1] = s*pmat[k][i]+c*f;
03795                                         pmat[k][i] = c*pmat[k][i]-s*f;
03796                                 }
03797                         }
03798                         pdiag[ell] -= p;
03799                         psubd[ell] = g;
03800                         psubd[m] = 0;
03801                 }
03802                 if ( iter == eigen_maxiter ) {
03803                         Report(ql_exceeded);
03804                         return;
03805                 }
03806         }
03807 }
03808 //---------------------------------------------------------------------------
03809 void mgcEigen::
03810 DecreasingSort (int n, float* eigval, float** eigvec)
03811 {
03812         // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
03813         for (int i = 0, k; i <= n-2; i++) {
03814                 // locate maximum eigenvalue
03815                 float max = eigval[k=i];
03816                 int j;
03817                 for (j = i+1; j < n; j++)
03818                         if ( eigval[j] > max )
03819                                 max = eigval[k=j];
03820
03821                 if ( k != i ) {
03822                         // swap eigenvalues
03823                         eigval[k] = eigval[i];
03824                         eigval[i] = max;
03825
03826                         // swap eigenvectors
03827                         for (j = 0; j < n; j++) {
03828                                 float tmp = eigvec[j][i];
03829                                 eigvec[j][i] = eigvec[j][k];
03830                                 eigvec[j][k] = tmp;
03831                         }
03832                 }
03833         }
03834 }
03835 //---------------------------------------------------------------------------
03836 void mgcEigen::
03837 IncreasingSort (int n, float* eigval, float** eigvec)
03838 {
03839         // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
03840         for (int i = 0, k; i <= n-2; i++) {
03841                 // locate minimum eigenvalue
03842                 float min = eigval[k=i];
03843         int j;
03844                 for (j = i+1; j < n; j++)
03845                         if ( eigval[j] < min )
03846                                 min = eigval[k=j];
03847
03848                 if ( k != i ) {
03849                         // swap eigenvalues
03850                         eigval[k] = eigval[i];
03851                         eigval[i] = min;
03852
03853                         // swap eigenvectors
03854                         for (j = 0; j < n; j++) {
03855                                 float tmp = eigvec[j][i];
03856                                 eigvec[j][i] = eigvec[j][k];
03857                                 eigvec[j][k] = tmp;
03858                         }
03859                 }
03860         }
03861 }
03862 //---------------------------------------------------------------------------
03863 int mgcEigen::
03864 Number (unsigned single_error)
03865 {
03866         int result;
03867         for (result = -1; single_error; single_error >>= 1)
03868                 result++;
03869         return result;
03870 }
03871 //---------------------------------------------------------------------------
03872 void mgcEigen::
03873 Report (unsigned single_error)
03874 {
03875         if ( mgcEigen::verbose1 )
03876                 cout << "mgcEigen: " << message[Number(single_error)] << endl;
03877         else  {
03878                 ofstream ostr("eigen.err",ios::out|ios::app);
03879                 ostr << "mgcEigen: " << message[Number(single_error)] << endl;
03880         }
03881         error |= single_error;
03882 }
03883 //---------------------------------------------------------------------------
03884 void mgcEigen::
03885 Report (ostream& ostr)
03886 {
03887         for (unsigned single_error = 1; single_error; single_error <<= 1)
03888                 if ( error & single_error )
03889                         ostr << "mgcEigen: " << message[Number(single_error)] << endl;
03890
03891         error = 0;
03892 }
03893 //===========================================================================
03894 // error handling
03895 int mgcEigenD::verbose1 = 0;
03896 unsigned mgcEigenD::error = 0;
03897 const unsigned mgcEigenD::invalid_size      = 0x00000001;
03898 const unsigned mgcEigenD::allocation_failed = 0x00000002;
03899 const unsigned mgcEigenD::ql_exceeded       = 0x00000004;
03900 const char* mgcEigenD::message[3] = {
03901         "invalid matrix size",
03902         "allocation failed",
03903         "QL algorithm - exceeded maximum iterations"
03904 };
03905 //---------------------------------------------------------------------------
03906 mgcEigenD::
03907 mgcEigenD (int _size)
03908 {
03909         if ( (size = _size) <= 1 ) {
03910                 Report(invalid_size);
03911                 return;
03912         }
03913         if ( (mat = new double*[size]) == 0 ) {
03914                 Report(allocation_failed);
03915                 return;
03916         }
03917         for (int d = 0; d < size; d++)
03918                 if ( (mat[d] = new double[size]) == 0 ) {
03919                         Report(allocation_failed);
03920                         return;
03921                 }
03922         if ( (diag = new double[size]) == 0 ) {
03923                 Report(allocation_failed);
03924                 return;
03925         }
03926         if ( (subd = new double[size]) == 0 ) {
03927                 Report(allocation_failed);
03928                 return;
03929         }
03930 }
03931 //---------------------------------------------------------------------------
03932 mgcEigenD::
03933 ~mgcEigenD ()
03934 {
03935         delete[] subd;
03936         delete[] diag;
03937         for (int d = 0; d < size; d++)
03938                 delete[] mat[d];
03939         delete[] mat;
03940 }
03941 //---------------------------------------------------------------------------
03942 void mgcEigenD::
03943 Tridiagonal2 (double** pmat, double* pdiag, double* psubd)
03944 {
03945         // matrix is already tridiagonal
03946
03947         pdiag[0] = pmat[0][0];
03948         pdiag[1] = pmat[1][1];
03949         psubd[0] = pmat[0][1];
03950         psubd[1] = 0;
03951         pmat[0][0] = 1;  pmat[0][1] = 0;
03952         pmat[1][0] = 0;  pmat[1][1] = 1;
03953 }
03954 //---------------------------------------------------------------------------
03955 void mgcEigenD::
03956 Tridiagonal3 (double** pmat, double* pdiag, double* psubd)
03957 {
03958         double a = pmat[0][0], b = pmat[0][1], c = pmat[0][2],
03959         d = pmat[1][1], e = pmat[1][2], f = pmat[2][2];
03960
03961         pdiag[0] = a;
03962         psubd[2] = 0;
03963         if ( c != 0 ) {
03964                 double ell = sqrt(b*b+c*c);
03965                 b /= ell;
03966                 c /= ell;
03967                 double q = 2*b*e+c*(f-d);
03968                 pdiag[1] = d+c*q;
03969                 pdiag[2] = f-c*q;
03970                 psubd[0] = ell;
03971                 psubd[1] = e-b*q;
03972                 pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
03973                 pmat[1][0] = 0; pmat[1][1] = b; pmat[1][2] = c;
03974                 pmat[2][0] = 0; pmat[2][1] = c; pmat[2][2] = -b;
03975         }
03976         else {
03977                 pdiag[1] = d;
03978                 pdiag[2] = f;
03979                 psubd[0] = b;
03980                 psubd[1] = e;
03981                 pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0;
03982                 pmat[1][0] = 0; pmat[1][1] = 1; pmat[1][2] = 0;
03983                 pmat[2][0] = 0; pmat[2][1] = 0; pmat[2][2] = 1;
03984         }
03985 }
03986 //---------------------------------------------------------------------------
03987 void mgcEigenD::
03988 Tridiagonal4 (double** pmat, double* pdiag, double* psubd)
03989 {
03990         // save pmatrix M
03991         double
03992         a = pmat[0][0], b = pmat[0][1], c = pmat[0][2], d = pmat[0][3],
03993         e = pmat[1][1], f = pmat[1][2], g = pmat[1][3],
03994         h = pmat[2][2], i = pmat[2][3], j = pmat[3][3];
03995
03996         pdiag[0] = a;
03997         psubd[3] = 0;
03998
03999         pmat[0][0] = 1; pmat[0][1] = 0; pmat[0][2] = 0; pmat[0][3] = 0;
04000         pmat[1][0] = 0;
04001         pmat[2][0] = 0;
04002         pmat[3][0] = 0;
04003
04004         if ( c != 0 || d != 0 ) {
04005                 double q11, q12, q13;
04006                 double q21, q22, q23;
04007                 double q31, q32, q33;
04008
04009                 // build column Q1
04010                 double len = sqrt(b*b+c*c+d*d);
04011                 q11 = b/len;
04012                 q21 = c/len;
04013                 q31 = d/len;
04014
04015                 psubd[0] = len;
04016
04017                 // compute S*Q1
04018                 double v0 = e*q11+f*q21+g*q31;
04019                 double v1 = f*q11+h*q21+i*q31;
04020                 double v2 = g*q11+i*q21+j*q31;
04021
04022                 pdiag[1] = q11*v0+q21*v1+q31*v2;
04023
04024         // build column Q3 = Q1x(S*Q1)
04025                 q13 = q21*v2-q31*v1;
04026                 q23 = q31*v0-q11*v2;
04027                 q33 = q11*v1-q21*v0;
04028                 len = sqrt(q13*q13+q23*q23+q33*q33);
04029                 if ( len > 0 ) {
04030                         q13 /= len;
04031                         q23 /= len;
04032                         q33 /= len;
04033
04034                         // build column Q2 = Q3xQ1
04035                         q12 = q23*q31-q33*q21;
04036                         q22 = q33*q11-q13*q31;
04037                         q32 = q13*q21-q23*q11;
04038
04039                         v0 = q12*e+q22*f+q32*g;
04040                         v1 = q12*f+q22*h+q32*i;
04041                         v2 = q12*g+q22*i+q32*j;
04042                         psubd[1] = q11*v0+q21*v1+q31*v2;
04043                         pdiag[2] = q12*v0+q22*v1+q32*v2;
04044                         psubd[2] = q13*v0+q23*v1+q33*v2;
04045
04046                         v0 = q13*e+q23*f+q33*g;
04047                         v1 = q13*f+q23*h+q33*i;
04048                         v2 = q13*g+q23*i+q33*j;
04049                         pdiag[3] = q13*v0+q23*v1+q33*v2;
04050                 }
04051                 else {  // S*Q1 parallel to Q1, choose any valid Q2 and Q3
04052                         psubd[1] = 0;
04053
04054                         len = q21*q21+q31*q31;
04055                         if ( len > 0 ) {
04056                                 double tmp = q11-1;
04057                                 q12 = -q21;
04058                                 q22 = 1+tmp*q21*q21/len;
04059                 q32 = tmp*q21*q31/len;
04060
04061                                 q13 = -q31;
04062                                 q23 = q32;
04063                                 q33 = 1+tmp*q31*q31/len;
04064
04065                                 v0 = q12*e+q22*f+q32*g;
04066                                 v1 = q12*f+q22*h+q32*i;
04067                                 v2 = q12*g+q22*i+q32*j;
04068                                 pdiag[2] = q12*v0+q22*v1+q32*v2;
04069                                 psubd[2] = q13*v0+q23*v1+q33*v2;
04070
04071                                 v0 = q13*e+q23*f+q33*g;
04072                                 v1 = q13*f+q23*h+q33*i;
04073                                 v2 = q13*g+q23*i+q33*j;
04074                                 pdiag[3] = q13*v0+q23*v1+q33*v2;
04075                         }
04076                         else {  // Q1 = (+-1,0,0)
04077                                 q12 = 0; q22 = 1; q32 = 0;
04078                                 q13 = 0; q23 = 0; q33 = 1;
04079
04080                                 pdiag[2] = h;
04081                                 pdiag[3] = j;
04082                                 psubd[2] = i;
04083                         }
04084                 }
04085
04086                 pmat[1][1] = q11; pmat[1][2] = q12; pmat[1][3] = q13;
04087                 pmat[2][1] = q21; pmat[2][2] = q22; pmat[2][3] = q23;
04088                 pmat[3][1] = q31; pmat[3][2] = q32; pmat[3][3] = q33;
04089         }
04090         else {
04091                 pdiag[1] = e;
04092                 psubd[0] = b;
04093                 pmat[1][1] = 1;
04094                 pmat[2][1] = 0;
04095                 pmat[3][1] = 0;
04096
04097                 if ( g != 0 ) {
04098                         double ell = sqrt(f*f+g*g);
04099                         f /= ell;
04100                         g /= ell;
04101                         double Q = 2*f*i+g*(j-h);
04102
04103                         pdiag[2] = h+g*Q;
04104                         pdiag[3] = j-g*Q;
04105                         psubd[1] = ell;
04106                         psubd[2] = i-f*Q;
04107                         pmat[1][2] = 0;  pmat[1][3] = 0;
04108                         pmat[2][2] = f;  pmat[2][3] = g;
04109                         pmat[3][2] = g;  pmat[3][3] = -f;
04110                 }
04111                 else {
04112                         pdiag[2] = h;
04113                         pdiag[3] = j;
04114                         psubd[1] = f;
04115                         psubd[2] = i;
04116                         pmat[1][2] = 0;  pmat[1][3] = 0;
04117                         pmat[2][2] = 1;  pmat[2][3] = 0;
04118                         pmat[3][2] = 0;  pmat[3][3] = 1;
04119                 }
04120         }
04121 }
04122 //---------------------------------------------------------------------------
04123 void mgcEigenD::
04124 TridiagonalN (int n, double** pmat, double* pdiag, double* psubd)
04125 {
04126         int i, j, k, ell;
04127
04128         for (i = n-1, ell = n-2; i >= 1; i--, ell--) {
04129                 double h = 0, scale = 0;
04130
04131                 if ( ell > 0 ) {
04132                         for (k = 0; k <= ell; k++)
04133                                 scale += fabs(pmat[i][k]);
04134                         if ( scale == 0 )
04135                                 psubd[i] = pmat[i][ell];
04136                         else {
04137                                 for (k = 0; k <= ell; k++) {
04138                                         pmat[i][k] /= scale;
04139                                         h += pmat[i][k]*pmat[i][k];
04140                                 }
04141                                 double f = pmat[i][ell];
04142                                 double g = ( f > 0 ? -sqrt(h) : sqrt(h) );
04143                                 psubd[i] = scale*g;
04144                                 h -= f*g;
04145                                 pmat[i][ell] = f-g;
04146                                 f = 0;
04147                                 for (j = 0; j <= ell; j++) {
04148                                         pmat[j][i] = pmat[i][j]/h;
04149                                         g = 0;
04150                                         for (k = 0; k <= j; k++)
04151                                                 g += pmat[j][k]*pmat[i][k];
04152                                         for (k = j+1; k <= ell; k++)
04153                                                 g += pmat[k][j]*pmat[i][k];
04154                                         psubd[j] = g/h;
04155                                         f += psubd[j]*pmat[i][j];
04156                                 }
04157                                 double hh = f/(h+h);
04158                                 for (j = 0; j <= ell; j++) {
04159                                         f = pmat[i][j];
04160                                         psubd[j] = g = psubd[j] - hh*f;
04161                                         for (k = 0; k <= j; k++)
04162                                                 pmat[j][k] -= f*psubd[k]+g*pmat[i][k];
04163                                 }
04164             }
04165                 }
04166                 else
04167                         psubd[i] = pmat[i][ell];
04168
04169                 pdiag[i] = h;
04170         }
04171
04172         pdiag[0] = psubd[0] = 0;
04173         for (i = 0, ell = -1; i <= n-1; i++, ell++) {
04174                 if ( pdiag[i] ) {
04175                         for (j = 0; j <= ell; j++) {
04176                                 double sum = 0;
04177                                 for (k = 0; k <= ell; k++)
04178                                         sum += pmat[i][k]*pmat[k][j];
04179                                 for (k = 0; k <= ell; k++)
04180                                         pmat[k][j] -= sum*pmat[k][i];
04181                         }
04182                 }
04183                 pdiag[i] = pmat[i][i];
04184                 pmat[i][i] = 1;
04185                 for (j = 0; j <= ell; j++)
04186                         pmat[j][i] = pmat[i][j] = 0;
04187         }
04188
04189         // re-ordering if mgcEigenD::QLAlgorithm is used subsequently
04190         for (i = 1, ell = 0; i < n; i++, ell++)
04191                 psubd[ell] = psubd[i];
04192         psubd[n-1] = 0;
04193 }
04194 //---------------------------------------------------------------------------
04195 void mgcEigenD::
04196 QLAlgorithm (int n, double* pdiag, double* psubd, double** pmat)
04197 {
04198         const int eigen_maxiter = 30;
04199
04200         for (int ell = 0; ell < n; ell++) {
04201                 int iter;
04202                 for (iter = 0; iter < eigen_maxiter; iter++) {
04203                         int m;
04204                         for (m = ell; m <= n-2; m++) {
04205                                 double dd = fabs(pdiag[m])+fabs(pdiag[m+1]);
04206                                 if ( (double)(fabs(psubd[m])+dd) == dd )
04207                                         break;
04208                         }
04209                         if ( m == ell )
04210                                 break;
04211
04212                         double g = (pdiag[ell+1]-pdiag[ell])/(2*psubd[ell]);
04213                         double r = sqrt(g*g+1);
04214                         if ( g < 0 )
04215                                 g = pdiag[m]-pdiag[ell]+psubd[ell]/(g-r);
04216                         else
04217                                 g = pdiag[m]-pdiag[ell]+psubd[ell]/(g+r);
04218                         double s = 1, c = 1, p = 0;
04219                         for (int i = m-1; i >= ell; i--) {
04220                                 double f = s*psubd[i], b = c*psubd[i];
04221                                 if ( fabs(f) >= fabs(g) ) {
04222                                         c = g/f;
04223                                         r = sqrt(c*c+1);
04224                                         psubd[i+1] = f*r;
04225                                         c *= (s = 1/r);
04226                                 }
04227                                 else {
04228                                         s = f/g;
04229                                         r = sqrt(s*s+1);
04230                                         psubd[i+1] = g*r;
04231                                         s *= (c = 1/r);
04232                                 }
04233                                 g = pdiag[i+1]-p;
04234                                 r = (pdiag[i]-g)*s+2*b*c;
04235                                 p = s*r;
04236                                 pdiag[i+1] = g+p;
04237                                 g = c*r-b;
04238
04239                                 for (int k = 0; k < n; k++) {
04240                                         f = pmat[k][i+1];
04241                                         pmat[k][i+1] = s*pmat[k][i]+c*f;
04242                                         pmat[k][i] = c*pmat[k][i]-s*f;
04243                                 }
04244                         }
04245                         pdiag[ell] -= p;
04246                         psubd[ell] = g;
04247                         psubd[m] = 0;
04248                 }
04249                 if ( iter == eigen_maxiter ) {
04250                         Report(ql_exceeded);
04251                         return;
04252                 }
04253         }
04254 }
04255 //---------------------------------------------------------------------------
04256 void mgcEigenD::
04257 DecreasingSort (int n, double* eigval, double** eigvec)
04258 {
04259         // sort eigenvalues in decreasing order, e[0] >= ... >= e[n-1]
04260         for (int i = 0, k; i <= n-2; i++) {
04261                 // locate maximum eigenvalue
04262                 double max = eigval[k=i];
04263                 int j;
04264                 for (j = i+1; j < n; j++)
04265                         if ( eigval[j] > max )
04266                                 max = eigval[k=j];
04267
04268                 if ( k != i ) {
04269                         // swap eigenvalues
04270                         eigval[k] = eigval[i];
04271                         eigval[i] = max;
04272
04273                         // swap eigenvectors
04274                         for (j = 0; j < n; j++) {
04275                                 double tmp = eigvec[j][i];
04276                                 eigvec[j][i] = eigvec[j][k];
04277                                 eigvec[j][k] = tmp;
04278                         }
04279                 }
04280         }
04281 }
04282 //---------------------------------------------------------------------------
04283 void mgcEigenD::
04284 IncreasingSort (int n, double* eigval, double** eigvec)
04285 {
04286         // sort eigenvalues in increasing order, e[0] <= ... <= e[n-1]
04287         for (int i = 0, k; i <= n-2; i++) {
04288                 // locate minimum eigenvalue
04289                 double min = eigval[k=i];
04290         int j;
04291                 for (j = i+1; j < n; j++)
04292                         if ( eigval[j] < min )
04293                                 min = eigval[k=j];
04294
04295                 if ( k != i ) {
04296                         // swap eigenvalues
04297                         eigval[k] = eigval[i];
04298                         eigval[i] = min;
04299
04300                         // swap eigenvectors
04301                         for (j = 0; j < n; j++) {
04302                                 double tmp = eigvec[j][i];
04303                                 eigvec[j][i] = eigvec[j][k];
04304                                 eigvec[j][k] = tmp;
04305                         }
04306                 }
04307         }
04308 }
04309 //---------------------------------------------------------------------------
04310 mgcEigenD& mgcEigenD::
04311 Matrix (double** inmat)
04312 {
04313         for (int row = 0; row < size; row++)
04314                 for (int col = 0; col < size; col++)
04315                         mat[row][col] = inmat[row][col];
04316         return *this;
04317 }
04318 //---------------------------------------------------------------------------
04319 void mgcEigenD::
04320 EigenStuff3 ()
04321 {
04322         Tridiagonal3(mat,diag,subd);
04323         QLAlgorithm(size,diag,subd,mat);
04324 }
04325 //---------------------------------------------------------------------------
04326 int mgcEigenD::
04327 Number (unsigned single_error)
04328 {
04329         int result;
04330         for (result = -1; single_error; single_error >>= 1)
04331                 result++;
04332         return result;
04333 }
04334 //---------------------------------------------------------------------------
04335 void mgcEigenD::
04336 Report (unsigned single_error)
04337 {
04338         if ( mgcEigenD::verbose1 )
04339                 cout << "mgcEigenD: " << message[Number(single_error)] << endl;
04340         else {
04341           ofstream ostr("eigen.err",ios::out|ios::app);
04342           ostr << "mgcEigenD: " << message[Number(single_error)] << endl;
04343         }
04344         error |= single_error;
04345 }
04346 //---------------------------------------------------------------------------
04347 void mgcEigenD::
04348 Report (ostream& ostr)
04349 {
04350         for (unsigned single_error = 1; single_error; single_error <<= 1)
04351                 if ( error & single_error )
04352                         ostr << "mgcEigenD: " << message[Number(single_error)] << endl;
04353
04354         error = 0;
04355 }
04356 //===========================================================================
04357
04358 #ifdef EIGEN_TEST
04359
04360 int main ()
04361 {
04362         mgcEigenD eig(3);
04363
04364         eig.Matrix(0,0) = 2;  eig.Matrix(0,1) = 1;  eig.Matrix(0,2) = 1;
04365         eig.Matrix(1,0) = 1;  eig.Matrix(1,1) = 2;  eig.Matrix(1,2) = 1;
04366         eig.Matrix(2,0) = 1;  eig.Matrix(2,1) = 1;  eig.Matrix(2,2) = 2;
04367
04368         eig.IncrSortEigenStuff3();
04369
04370         cout.setf(ios::fixed);
04371
04372         cout << "eigenvalues = " << endl;
04373         for (int row = 0; row < 3; row++)
04374                 cout << eig.Eigenvalue(row) << ' ';
04375         cout << endl;
04376
04377         cout << "eigenvectors = " << endl;
04378         for (row = 0; row < 3; row++) {
04379                 for (int col = 0; col < 3; col++)
04380                         cout << eig.Eigenvector(row,col) << ' ';
04381                 cout << endl;
04382         }
04383
04384         // eigenvalues =
04385         //    1.000000 1.000000 4.000000
04386         // eigenvectors =
04387         //    0.411953  0.704955 0.577350
04388         //    0.404533 -0.709239 0.577350
04389         //   -0.816485  0.004284 0.577350
04390
04391         return 0;
04392 }
04393 #endif
04394 #endif
04395
04396 // FILE: tri3tri3.cpp
04397 //---------------------------------------------------------------------------
04398 // MinTriangleTriangle
04399 //
04400 // The quadratic form representing the squared distance between two planes
04401 // never has an isolated global minimum.  The generic case is for it to
04402 // have an entire line of zeros (the line of intersection of the two
04403 // planes).  Therefore, it is sufficient to compare edges of each triangle
04404 // with the entire other triangle, looking for the minimum distance.
04405 //---------------------------------------------------------------------------
04406
04407 // NOTE:  This code is not fully optimized as is the code in pt3lin3,
04408 // pt3tri3, and lin3lin3.
04409
04410 //---------------------------------------------------------------------------
04411 double
04412 AnalyticGeometryTool::MinTriangleTriangle (Triangle3& tri0,
04413                                            Triangle3& tri1,
04414                                            double& s, double& t, double& u,
04415                                            double& v)
04416 {
04417     double s0, t0, u0, v0, min, min0;
04418     Line3 line;
04419
04420     // compare edges of tri0 against all of tri1
04421     line.b = tri0.b;
04422     line.m = tri0.e0;
04423     min = MinLineSegmentTriangle(line,tri1,s,u,v);
04424     t = 0;
04425
04426     line.m = tri0.e1;
04427     min0 = MinLineSegmentTriangle(line,tri1,t0,u0,v0);
04428     s0 = 0;
04429     if ( min0 < min )
04430     {
04431         min = min0;
04432         s = s0;
04433         t = t0;
04434         u = u0;
04435         v = v0;
04436     }
04437
04438     line.b = line.b + tri0.e0;
04439     line.m = line.m - tri0.e0;
04440     min0 = MinLineSegmentTriangle(line,tri1,t0,u0,v0);
04441     s0 = 1-t0;
04442     if ( min0 < min )
04443     {
04444         min = min0;
04445         s = s0;
04446         t = t0;
04447         u = u0;
04448         v = v0;
04449     }
04450
04451     // compare edges of tri1 against all of tri0
04452     line.b = tri1.b;
04453     line.m = tri1.e0;
04454     min0 = MinLineSegmentTriangle(line,tri0,u0,s0,t0);
04455     v0 = 0;
04456     if ( min0 < min )
04457     {
04458         min = min0;
04459         s = s0;
04460         t = t0;
04461         u = u0;
04462         v = v0;
04463     }
04464
04465     line.m = tri1.e1;
04466     min0 = MinLineSegmentTriangle(line,tri0,v0,s0,t0);
04467     u0 = 0;
04468     if ( min0 < min )
04469     {
04470         min = min0;
04471         s = s0;
04472         t = t0;
04473         u = u0;
04474         v = v0;
04475     }
04476
04477     line.b = line.b + tri1.e0;
04478     line.m = line.m - tri1.e0;
04479     min0 = MinLineSegmentTriangle(line,tri0,v0,s0,t0);
04480     u0 = 1-v0;
04481     if ( min0 < min )
04482     {
04483         min = min0;
04484         s = s0;
04485         t = t0;
04486         u = u0;
04487         v = v0;
04488     }
04489
04490     return min;
04491 }
04492
04493 //---------------------------------------------------------------------------
04494
04495 #ifdef TRI3TRI3_TEST
04496
04497 //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
04498
04499 //ofstream ostr("data.txt");
04500
04501 void main ()
04502 {
04503     Triangle3 tri0, tri1;
04504     Point3 p, q, diff;
04505     double u, v, s, t, u0, v0, s0, t0, min0, min1, dist;
04506
04507     double maxdiff = 0;
04508
04509     for (int i = 0; i < 128; i++)
04510     {
04511         tri0.b.x = RAND;
04512         tri0.b.y = RAND;
04513         tri0.b.z = RAND;
04514         tri0.e0.x = RAND;
04515         tri0.e0.y = RAND;
04516         tri0.e0.z = RAND;
04517         tri0.e1.x = RAND;
04518         tri0.e1.y = RAND;
04519         tri0.e1.z = RAND;
04520
04521         tri1.b.x = RAND;
04522         tri1.b.y = RAND;
04523         tri1.b.z = RAND;
04524         tri1.e0.x = RAND;
04525         tri1.e0.y = RAND;
04526         tri1.e0.z = RAND;
04527         tri1.e1.x = RAND;
04528         tri1.e1.y = RAND;
04529         tri1.e1.z = RAND;
04530
04531         min0 = FLT_MAX;
04532         int max = 32;
04533         for (int w = 0; w <= max; w++)
04534         {
04535             t0 = w/double(max);
04536             for (int z = 0; w+z <= max; z++)
04537             {
04538                 s0 = z/double(max);
04539                 p = tri0.b+s0*tri0.e0+t0*tri0.e1;
04540                 for (int y = 0; y <= max; y++)
04541                 {
04542                     v0 = y/double(max);
04543                     for (int x = 0; x+y <= max; x++)
04544                     {
04545                         u0 = x/double(max);
04546                         q = tri1.b+u0*tri1.e0+v0*tri1.e1;
04547
04548                         diff = p-q;
04549                         dist = Length(diff);
04550                         if ( dist < min0 )
04551                         {
04552                             min0 = dist;
04553                             s = s0;
04554                             t = t0;
04555                             u = u0;
04556                             v = v0;
04557                         }
04558                     }
04559                 }
04560             }
04561         }
04562         ostr << "i = " << i << endl;
04563         ostr << "sampled = " << s << ' ' << t << ' ' << u << ' '
04564              << v << ' ' << min0 << endl;
04565
04566         min1 = MinTriangleTriangle(tri0,tri1,s,t,u,v);
04567         ostr << "analytic = " << s << ' ' << t << ' ' << u << ' '
04568              << v << ' ' << min1 << endl;
04569
04570         ostr << "diff = " << min1-min0 << endl;
04571
04572         if ( min1-min0 > maxdiff )
04573             maxdiff = min1-min0;
04574
04575         ostr << endl;
04576     }
04577
04578     ostr << "max diff = " << maxdiff << endl;
04579 }
04580 #endif
04581
04582 //---------------------------------------------------------------------------
04583 double
04584 AnalyticGeometryTool::MinLineSegmentLineSegment (const Line3& seg0, const Line3& seg1,
04585                                                  double& s, double& t)
04586 {
04587     Point3 diff = seg0.b - seg1.b;
04588     double A = Dot(seg0.m,seg0.m);
04589     double B = -Dot(seg0.m,seg1.m);
04590     double C = Dot(seg1.m,seg1.m);
04591     double D = Dot(seg0.m,diff);
04592     double E;  // -Dot(seg1.m,diff), defer until needed
04593     double F = Dot(diff,diff);
04594     double det = Abs(A*C-B*B);  // A*C-B*B = |Cross(M0,M1)|^2 >= 0
04595
04596     double tmp;
04597
04598     if ( det >= par_tolerance )
04599     {
04600         // line segments are not parallel
04601         E = -Dot(seg1.m,diff);
04602         s = B*E-C*D;
04603         t = B*D-A*E;
04604
04605         if ( s >= 0 )
04606         {
04607             if ( s <= det )
04608             {
04609                 if ( t >= 0 )
04610                 {
04611                     if ( t <= det )  // region 0 (interior)
04612                     {
04613                         // minimum at two interior points of 3D lines
04614                         double invDet = 1.0f/det;
04615                         s *= invDet;
04616                         t *= invDet;
04617                         return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
04618                     }
04619                     else  // region 3 (side)
04620                     {
04621                         t = 1;
04622                         tmp = B+D;
04623                         if ( tmp >= 0 )
04624                         {
04625                             s = 0;
04626                             return DIST(C+2*E+F);
04627                         }
04628                         else if ( -tmp >= A )
04629                         {
04630                             s = 1;
04631                             return DIST(A+C+F+2*(E+tmp));
04632                         }
04633                         else
04634                         {
04635                             s = -tmp/A;
04636                             return DIST(tmp*s+C+2*E+F);
04637                         }
04638                     }
04639                 }
04640                 else  // region 7 (side)
04641                 {
04642                     t = 0;
04643                     if ( D >= 0 )
04644                     {
04645                         s = 0;
04646                         return DIST(F);
04647                     }
04648                     else if ( -D >= A )
04649                     {
04650                         s = 1;
04651                         return DIST(A+2*D+F);
04652                     }
04653                     else
04654                     {
04655                         s = -D/A;
04656                         return DIST(D*s+F);
04657                     }
04658                 }
04659             }
04660             else
04661             {
04662                 if ( t >= 0 )
04663                 {
04664                     if ( t <= det )  // region 1 (side)
04665                     {
04666                         s = 1;
04667                         tmp = B+E;
04668                         if ( tmp >= 0 )
04669                         {
04670                             t = 0;
04671                             return DIST(A+2*D+F);
04672                         }
04673                         else if ( -tmp >= C )
04674                         {
04675                             t = 1;
04676                             return DIST(A+C+F+2*(D+tmp));
04677                         }
04678                         else
04679                         {
04680                             t = -tmp/C;
04681                             return DIST(tmp*t+A+2*D+F);
04682                         }
04683                     }
04684                     else  // region 2 (corner)
04685                     {
04686                         tmp = B+D;
04687                         if ( -tmp <= A )
04688                         {
04689                             t = 1;
04690                             if ( tmp >= 0 )
04691                             {
04692                                 s = 0;
04693                                 return DIST(C+2*E+F);
04694                             }
04695                             else
04696                             {
04697                                  s = -tmp/A;
04698                                  return DIST(tmp*s+C+2*E+F);
04699                             }
04700                         }
04701                         else
04702                         {
04703                             s = 1;
04704                             tmp = B+E;
04705                             if ( tmp >= 0 )
04706                             {
04707                                 t = 0;
04708                                 return DIST(A+2*D+F);
04709                             }
04710                             else if ( -tmp >= C )
04711                             {
04712                                 t = 1;
04713                                 return DIST(A+C+F+2*(D+tmp));
04714                             }
04715                             else
04716                             {
04717                                 t = -tmp/C;
04718                                 return DIST(tmp*t+A+2*D+F);
04719                             }
04720                         }
04721                     }
04722                 }
04723                 else  // region 8 (corner)
04724                 {
04725                     if ( -D < A )
04726                     {
04727                         t = 0;
04728                         if ( D >= 0 )
04729                         {
04730                             s = 0;
04731                             return DIST(F);
04732                         }
04733                         else
04734                         {
04735                             s = -D/A;
04736                             return DIST(D*s+F);
04737                         }
04738                     }
04739                     else
04740                     {
04741                         s = 1;
04742                         tmp = B+E;
04743                         if ( tmp >= 0 )
04744                         {
04745                             t = 0;
04746                             return DIST(A+2*D+F);
04747                         }
04748                         else if ( -tmp >= C )
04749                         {
04750                             t = 1;
04751                             return DIST(A+C+F+2*(D+tmp));
04752                         }
04753                         else
04754                         {
04755                             t = -tmp/C;
04756                             return DIST(tmp*t+A+2*D+F);
04757                         }
04758                     }
04759                 }
04760             }
04761         }
04762         else
04763         {
04764             if ( t >= 0 )
04765             {
04766                 if ( t <= det )  // region 5 (side)
04767                 {
04768                     s = 0;
04769                     if ( E >= 0 )
04770                     {
04771                         t = 0;
04772                         return DIST(F);
04773                     }
04774                     else if ( -E >= C )
04775                     {
04776                         t = 1;
04777                         return DIST(C+2*E+F);
04778                     }
04779                     else
04780                     {
04781                         t = -E/C;
04782                         return DIST(E*t+F);
04783                     }
04784                 }
04785                 else  // region 4 (corner)
04786                 {
04787                     tmp = B+D;
04788                     if ( tmp < 0 )
04789                     {
04790                         t = 1;
04791                         if ( -tmp >= A )
04792                         {
04793                             s = 1;
04794                             return DIST(A+C+F+2*(E+tmp));
04795                         }
04796                         else
04797                         {
04798                             s = -tmp/A;
04799                             return DIST(tmp*s+C+2*E+F);
04800                         }
04801                     }
04802                     else
04803                     {
04804                         s = 0;
04805                         if ( E >= 0 )
04806                         {
04807                             t = 0;
04808                             return DIST(F);
04809                         }
04810                         else if ( -E >= C )
04811                         {
04812                             t = 1;
04813                             return DIST(C+2*E+F);
04814                         }
04815                         else
04816                         {
04817                             t = -E/C;
04818                             return DIST(E*t+F);
04819                         }
04820                     }
04821                 }
04822             }
04823             else   // region 6 (corner)
04824             {
04825                 if ( D < 0 )
04826                 {
04827                     t = 0;
04828                     if ( -D >= A )
04829                     {
04830                         s = 1;
04831                         return DIST(A+2*D+F);
04832                     }
04833                     else
04834                     {
04835                         s = -D/A;
04836                         return DIST(D*s+F);
04837                     }
04838                 }
04839                 else
04840                 {
04841                     s = 0;
04842                     if ( E >= 0 )
04843                     {
04844                         t = 0;
04845                         return DIST(F);
04846                     }
04847                     else if ( -E >= C )
04848                     {
04849                         t = 1;
04850                         return DIST(C+2*E+F);
04851                     }
04852                     else
04853                     {
04854                         t = -E/C;
04855                         return DIST(E*t+F);
04856                     }
04857                 }
04858             }
04859         }
04860     }
04861     else
04862     {
04863         // line segments are parallel
04864         if ( B > 0 )
04865         {
04866             // direction vectors form an obtuse angle
04867             if ( D >= 0 )
04868             {
04869                 s = 0;
04870                 t = 0;
04871                 return DIST(F);
04872             }
04873             else if ( -D <= A )
04874             {
04875                 s = -D/A;
04876                 t = 0;
04877                 return DIST(D*s+F);
04878             }
04879             else
04880             {
04881                 E = -Dot(seg1.m,diff);
04882                 s = 1;
04883                 tmp = A+D;
04884                 if ( -tmp >= B )
04885                 {
04886                     t = 1;
04887                     return DIST(A+C+F+2*(B+D+E));
04888                 }
04889                 else
04890                 {
04891                     t = -tmp/B;
04892                     return DIST(A+2*D+F+t*(C*t+2*(B+E)));
04893                 }
04894             }
04895         }
04896         else
04897         {
04898             // direction vectors form an acute angle
04899             if ( -D >= A )
04900             {
04901                 s = 1;
04902                 t = 0;
04903                 return DIST(A+2*D+F);
04904             }
04905             else if ( D <= 0 )
04906             {
04907                 s = -D/A;
04908                 t = 0;
04909                 return DIST(D*s+F);
04910             }
04911             else
04912             {
04913                 E = -Dot(seg1.m,diff);
04914                 s = 0;
04915                 if ( D >= -B )
04916                 {
04917                     t = 1;
04918                     return DIST(C+2*E+F);
04919                 }
04920                 else
04921                 {
04922                     t = -D/B;
04923                     return DIST(F+t*(2*E+C*t));
04924                 }
04925             }
04926         }
04927     }
04928 }
04929 //---------------------------------------------------------------------------
04930
04931 #ifdef LIN3LIN3_TEST
04932
04933 //#include <stdlib.h>
04934 //#include <fstream.h>
04935
04936 //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
04937
04938 //ofstream ostr("data.txt");
04939
04940 void TestSegSeg ()
04941 {
04942     Line3 seg0, seg1;
04943     Point3 p0, p1, diff;
04944     double s, t, s0, t0, min0, min1, dist;
04945     double maxDiff = 0.0f;
04946
04947     for (int i = 0; i < 128; i++)
04948     {
04949         seg0.b.x = RAND;
04950         seg0.b.y = RAND;
04951         seg0.b.z = RAND;
04952
04953         seg0.m.x = RAND;
04954         seg0.m.y = RAND;
04955         seg0.m.z = RAND;
04956
04957         seg1.b.x = RAND;
04958         seg1.b.y = RAND;
04959         seg1.b.z = RAND;
04960
04961         if ( i % 2 )
04962         {
04963             // non-parallel line segments
04964             seg1.m.x = RAND;
04965             seg1.m.y = RAND;
04966             seg1.m.z = RAND;
04967         }
04968         else
04969         {
04970             // parallel line segments
04971             double scale = RAND;
04972             seg1.m = scale*seg0.m;
04973         }
04974
04975         min0 = FLT_MAX;
04976         int ymax = 128, xmax = 128;
04977         for (int y = 0; y < ymax; y++)
04978         {
04979             s0 = y/double(ymax-1);
04980             p0 = seg0.b+s0*seg0.m;
04981             for (int x = 0; x < xmax; x++)
04982             {
04983                 t0 = x/double(xmax-1);
04984                 p1 = seg1.b+t0*seg1.m;
04985
04986                 diff = p1-p0;
04987                 dist = Length(diff);
04988                 if ( dist < min0 )
04989                 {
04990                     min0 = dist;
04991                     s = s0;
04992                     t = t0;
04993                 }
04994             }
04995         }
04996         ostr << "sampled = " << s << ' ' << t << ' ' << min0 << endl;
04997
04998         min1 = MinLineSegmentLineSegment(seg0,seg1,s,t);
04999         ostr << "analytic = " << s << ' ' << t << ' ' << min1 << endl;
05000
05001         double compDiff = min1-min0;
05002         ostr << "diff = " << compDiff << endl;
05003         if ( compDiff > maxDiff )
05004             maxDiff = compDiff;
05005
05006         ostr << endl;
05007     }
05008     ostr << "max diff = " << maxDiff << endl;
05009 }
05010 #endif
05011
05012 // FILE: pt3tri3.cpp
05013 //---------------------------------------------------------------------------
05014 double
05015 AnalyticGeometryTool::MinPointTriangle (const Point3& p, const Triangle3& tri,
05016                                         double& s, double& t)
05017 {
05018     Point3 diff = tri.b - p;
05019     double A = Dot(tri.e0,tri.e0);
05020     double B = Dot(tri.e0,tri.e1);
05021     double C = Dot(tri.e1,tri.e1);
05022     double D = Dot(tri.e0,diff);
05023     double E = Dot(tri.e1,diff);
05024     double F = Dot(diff,diff);
05025     double det = Abs(A*C-B*B);  // A*C-B*B = |Cross(e0,e1)|^2 >= 0
05026     // A-2*B+C = Dot(e0,e0)-2*Dot(e0,e1)+Dot(e1,e1) = |e0-e1|^2 > 0
05027
05028     s = B*E-C*D;
05029     t = B*D-A*E;
05030
05031     if ( s+t <= det )
05032     {
05033         if ( s < 0 )
05034         {
05035             if ( t < 0 )  // region 4
05036             {
05037                 if ( D < 0 )
05038                 {
05039                     t = 0;
05040                     if ( -D >= A )
05041                     {
05042                         s = 1;
05043                         return DIST(A+2*D+F);
05044                     }
05045                     else
05046                     {
05047                         s = -D/A;
05048                         return DIST(D*s+F);
05049                     }
05050                 }
05051                 else
05052                 {
05053                     s = 0;
05054                     if ( E >= 0 )
05055                     {
05056                         t = 0;
05057                         return DIST(F);
05058                     }
05059                     else if ( -E >= C )
05060                     {
05061                         t = 1;
05062                         return DIST(C+2*E+F);
05063                     }
05064                     else
05065                     {
05066                         t = -E/C;
05067                         return DIST(E*t+F);
05068                     }
05069                 }
05070             }
05071             else  // region 3
05072             {
05073                 s = 0;
05074                 if ( E >= 0 )
05075                 {
05076                     t = 0;
05077                     return DIST(F);
05078                 }
05079                 else if ( -E >= C )
05080                 {
05081                     t = 1;
05082                     return DIST(C+2*E+F);
05083                 }
05084                 else
05085                 {
05086                     t = -E/C;
05087                     return DIST(E*t+F);
05088                 }
05089             }
05090         }
05091         else if ( t < 0 )  // region 5
05092         {
05093             t = 0;
05094             if ( D >= 0 )
05095             {
05096                 s = 0;
05097                 return DIST(F);
05098             }
05099             else if ( -D >= A )
05100             {
05101                 s = 1;
05102                 return DIST(A+2*D+F);
05103             }
05104             else
05105             {
05106                 s = -D/A;
05107                 return DIST(D*s+F);
05108             }
05109         }
05110         else  // region 0
05111         {
05112             // minimum at interior point
05113           if( det == 0.0 )
05114           {
05115             //PRINT_WARNING( "Found zero determinant\n" );
05116             return CUBIT_DBL_MAX;
05117           }
05118
05119             double invDet = 1.0f/det;
05120             s *= invDet;
05121             t *= invDet;
05122             return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
05123         }
05124     }
05125     else
05126     {
05127         double tmp0, tmp1, numer, denom;
05128
05129         if ( s < 0 )  // region 2
05130         {
05131             tmp0 = B+D;
05132             tmp1 = C+E;
05133             if ( tmp1 > tmp0 )
05134             {
05135                 numer = tmp1 - tmp0;
05136                 denom = A-2*B+C;
05137                 if ( numer >= denom )
05138                 {
05139                     s = 1;
05140                     t = 0;
05141                     return DIST(A+2*D+F);
05142                 }
05143                 else
05144                 {
05145                     s = numer/denom;
05146                     t = 1-s;
05147                     return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
05148                 }
05149             }
05150             else
05151             {
05152                 s = 0;
05153                 if ( tmp1 <= 0 )
05154                 {
05155                     t = 1;
05156                     return DIST(C+2*E+F);
05157                 }
05158                 else if ( E >= 0 )
05159                 {
05160                     t = 0;
05161                     return DIST(F);
05162                 }
05163                 else
05164                 {
05165                     t = -E/C;
05166                     return DIST(E*t+F);
05167                 }
05168             }
05169         }
05170         else if ( t < 0 )  // region 6
05171         {
05172             tmp0 = B+E;
05173             tmp1 = A+D;
05174             if ( tmp1 > tmp0 )
05175             {
05176                 numer = tmp1 - tmp0;
05177                 denom = A-2*B+C;
05178                 if ( numer >= denom )
05179                 {
05180                     t = 1;
05181                     s = 0;
05182                     return DIST(C+2*E+F);
05183                 }
05184                 else
05185                 {
05186                     t = numer/denom;
05187                     s = 1-t;
05188                     return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
05189                 }
05190             }
05191             else
05192             {
05193                 t = 0;
05194                 if ( tmp1 <= 0 )
05195                 {
05196                     s = 1;
05197                     return DIST(A+2*D+F);
05198                 }
05199                 else if ( D >= 0 )
05200                 {
05201                     s = 0;
05202                     return DIST(F);
05203                 }
05204                 else
05205                 {
05206                     s = -D/A;
05207                     return DIST(D*s+F);
05208                 }
05209             }
05210         }
05211         else  // region 1
05212         {
05213             numer = C+E-B-D;
05214             if ( numer <= 0 )
05215             {
05216                 s = 0;
05217                 t = 1;
05218                 return DIST(C+2*E+F);
05219             }
05220             else
05221             {
05222                 denom = A-2*B+C;
05223                 if ( numer >= denom )
05224                 {
05225                     s = 1;
05226                     t = 0;
05227                     return DIST(A+2*D+F);
05228                 }
05229                 else
05230                 {
05231                     s = numer/denom;
05232                     t = 1-s;
05233                     return DIST(s*(A*s+B*t+2*D)+t*(B*s+C*t+2*E)+F);
05234                 }
05235             }
05236         }
05237     }
05238 }
05239 //---------------------------------------------------------------------------
05240
05241 #ifdef PT3TRI3_TEST
05242
05243 //#include <float.h>
05244 //#include <fstream.h>
05245 //#include <stdlib.h>
05246
05247 //ofstream ostr("data.txt");
05248
05249 //#define RAND (2.0f*rand()/double(RAND_MAX)-1)
05250
05251 void main ()
05252 {
05253     Triangle3 tri;
05254     Point3 p, q, diff;
05255     double s, t, s0, t0, min0, min1, dist;
05256
05257     double maxdiff = 0;
05258
05259     for (int i = 0; i < 128; i++)
05260     {
05261         tri.b.x = RAND;
05262         tri.b.y = RAND;
05263         tri.b.z = RAND;
05264         tri.e0.x = RAND;
05265         tri.e0.y = RAND;
05266         tri.e0.z = RAND;
05267         tri.e1.x = RAND;
05268         tri.e1.y = RAND;
05269         tri.e1.z = RAND;
05270
05271         p.x = RAND;
05272         p.y = RAND;
05273         p.z = RAND;
05274
05275         min0 = FLT_MAX;
05276         int max = 128;
05277         for (int y = 0; y <= max; y++)
05278         {
05279             s0 = y/double(max);
05280             for (int x = 0; x+y <= max; x++)
05281             {
05282                 t0 = x/double(max);
05283                 q = tri.b+s0*tri.e0+t0*tri.e1;
05284
05285                 diff = p-q;
05286                 dist = Length(diff);
05287                 if ( dist < min0 )
05288                 {
05289                     min0 = dist;
05290                     s = s0;
05291                     t = t0;
05292                 }
05293             }
05294         }
05295         ostr << "sampled = " << s << ' ' << t << ' ' << min0 << endl;
05296
05297         min1 = MinPointTriangle(p,tri,s,t);
05298         ostr << "analytic = " << s << ' ' << t << ' ' << min1 << endl;
05299
05300         ostr << "diff = " << min1-min0 << endl;
05301
05302         if ( min1-min0 > maxdiff )
05303             maxdiff = min1-min0;
05304
05305         ostr << endl;
05306     }
05307     ostr << "max diff = " << maxdiff << endl;
05308 }
05309 #endif
05310
05311 // FILE: lin3tri3.cpp
05312 //---------------------------------------------------------------------------
05313 // This code computes the closest points of a line L(r) = a0+r*a1 and a
05314 // triangle Tri(s,t) = b0+s*b1+t*b2, where 0 <= r <= 1 and 0 <= s <= 1,
05315 // 0 <= t <= 1, and 0 <= s+t <= 1.
05316 //
05317 // In calculus terms, the goal is to minimize the squared-distance function
05318 // Q(r,s,t) = Dot(L(r)-Tri(s,t),L(r)-Tri(s,t)) over the prism domain
05319 // 0 <= r <= 1, 0 <= s <= 1, 0 <= t <= 1, 0 <= s+t <= 1.  This function is
05320 //
05321 //   Q(r,s,t) = [r s t] A [r s t] + 2 B [r s t] + C
05322 //
05323 // where A is a 3x3 symmetric matrix, B is a 3x1 column vector, and C is
05324 // a constant.  The entries of A and B are various dot products derived
05325 // from the vectors a0, a1, b0, b1, and b2.
05326 //
05327 // The analysis is similar to that of MinPointTriangle.  The (s,t) domain
05328 // is partitioned similarly, but the prism is obtained by extruding the
05329 // domain in the r-direction.  The three cases for r are: r < 0, 0 <= r <= 1,
05330 // and r > 1.  For each case the partitioning in (s,t) is identical to that
05331 // of MinPointTriangle.  The region numbers for r < 0 are appended with an
05332 // 'm' and the region numbers for r > 1 are appended with a 'p'.
05333 //---------------------------------------------------------------------------
05334
05335 // NOTE:  This code is not fully optimized as is the code in pt3lin3,
05336 // pt3tri3, and lin3lin3.
05337
05338 //---------------------------------------------------------------------------
05339 double
05340 AnalyticGeometryTool::MinLineSegmentTriangle (const Line3& seg, const Triangle3& tri,
05341                                               double& r, double& s, double& t)
05342 {
05343     Point3 diff = tri.b - seg.b;
05344     double A00 = Dot(seg.m,seg.m);
05345     double A01 = -Dot(seg.m,tri.e0);
05346     double A02 = -Dot(seg.m,tri.e1);
05347     double A11 = Dot(tri.e0,tri.e0);
05348     double A12 = Dot(tri.e0,tri.e1);
05349     double A22 = Dot(tri.e1,tri.e1);
05350     double B0  = -Dot(diff,seg.m);
05351     double B1  = Dot(diff,tri.e0);
05352     double B2  = Dot(diff,tri.e1);
05353     double cof00 = A11*A22-A12*A12;
05354     double cof01 = A02*A12-A01*A22;
05355     double cof02 = A01*A12-A02*A11;
05356     double det = A00*cof00+A01*cof01+A02*cof02;
05357
05358     Line3 triseg;
05359     Point3 pt;
05360     double min, min0, r0, s0, t0;
05361
05362     if ( Abs(det) >= par_tolerance )
05363     {
05364         double cof11 = A00*A22-A02*A02;
05365         double cof12 = A02*A01-A00*A12;
05366         double cof22 = A00*A11-A01*A01;
05367         double invDet = 1.0f/det;
05368         double rhs0 = -B0*invDet;
05369         double rhs1 = -B1*invDet;
05370         double rhs2 = -B2*invDet;
05371
05372         r = cof00*rhs0+cof01*rhs1+cof02*rhs2;
05373         s = cof01*rhs0+cof11*rhs1+cof12*rhs2;
05374         t = cof02*rhs0+cof12*rhs1+cof22*rhs2;
05375
05376         if ( r < 0 )
05377         {
05378             if ( s+t <= 1 )
05379             {
05380                 if ( s < 0 )
05381                 {
05382                     if ( t < 0 )  // region 4m
05383                     {
05384                         // min on face s=0 or t=0 or r=0
05385                         triseg.b = tri.b;
05386                         triseg.m = tri.e1;
05387                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05388                         s = 0;
05389                         triseg.b = tri.b;
05390                         triseg.m = tri.e0;
05391                         min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
05392                         t0 = 0;
05393                         if ( min0 < min )
05394                         {
05395                             min = min0;
05396                             r = r0;
05397                             s = s0;
05398                             t = t0;
05399                         }
05400                         min0 = MinPointTriangle(seg.b,tri,s0,t0);
05401                         r0 = 0;
05402                         if ( min0 < min )
05403                         {
05404                             min = min0;
05405                             r = r0;
05406                             s = s0;
05407                             t = t0;
05408                         }
05409                     }
05410                     else  // region 3m
05411                     {
05412                         // min on face s=0 or r=0
05413                         triseg.b = tri.b;
05414                         triseg.m = tri.e1;
05415                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05416                         s = 0;
05417                         min0 = MinPointTriangle(seg.b,tri,s0,t0);
05418                         r0 = 0;
05419                         if ( min0 < min )
05420                         {
05421                             min = min0;
05422                             r = r0;
05423                             s = s0;
05424                             t = t0;
05425                         }
05426                     }
05427                 }
05428                 else if ( t < 0 )  // region 5m
05429                 {
05430                     // min on face t=0 or r=0
05431                     triseg.b = tri.b;
05432                     triseg.m = tri.e0;
05433                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05434                     t = 0;
05435                     min0 = MinPointTriangle(seg.b,tri,s0,t0);
05436                     r0 = 0;
05437                     if ( min0 < min )
05438                     {
05439                         min = min0;
05440                         r = r0;
05441                         s = s0;
05442                         t = t0;
05443                     }
05444                 }
05445                 else  // region 0m
05446                 {
05447                     // min face on r=0
05448                     min = MinPointTriangle(seg.b,tri,s,t);
05449                     r = 0;
05450                 }
05451             }
05452             else
05453             {
05454                 if ( s < 0 )  // region 2m
05455                 {
05456                     // min on face s=0 or s+t=1 or r=0
05457                     triseg.b = tri.b;
05458                     triseg.m = tri.e1;
05459                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05460                     s = 0;
05461                     triseg.b = tri.b+tri.e0;
05462                     triseg.m = tri.e1-tri.e0;
05463                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05464                     s0 = 1-t0;
05465                     if ( min0 < min )
05466                     {
05467                         min = min0;
05468                         r = r0;
05469                         s = s0;
05470                         t = t0;
05471                     }
05472                     min0 = MinPointTriangle(seg.b,tri,s0,t0);
05473                     r0 = 0;
05474                     if ( min0 < min )
05475                     {
05476                         min = min0;
05477                         r = r0;
05478                         s = s0;
05479                         t = t0;
05480                     }
05481                 }
05482                 else if ( t < 0 )  // region 6m
05483                 {
05484                     // min on face t=0 or s+t=1 or r=0
05485                     triseg.b = tri.b;
05486                     triseg.m = tri.e0;
05487                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05488                     t = 0;
05489                     triseg.b = tri.b+tri.e0;
05490                     triseg.m = tri.e1-tri.e0;
05491                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05492                     s0 = 1-t0;
05493                     if ( min0 < min )
05494                     {
05495                         min = min0;
05496                         r = r0;
05497                         s = s0;
05498                         t = t0;
05499                     }
05500                     min0 = MinPointTriangle(seg.b,tri,s0,t0);
05501                     r0 = 0;
05502                     if ( min0 < min )
05503                     {
05504                         min = min0;
05505                         r = r0;
05506                         s = s0;
05507                         t = t0;
05508                     }
05509                 }
05510                 else  // region 1m
05511                 {
05512                     // min on face s+t=1 or r=0
05513                     triseg.b = tri.b+tri.e0;
05514                     triseg.m = tri.e1-tri.e0;
05515                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05516                     s = 1-t;
05517                     min0 = MinPointTriangle(seg.b,tri,s0,t0);
05518                     r0 = 0;
05519                     if ( min0 < min )
05520                     {
05521                         min = min0;
05522                         r = r0;
05523                         s = s0;
05524                         t = t0;
05525                     }
05526                 }
05527             }
05528         }
05529         else if ( r <= 1 )
05530         {
05531             if ( s+t <= 1 )
05532             {
05533                 if ( s < 0 )
05534                 {
05535                     if ( t < 0 )  // region 4
05536                     {
05537                         // min on face s=0 or t=0
05538                         triseg.b = tri.b;
05539                         triseg.m = tri.e1;
05540                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05541                         s = 0;
05542                         triseg.b = tri.b;
05543                         triseg.m = tri.e0;
05544                         min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
05545                         t0 = 0;
05546                         if ( min0 < min )
05547                         {
05548                             min = min0;
05549                             r = r0;
05550                             s = s0;
05551                             t = t0;
05552                         }
05553                     }
05554                     else  // region 3
05555                     {
05556                         // min on face s=0
05557                         triseg.b = tri.b;
05558                         triseg.m = tri.e1;
05559                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05560                         s = 0;
05561                     }
05562                 }
05563                 else if ( t < 0 )  // region 5
05564                 {
05565                     // min on face t=0
05566                     triseg.b = tri.b;
05567                     triseg.m = tri.e0;
05568                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05569                     t = 0;
05570                 }
05571                 else  // region 0
05572                 {
05573                     // global minimum is interior, done
05574                     min = Sqrt(Abs(r*(A00*r+A01*s+A02*t+2.0f*B0)
05575                           +s*(A01*r+A11*s+A12*t+2.0f*B1)
05576                           +t*(A02*r+A12*s+A22*t+2.0f*B2)
05577                           +Dot(diff,diff)));
05578                 }
05579             }
05580             else
05581             {
05582                 if ( s < 0 )  // region 2
05583                 {
05584                     // min on face s=0 or s+t=1
05585                     triseg.b = tri.b;
05586                     triseg.m = tri.e1;
05587                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05588                     s = 0;
05589                     triseg.b = tri.b+tri.e0;
05590                     triseg.m = tri.e1-tri.e0;
05591                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05592                     s0 = 1-t0;
05593                     if ( min0 < min )
05594                     {
05595                         min = min0;
05596                         r = r0;
05597                         s = s0;
05598                         t = t0;
05599                     }
05600                 }
05601                 else if ( t < 0 )  // region 6
05602                 {
05603                     // min on face t=0 or s+t=1
05604                     triseg.b = tri.b;
05605                     triseg.m = tri.e0;
05606                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05607                     t = 0;
05608                     triseg.b = tri.b+tri.e0;
05609                     triseg.m = tri.e1-tri.e0;
05610                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05611                     s0 = 1-t0;
05612                     if ( min0 < min )
05613                     {
05614                         min = min0;
05615                         r = r0;
05616                         s = s0;
05617                         t = t0;
05618                     }
05619                 }
05620                 else  // region 1
05621                 {
05622                     // min on face s+t=1
05623                     triseg.b = tri.b+tri.e0;
05624                     triseg.m = tri.e1-tri.e0;
05625                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05626                     s = 1-t;
05627                 }
05628             }
05629         }
05630         else  // r > 1
05631         {
05632             if ( s+t <= 1 )
05633             {
05634                 if ( s < 0 )
05635                 {
05636                     if ( t < 0 )  // region 4p
05637                     {
05638                         // min on face s=0 or t=0 or r=1
05639                         triseg.b = tri.b;
05640                         triseg.m = tri.e1;
05641                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05642                         s = 0;
05643                         triseg.b = tri.b;
05644                         triseg.m = tri.e0;
05645                         min0 = MinLineSegmentLineSegment(seg,triseg,r0,s0);
05646                         t0 = 0;
05647                         if ( min0 < min )
05648                         {
05649                             min = min0;
05650                             r = r0;
05651                             s = s0;
05652                             t = t0;
05653                         }
05654                         pt = seg.b+seg.m;
05655                         min0 = MinPointTriangle(pt,tri,s0,t0);
05656                         r0 = 1;
05657                         if ( min0 < min )
05658                         {
05659                             min = min0;
05660                             r = r0;
05661                             s = s0;
05662                             t = t0;
05663                         }
05664                     }
05665                     else  // region 3p
05666                     {
05667                         // min on face s=0 or r=1
05668                         triseg.b = tri.b;
05669                         triseg.m = tri.e1;
05670                         min = MinLineSegmentLineSegment(seg,triseg,r,t);
05671                         s = 0;
05672                         pt = seg.b+seg.m;
05673                         min0 = MinPointTriangle(pt,tri,s0,t0);
05674                         r0 = 1;
05675                         if ( min0 < min )
05676                         {
05677                             min = min0;
05678                             r = r0;
05679                             s = s0;
05680                             t = t0;
05681                         }
05682                     }
05683                 }
05684                 else if ( t < 0 )  // region 5p
05685                 {
05686                     // min on face t=0 or r=1
05687                     triseg.b = tri.b;
05688                     triseg.m = tri.e0;
05689                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05690                     t = 0;
05691                     pt = seg.b+seg.m;
05692                     min0 = MinPointTriangle(pt,tri,s0,t0);
05693                     r0 = 1;
05694                     if ( min0 < min )
05695                     {
05696                         min = min0;
05697                         r = r0;
05698                         s = s0;
05699                         t = t0;
05700                     }
05701                 }
05702                 else  // region 0p
05703                 {
05704                     // min face on r=1
05705                     pt = seg.b+seg.m;
05706                     min = MinPointTriangle(pt,tri,s,t);
05707                     r = 1;
05708                 }
05709             }
05710             else
05711             {
05712                 if ( s < 0 )  // region 2p
05713                 {
05714                     // min on face s=0 or s+t=1 or r=1
05715                     triseg.b = tri.b;
05716                     triseg.m = tri.e1;
05717                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05718                     s = 0;
05719                     triseg.b = tri.b+tri.e0;
05720                     triseg.m = tri.e1-tri.e0;
05721                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05722                     s0 = 1-t0;
05723                     if ( min0 < min )
05724                     {
05725                         min = min0;
05726                         r = r0;
05727                         s = s0;
05728                         t = t0;
05729                     }
05730                     pt = seg.b+seg.m;
05731                     min0 = MinPointTriangle(pt,tri,s0,t0);
05732                     r0 = 1;
05733                     if ( min0 < min )
05734                     {
05735                         min = min0;
05736                         r = r0;
05737                         s = s0;
05738                         t = t0;
05739                     }
05740                 }
05741                 else if ( t < 0 )  // region 6p
05742                 {
05743                     // min on face t=0 or s+t=1 or r=1
05744                     triseg.b = tri.b;
05745                     triseg.m = tri.e0;
05746                     min = MinLineSegmentLineSegment(seg,triseg,r,s);
05747                     t = 0;
05748                     triseg.b = tri.b+tri.e0;
05749                     triseg.m = tri.e1-tri.e0;
05750                     min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05751                     s0 = 1-t0;
05752                     if ( min0 < min )
05753                     {
05754                         min = min0;
05755                         r = r0;
05756                         s = s0;
05757                         t = t0;
05758                     }
05759                     pt = seg.b+seg.m;
05760                     min0 = MinPointTriangle(pt,tri,s0,t0);
05761                     r0 = 1;
05762                     if ( min0 < min )
05763                     {
05764                         min = min0;
05765                         r = r0;
05766                         s = s0;
05767                         t = t0;
05768                     }
05769                 }
05770                 else  // region 1p
05771                 {
05772                     // min on face s+t=1 or r=1
05773                     triseg.b = tri.b+tri.e0;
05774                     triseg.m = tri.e1-tri.e0;
05775                     min = MinLineSegmentLineSegment(seg,triseg,r,t);
05776                     s = 1-t;
05777                     pt = seg.b+seg.m;
05778                     min0 = MinPointTriangle(pt,tri,s0,t0);
05779                     r0 = 1;
05780                     if ( min0 < min )
05781                     {
05782                         min = min0;
05783                         r = r0;
05784                         s = s0;
05785                         t = t0;
05786                     }
05787                 }
05788             }
05789         }
05790     }
05791     else
05792     {
05793         // line and triangle are parallel
05794         triseg.b = tri.b;
05795         triseg.m = tri.e0;
05796         min = MinLineSegmentLineSegment(seg,triseg,r,s);
05797         t = 0;
05798
05799         triseg.m = tri.e1;
05800         min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05801         s0 = 0;
05802         if ( min0 < min )
05803         {
05804             min = min0;
05805             r = r0;
05806             s = s0;
05807             t = t0;
05808         }
05809
05810         triseg.b = triseg.b + tri.e0;
05811         triseg.m = triseg.m - tri.e0;
05812         min0 = MinLineSegmentLineSegment(seg,triseg,r0,t0);
05813         s0 = 1-t0;
05814         if ( min0 < min )
05815         {
05816             min = min0;
05817             r = r0;
05818             s = s0;
05819             t = t0;
05820         }
05821
05822         min0 = MinPointTriangle(seg.b,tri,s0,t0);
05823         r0 = 0;
05824         if ( min0 < min )
05825         {
05826             min = min0;
05827             r = r0;
05828             s = s0;
05829             t = t0;
05830         }
05831
05832         pt = seg.b+seg.m;
05833         min0 = MinPointTriangle(pt,tri,s0,t0);
05834         r0 = 1;
05835         if ( min0 < min )
05836         {
05837             min = min0;
05838             r = r0;
05839             s = s0;
05840             t = t0;
05841         }
05842     }
05843
05844     return min;
05845 }
05846 //---------------------------------------------------------------------------
05847
05848 #ifdef LIN3TRI3_TEST
05849
05850 //#define RAND (2.0f*rand()/float(RAND_MAX)-1)
05851
05852 //ofstream ostr("data.txt");
05853
05854 void main ()
05855 {
05856     Triangle3 tri;
05857     Line3 line;
05858     Point3 p, q, diff;
05859     double r, s, t, r0, s0, t0, min0, min1, dist;
05860
05861     double maxdiff = 0;
05862
05863     for (int i = 0; i < 128; i++)
05864     {
05865         tri.b.x = RAND;
05866         tri.b.y = RAND;
05867         tri.b.z = RAND;
05868         tri.e0.x = RAND;
05869         tri.e0.y = RAND;
05870         tri.e0.z = RAND;
05871         tri.e1.x = RAND;
05872         tri.e1.y = RAND;
05873         tri.e1.z = RAND;
05874
05875         line.b.x = RAND;
05876         line.b.y = RAND;
05877         line.b.z = RAND;
05878         if ( i % 2 )
05879         {
05880             // non-parallel line and triangle
05881             line.m.x = RAND;
05882             line.m.y = RAND;
05883             line.m.z = RAND;
05884         }
05885         else
05886         {
05887             // line is parallel to triangle
05888             double c0 = RAND;
05889             double c1 = RAND;
05890             line.m = c0*tri.e0+c1*tri.e1;
05891         }
05892
05893         min0 = FLT_MAX;
05894         int max = 32;
05895         for (int z = 0; z <= max; z++)
05896         {
05897             r0 = z/double(max);
05898             p = line.b+r0*line.m;
05899             for (int y = 0; y <= max; y++)
05900             {
05901                 s0 = y/double(max);
05902                 for (int x = 0; x+y <= max; x++)
05903                 {
05904                     t0 = x/double(max);
05905                     q = tri.b+s0*tri.e0+t0*tri.e1;
05906
05907                     diff = p-q;
05908                     dist = Length(diff);
05909                     if ( dist < min0 )
05910                     {
05911                         min0 = dist;
05912                         r = r0;
05913                         s = s0;
05914                         t = t0;
05915                     }
05916                 }
05917             }
05918         }
05919         ostr << "i = " << i << endl;
05920         ostr << "sampled = " << r << ' ' << s << ' ' << t << ' '
05921              << min0 << endl;
05922
05923         min1 = MinLineSegmentTriangle(line,tri,r,s,t);
05924         ostr << "analytic = " << r << ' ' << s << ' ' << t << ' '
05925              << min1 << endl;
05926
05927         ostr << "diff = " << min1-min0 << endl;
05928
05929         if ( min1-min0 > maxdiff )
05930             maxdiff = min1-min0;
05931
05932         ostr << endl;
05933     }
05934
05935     ostr << "max diff = " << maxdiff << endl;
05936 }
05937 #endif
05938
05939 //FILE: triasect.cpp
05940 //---------------------------------------------------------------------------
05941 AgtLine
05942 AnalyticGeometryTool::EdgeToLine (Point2* v0, Point2* v1)
05943 {
05944     // assert:  v0 and v1 are distinct
05945
05946     Point2 edge = { v1->x - v0->x, v1->y - v0->y };
05947
05948     AgtLine line;
05949     line.N.x = edge.y;
05950     line.N.y = -edge.x;
05951     line.c = line.N.x*v0->x + line.N.y*v0->y;
05952
05953     return line;
05954 }
05955 //---------------------------------------------------------------------------
05956 void
05957 AnalyticGeometryTool::TriangleLines (Triangle* T, AgtLine line[3])
05958 {
05959     line[0] = EdgeToLine(&T->v[0],&T->v[1]);
05960     line[1] = EdgeToLine(&T->v[0],&T->v[2]);
05961     line[2] = EdgeToLine(&T->v[1],&T->v[2]);
05962
05963     // make sure normals point outwards
05964     Point2 avr = { 0.0, 0.0 };
05965     int i;
05966     for (i = 0; i < 3; i++)
05967     {
05968         avr.x += T->v[i].x;
05969         avr.y += T->v[i].y;
05970     }
05971     static double oneThird = 1.0/3.0;
05972     avr.x *= oneThird;
05973     avr.y *= oneThird;
05974
05975     for (i = 0; i < 3; i++)
05976     {
05977         double dot = line[i].N.x*avr.x+line[i].N.y*avr.y-line[i].c;
05978         if ( dot > 0.0 )
05979         {
05980             line[i].N.x = -line[i].N.x;
05981             line[i].N.y = -line[i].N.y;
05982             line[i].c = -line[i].c;
05983         }
05984     }
05985 }
05986 //---------------------------------------------------------------------------
05987 AgtTriList*
05988 AnalyticGeometryTool::SplitAndDecompose (Triangle* T, AgtLine* line)
05989 {
05990     double c[3];
05991     int positive = 0, ip[3];
05992     int negative = 0, in[3];
05993     int i;
05994     for (i = 0; i < 3; i++)
05995     {
05996         c[i] = line->N.x*T->v[i].x + line->N.y*T->v[i].y - line->c;
05997         if ( c[i] > 0.0 )
05998         {
05999             ip[positive++] = i;
06000         }
06001         else if ( c[i] < 0.0 )
06002         {
06003             in[negative++] = i;
06004         }
06005     }
06006
06007     // For a split to occur, one of the c_i must be positive and one must
06008     // be negative.
06009     AgtTriList* list = NULL;
06010
06011     if ( negative == 0 ) // T is completely on the positive side of line
06012         return 0;
06013
06014     if ( positive == 0 )
06015     {
06016         // T is completely on the negative side of line
06017         list = new AgtTriList;
06018         list->tri = T;
06019         list->next = 0;
06020         return list;
06021     }
06022
06023     // T is split by line.  Determine how it is split and how to decompose
06024     // the negative-side portion into triangles (3 cases).
06025     double w0, w1, cdiff;
06026     Point2 intrp[2];
06027
06028     if ( positive == 2 )
06029     {
06030         // ++-
06031         for (i = 0; i < 2 /* = positive */; i++)
06032         {
06033             cdiff = c[ip[i]]-c[in[0]];
06034             w0 = -c[in[0]]/cdiff;
06035             w1 = c[ip[i]]/cdiff;
06036             T->v[ip[i]].x = w0*T->v[ip[i]].x+w1*T->v[in[0]].x;
06037             T->v[ip[i]].y = w0*T->v[ip[i]].y+w1*T->v[in[0]].y;
06038         }
06039         list = new AgtTriList;
06040         list->tri = T;
06041         list->next = 0;
06042     }
06043     else if ( positive == 1 )
06044     {
06045         if ( negative == 2 )
06046         {
06047             // +--
06048             for (i = 0; i < 2 /* = negative */; i++)
06049             {
06050                 cdiff = c[ip[0]]-c[in[i]];
06051                 w0 = -c[in[i]]/cdiff;
06052                 w1 = c[ip[0]]/cdiff;
06053                 intrp[i].x = w0*T->v[ip[0]].x+w1*T->v[in[i]].x;
06054                 intrp[i].y = w0*T->v[ip[0]].y+w1*T->v[in[i]].y;
06055             }
06056
06057             T->v[ip[0]] = intrp[0];
06058             list = new AgtTriList;
06059             list->tri = T;
06060
06061             Triangle* T1 = new Triangle;
06062             T1->v[0] = intrp[0];
06063             T1->v[1] = T->v[in[1]];
06064             T1->v[2] = intrp[1];
06065             list->next = new AgtTriList;
06066             list->next->tri = T1;
06067             list->next->next = 0;
06068         }
06069         else
06070         {
06071             // +-0
06072             cdiff = c[ip[0]]-c[in[0]];
06073             w0 = -c[in[0]]/cdiff;
06074             w1 = c[ip[0]]/cdiff;
06075             T->v[ip[0]].x = w0*T->v[ip[0]].x+w1*T->v[in[0]].x;
06076             T->v[ip[0]].y = w0*T->v[ip[0]].y+w1*T->v[in[0]].y;
06077             list = new AgtTriList;
06078             list->tri = T;
06079             list->next = 0;
06080         }
06081     }
06082
06083     return list;
06084 }
06085 //---------------------------------------------------------------------------
06086 AgtTriList*
06087 AnalyticGeometryTool::Intersection (Triangle* T0, Triangle* T1)
06088 {
06089     // build edges of T0
06090     AgtLine line[3];
06091     TriangleLines(T0,line);
06092
06093     // initial list is copy of T1 (since triangle may be deleted)
06094     AgtTriList* list = new AgtTriList;
06095     list->tri = new Triangle;
06096     memcpy(list->tri,T1,sizeof(Triangle));
06097     list->next = 0;
06098
06099     // process subtriangles of T1 against lines of T0
06100     for (int i = 0; i < 3; i++)
06101     {
06102         AgtTriList* save = 0;
06103         while ( list )
06104         {
06105             // get head of list
06106             Triangle* T = list->tri;
06107
06110             {
06111                 // search for end of list
06113                 while ( end->next )
06114                     end = end->next;
06115
06116                 // attach decomposition to front of save-list
06117                 end->next = save;
06119             }
06120
06121             // remove head of list
06122             AgtTriList* tmp = list;
06123             list = list->next;
06124
06125             if( sad == NULL )
06126               delete tmp->tri;
06127             delete tmp;
06128         }
06129         list = save;
06130     }
06131
06132     return list;
06133 }
06134 //---------------------------------------------------------------------------
06135 double
06136 AnalyticGeometryTool::AreaTriangle (Triangle* T)
06137 {
06138     Point2 e1 = { T->v[1].x - T->v[0].x, T->v[1].y - T->v[0].y };
06139     Point2 e2 = { T->v[2].x - T->v[0].x, T->v[2].y - T->v[0].y };
06140
06141     return fabs(0.5*(e1.x*e2.y-e1.y*e2.x));
06142 }
06143 //---------------------------------------------------------------------------
06144 double
06145 AnalyticGeometryTool::Area (AgtTriList* list)
06146 {
06147     double area = 0.0;
06148
06149     while ( list )
06150     {
06151         area += AreaTriangle(list->tri);
06152         list = list->next;
06153     }
06154
06155     return area;
06156 }
06157 //---------------------------------------------------------------------------
06158
06159 double
06160 AnalyticGeometryTool::AreaIntersection( Triangle &tri1, Triangle &tri2  )
06161 {
06162   AgtTriList* list = Intersection(&tri1,&tri2);
06163   double area = Area(list);
06164   while ( list )
06165   {
06166     AgtTriList* save = list;
06167     list = list->next;
06168     delete save->tri;
06169     delete save;
06170   }
06171   delete list;
06172   return area;
06173 }
06174
06175 #ifdef TRIASECT_TEST
06176
06177 void main ()
06178 {
06179     Triangle T0;
06180     T0.v[0].x = 0.0;  T0.v[0].y = 0.0;
06181     T0.v[1].x = 1.0;  T0.v[1].y = 0.0;
06182     T0.v[2].x = 0.0;  T0.v[2].y = 1.0;
06183
06184     Triangle T1;
06185     const double eps = 0.001;
06186     T1.v[0].x = 0.5+eps;  T1.v[0].y = 0.5+eps;
06187     T1.v[1].x = -eps;  T1.v[1].y = 0.5;
06188     T1.v[2].x = 0.5;  T1.v[2].y = -eps;
06189
06190     AgtTriList* list = Intersection(&T0,&T1);
06191     double area = Area(list);
06192
06193     while ( list )
06194     {
06195         AgtTriList* save = list;
06196         list = list->next;
06197         delete save->tri;
06198         delete save;
06199     }
06200 }
06201 #endif
06202
06203 double
06204 AnalyticGeometryTool::Angle( Triangle3& tri1, Triangle3& tri2 )
06205 {
06206   double norm1[3];
06207   Normal( tri1, norm1 );
06208
06209   double norm2[3];
06210   Normal( tri2, norm2 );
06211
06212   return angle_vec_vec( norm1, norm2 );
06213 }
06214
06215
06216 void
06217 AnalyticGeometryTool::Normal( Triangle3& tri, double normal[3] )
06218 {
06219   double vec1[3];
06220   vec1[0]=tri.e0.x; vec1[1]=tri.e0.y; vec1[2]=tri.e0.z;
06221
06222   double vec2[3];
06223   vec2[0]=tri.e1.x; vec2[1]=tri.e1.y; vec2[2]=tri.e1.z;
06224
06225   cross_vec_unit( vec1, vec2, normal );
06226 }
06227
06228 double
06229 AnalyticGeometryTool::ProjectedOverlap( Triangle3& tri1, Triangle3& tri2, bool draw_overlap )
06230 {
06231   // Transform both triangles into local coordinate system of tri1
06232   // to eliminate z-coordinate.
06233
06234   // Use normal as z-axis
06235   double z[3];
06236   Normal( tri1, z );
06237
06238   // x-axis goes from b to e0
06239   double x[3];
06240   x[0] = tri1.e0.x;
06241   x[1] = tri1.e0.y;
06242   x[2] = tri1.e0.z;
06243
06244   // Cross z with x to get y-axis
06245   double y[3];
06246   cross_vec( z, x, y );
06247
06248   // Unitize them all
06249   unit_vec( x, x );
06250   unit_vec( y, y );
06251   unit_vec( z, z );
06252
06253   double origin[3];
06254   origin[0] = tri1.b.x;
06255   origin[1] = tri1.b.y;
06256   origin[2] = tri1.b.z;
06257
06258   double mtxTriOne2Global[4][4];
06259   vecs_to_mtx( x, y, z, origin, mtxTriOne2Global ); // Really mtxTriOne2Global
06260   double mtxGlobal2TriOne[4][4];
06261   inv_trans_mtx( mtxTriOne2Global, mtxGlobal2TriOne );
06262
06263   double v0[3], v1[3], v2[3]; // Vertices of triangle
06264   v0[0]=tri1.b.x;   v0[1]=tri1.b.y;  v0[2]=tri1.b.z;
06265   v1[0]=tri1.e0.x+tri1.b.x; v1[1]=tri1.e0.y+tri1.b.y; v1[2]=tri1.e0.z+tri1.b.z;
06266   v2[0]=tri1.e1.x+tri1.b.x; v2[1]=tri1.e1.y+tri1.b.y; v2[2]=tri1.e1.z+tri1.b.z;
06267   transform_pnt( mtxGlobal2TriOne, v0, v0 );
06268   transform_pnt( mtxGlobal2TriOne, v1, v1 );
06269   transform_pnt( mtxGlobal2TriOne, v2, v2 );
06270
06271   // Load this into a 2D triangle T1 ( z-axis is now zero )
06272   Triangle T1;
06273   T1.v[0].x = v0[0]; T1.v[0].y = v0[1];
06274   T1.v[1].x = v1[0]; T1.v[1].y = v1[1];
06275   T1.v[2].x = v2[0]; T1.v[2].y = v2[1];
06276
06277   // Setup the next triangle, in this coordinate system
06278   v0[0]=tri2.b.x;   v0[1]=tri2.b.y;  v0[2]=tri2.b.z;
06279   v1[0]=tri2.e0.x+tri2.b.x; v1[1]=tri2.e0.y+tri2.b.y; v1[2]=tri2.e0.z+tri2.b.z;
06280   v2[0]=tri2.e1.x+tri2.b.x; v2[1]=tri2.e1.y+tri2.b.y; v2[2]=tri2.e1.z+tri2.b.z;
06281
06282   // The following creates coordinates of tri2 in tri1's csys
06283   transform_pnt( mtxGlobal2TriOne, v0, v0 );
06284   transform_pnt( mtxGlobal2TriOne, v1, v1 );
06285   transform_pnt( mtxGlobal2TriOne, v2, v2 );
06286
06287   // Now we can project tri2 to tri1 simply by dropping the z-coordinate
06288   Triangle T2;
06289   T2.v[0].x = v0[0]; T2.v[0].y = v0[1];
06290   T2.v[1].x = v1[0]; T2.v[1].y = v1[1];
06291   T2.v[2].x = v2[0]; T2.v[2].y = v2[1];
06292
06293   // Now find area of overlap
06294   if( draw_overlap )
06295   {
06296     AgtTriList* list = Intersection(&T1, &T2);
06297     double overlap_area = Area(list);
06298     while ( list )
06299     {
06300       Triangle *tmp_tri = list->tri;
06301
06302       v0[0]=tmp_tri->v[0].x;
06303       v0[1]=tmp_tri->v[0].y;
06304       v0[2]=0;
06305
06306       v1[0]=tmp_tri->v[1].x;
06307       v1[1]=tmp_tri->v[1].y;
06308       v1[2]=0;
06309
06310       v2[0]=tmp_tri->v[2].x;
06311       v2[1]=tmp_tri->v[2].y;
06312       v2[2]=0;
06313
06314       transform_pnt( mtxTriOne2Global, v0, v0 );
06315       transform_pnt( mtxTriOne2Global, v1, v1 );
06316       transform_pnt( mtxTriOne2Global, v2, v2 );
06317
06318       //Now Draw the thing
06319        GMem g_mem;
06320        g_mem.allocate_tri(1);
06321        g_mem.pointListCount = 3;
06322
06323        g_mem.point_list()[0].x = v0[0];
06324        g_mem.point_list()[0].y = v0[1];
06325        g_mem.point_list()[0].z = v0[2];
06326
06327        g_mem.point_list()[1].x = v1[0];
06328        g_mem.point_list()[1].y = v1[1];
06329        g_mem.point_list()[1].z = v1[2];
06330
06331        g_mem.point_list()[2].x = v2[0];
06332        g_mem.point_list()[2].y = v2[1];
06333        g_mem.point_list()[2].z = v2[2];
06334
06335        g_mem.facet_list()[0] = 3;
06336        g_mem.facet_list()[1] = 0;
06337        g_mem.facet_list()[2] = 1;
06338        g_mem.facet_list()[3] = 2;
06339
06340        g_mem.fListCount = 1;
06341        GfxPreview::draw_polygon(g_mem.point_list(),3,4,4,true);
06342
06343       AgtTriList* save = list;
06344       list = list->next;
06345       delete save->tri;
06346       delete save;
06347     }
06348     delete list;
06349     return overlap_area;
06350   }
06351   else
06352     return AreaIntersection( T1, T2 );
06353 }
```