opennurbs_math.cpp
Go to the documentation of this file.
00001 /* $NoKeywords: $ */
00002 /*
00003 //
00004 // Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
00005 // OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
00006 // McNeel & Associates.
00007 //
00008 // THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
00009 // ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
00010 // MERCHANTABILITY ARE HEREBY DISCLAIMED.
00011 //                              
00012 // For complete openNURBS copyright information see <http://www.opennurbs.org>.
00013 //
00015 */
00016 
00017 #include "pcl/surface/3rdparty/opennurbs/opennurbs.h"
00018 
00019 /*
00020 Description:
00021   Test math library functions.
00022 Parameters:
00023   function_index - [in]  Determines which math library function is called.
00024 
00025            1:    z = x+y
00026            2:    z = x-y
00027            3:    z = x*y
00028            4:    z = x/y
00029            5:    z = fabs(x)
00030            6:    z = exp(x)
00031            7:    z = log(x)
00032            8:    z = log10(x)
00033            9:    z = frexp(x)
00034           10:    z = pow(x,y)
00035           11:    z = sqrt(x)
00036           12:    z = sin(x)
00037           13:    z = cos(x)
00038           14:    z = tan(x)
00039           15:    z = sinh(x)
00040           16:    z = cosh(x)
00041           17:    z = tanh(x)
00042           18:    z = asin(x)
00043           19:    z = acos(x)
00044           20:    z = atan(x)
00045           21:    z = atan2(y,x)
00046           22:    z = fmod(x,y)
00047           23:    z = modf(x,&y)
00048 
00049   double x - [in]
00050   double y - [in]
00051 Returns:
00052   Returns the "z" value listed in the function_index parameter
00053   description.
00054 Remarks:
00055   This function is used to test the results of class floating
00056   point functions.  It is primarily used to see what happens
00057   when opennurbs is used as a DLL and illegal operations are
00058   performed.
00059 */
00060 
00061 double ON_TestMathFunction( int function_index, double x, double y )
00062 {
00063   // This function is used to test the results of performing operations.
00064   //
00065   // module          function
00066   // opennurbs.dll   ON_TestMathFunction
00067   // tl.dll          TL_TestMathFunction
00068   // rhino.exe       Rhino_TestMathFunction
00069 
00070   double z = ON_UNSET_VALUE;
00071   int i;
00072 
00073   switch(function_index)
00074   {
00075   case 1: // addition
00076     z = x+y;
00077     break;
00078   case 2: // subtraction
00079     z = x-y;
00080     break;
00081   case 3: // multiplication
00082     z = x*y;
00083     break;
00084   case 4: // division
00085     z = x/y;
00086     break;
00087   case 5: // absolute value
00088     z = fabs(x);
00089     break;
00090   case 6: // exp
00091     z = exp(x);
00092     break;
00093   case 7: // log
00094     z = log(x);
00095     break;
00096   case 8: // log10
00097     z = log10(x);
00098     break;
00099   case 9: // frexp
00100     z = frexp(x,&i);
00101     break;
00102   case 10: // pow
00103     z = pow(x,y);
00104     break;
00105   case 11: // square root
00106     z = sqrt(x);
00107     break;
00108   case 12: // sine
00109     z = sin(x);
00110     break;
00111   case 13: // cosine
00112     z = cos(x);
00113     break;
00114   case 14: // tangent
00115     z = tan(x);
00116     break;
00117   case 15: // hyperbolic sine
00118     z = sinh(x);
00119     break;
00120   case 16: // hyperbolic cosine
00121     z = cosh(x);
00122     break;
00123   case 17: // hyperbolic tangent
00124     z = tanh(x);
00125     break;
00126   case 18: // arcsine
00127     z = asin(x);
00128     break;
00129   case 19: // arccosine
00130     z = acos(x);
00131     break;
00132   case 20: // arctangent
00133     z = atan(x);
00134     break;
00135   case 21: // arctangent
00136     z = atan2(y,x);
00137     break;
00138   case 22:
00139     z = fmod(x,y);
00140       break;
00141   case 23:
00142     z = modf(x,&y);
00143       break;
00144   default:
00145     z = 0.0;
00146     break;
00147   }
00148 
00149   return z;
00150 }
00151 
00152 double ON_ArrayDotProduct(int dim, const double* A, const double* B)
00153 {
00154   double AoB;
00155   // do low dimensional cases on one line so we get 80 bit
00156   // intermediate precision in optimized mode.
00157   if (dim==1) return (A[0]*B[0]);
00158   if (dim==2) return (A[0]*B[0] + A[1]*B[1]);
00159   if (dim==3) return (A[0]*B[0] + A[1]*B[1] + A[2]*B[2]);
00160   if (dim==4) return (A[0]*B[0] + A[1]*B[1] + A[2]*B[2] +A[3]*B[3]);
00161   AoB = 0.0;
00162   while (dim--) AoB += *A++ * *B++;
00163   return AoB;
00164 }
00165   
00166 
00167 double 
00168 ON_ArrayDotDifference( int dim, const double* A, const double* B, const double* C )
00169 {
00170   // returns A o ( B - C )
00171   double AoBminusC; // low dim cases inline for better optimization
00172   if (dim==1) return (A[0]*(B[0] - C[0]));
00173   if (dim==2) return (A[0]*(B[0] - C[0]) + A[1]*(B[1] - C[1]));
00174   if (dim==3) return (A[0]*(B[0] - C[0]) + A[1]*(B[1] - C[1]) + A[2]*(B[2] - C[2]));
00175   AoBminusC = 0.0;
00176   while (dim--) AoBminusC += *A++ * (*B++ - *C++);
00177   return AoBminusC;
00178 }
00179 
00180 
00181 double ON_ArrayDistance(int dim, const double *A, const double *B)
00182 {
00183   // returns sqrt((A-B)o(A-B))
00184   double a, b, c, len;
00185   switch(dim) {
00186   case 1:
00187     len = fabs(*B - *A); 
00188     break;
00189   case 2:
00190     a = fabs(*B++ - *A++); b = fabs(*B - *A);
00191     if (a > b) 
00192       {b /= a; len = a*sqrt(1.0+b*b);}
00193     else if (b > a) 
00194       {a /= b; len = b*sqrt(1.0+a*a);}
00195     else
00196       len = a*ON_SQRT2;
00197     break;
00198   case 3:
00199     a = fabs(*B++ - *A++); b = fabs(*B++ - *A++); c = fabs(*B - *A);
00200     if (a >= b) {
00201       if (a >= c) {
00202         if      (a == 0.0)         len = 0.0;
00203         else if (a == b && a == c) len = a*ON_SQRT3;
00204         else      {b /= a; c /= a; len = a*sqrt(1.0 + (b*b + c*c));}
00205       }
00206       else 
00207         {a /= c; b /= c; len = c*sqrt(1.0 + (a*a + b*b));}
00208     }
00209     else if (b >= c) 
00210       {a /= b; c /= b; len = b*sqrt(1.0 + (a*a + c*c));}
00211     else 
00212       {b /= c; a /= c; len = c*sqrt(1.0 + (a*a + b*b));}
00213     break;
00214   default:
00215     len = 0.0;
00216     while (dim--) {a = *B++ - *A++; len += a*a;}
00217     len = sqrt(len);
00218     break;
00219   }
00220   return len;
00221 }
00222 
00223 
00224 double ON_ArrayDistanceSquared(int dim, const double* A, const double* B)
00225 {
00226   // returns (A-B)o(A-B)
00227   double x, dist_sq = 0.0;
00228   while (dim--) {
00229     x = (*B++) - (*A++); 
00230     dist_sq += x*x;
00231   }
00232   return dist_sq;
00233 }
00234 
00235 
00236 double ON_ArrayMagnitude(int dim, const double* A)
00237 {
00238   double a, b, c, len;
00239   switch(dim) {
00240   case 1:
00241     len = fabs(*A); 
00242     break;
00243   case 2:
00244     a = fabs(*A++); b = fabs(*A);
00245     if (a > b) 
00246       {b /= a; len = a*sqrt(1.0+b*b);}
00247     else if (b > a) 
00248       {a /= b; len = b*sqrt(1.0+a*a);}
00249     else
00250       len = a*ON_SQRT2;
00251     break;
00252   case 3:
00253     a = fabs(*A++); b = fabs(*A++); c = fabs(*A);
00254     if (a >= b) {
00255       if (a >= c) {
00256         if (a == b && a == c) 
00257           len = a*ON_SQRT3;
00258         else
00259           {b /= a; c /= a; len = a*sqrt(1.0 + (b*b + c*c));}
00260       }
00261       else 
00262         {a /= c; b /= c; len = c*sqrt(1.0 + (a*a + b*b));}
00263     }
00264     else if (b >= c) 
00265       {a /= b; c /= b; len = b*sqrt(1.0 + (a*a + c*c));}
00266     else 
00267       {b /= c; a /= c; len = c*sqrt(1.0 + (a*a + b*b));}
00268     break;
00269   default:
00270     len = 0.0;
00271     while (dim--) {a = *A++; len += a*a;}
00272     len = sqrt(len);
00273     break;
00274   }
00275   return len;
00276 }
00277 
00278 
00279 double ON_ArrayMagnitudeSquared(int dim, const double* A)
00280 {
00281   double x, len_sq=0.0;
00282   while (dim--) {
00283     x = *A++;
00284     len_sq += x*x;
00285   }
00286   return len_sq;
00287 }
00288 
00289 
00290 
00291 void 
00292 ON_ArrayScale( int dim, double s, const double* A, double* sA )
00293 {
00294   if ( dim > 0 ) {
00295     while ( dim-- )
00296       *sA++ = s * *A++;
00297   }
00298 }
00299 
00300 
00301 void
00302 ON_Array_aA_plus_B( int dim, double a, const double* A, const double* B, double* aA_plus_B )
00303 {
00304   if ( dim > 0 ) {
00305     while ( dim-- )
00306       *aA_plus_B++ = a * *A++ + *B++;
00307   }
00308 }
00309 
00310 
00311 float 
00312 ON_ArrayDotProduct( int dim, const float* A, const float* B )
00313 {
00314   float d = 0.0;
00315   if ( dim > 0 ) {
00316     while(dim--)
00317       d += *A++ * *B++;
00318   }
00319   return d;
00320 }
00321 
00322 
00323 void
00324 ON_ArrayScale( int dim, float s, const float* A, float* sA )
00325 {
00326   if ( dim > 0 ) {
00327     while ( dim-- )
00328       *sA++ = s * *A++;
00329   }
00330 }
00331 
00332 
00333 void
00334 ON_Array_aA_plus_B( int dim, float a, const float* A, const float* B, float* aA_plus_B )
00335 {
00336   if ( dim > 0 ) {
00337     while ( dim-- )
00338       *aA_plus_B++ = a * *A++ + *B++;
00339   }
00340 }
00341 
00342 
00343 int 
00344 ON_DecomposeVector(
00345         const ON_3dVector& V,
00346         const ON_3dVector& A,
00347         const ON_3dVector& B,
00348         double* x, double* y
00349         )
00350 {
00351   int rank;
00352   double pr;
00353   const double AoV = A*V;
00354   const double BoV = B*V;
00355   const double AoA = A*A;
00356   const double AoB = A*B;
00357   const double BoB = B*B;
00358   rank =  ON_Solve2x2(  AoA, AoB, AoB, BoB, AoV, BoV, x, y, &pr );
00359   return (rank==2)?true:false;
00360 }
00361 
00362 
00363 ON_BOOL32 
00364 ON_EvJacobian( double ds_o_ds, double ds_o_dt, double dt_o_dt,
00365                     double* det_addr )
00366 /* Carefully compute the Jacobian determinant
00367  *
00368  * INPUT:
00369  *   ds_o_ds, ds_o_dt, dt_o_dt
00370  *      Dot products of the first partial derivatives
00371  *   det_addr
00372  *      address of an unused double
00373  * OUTPUT:
00374  *   *det_addr = ds_o_ds*dt_o_dt - ds_o_dt^2
00375  *   ON_EvJacobian()
00376  *       0: successful
00377  *      -1: failure
00378  *
00379  * COMMENTS:
00380  *      ...
00381  *
00382  * EXAMPLE:
00383  *      // ...
00384  *
00385  * REFERENCE:
00386  *      
00387  *
00388  * RELATED FUNCTIONS:
00389  *      ON_EvBsplineBasis(), ON_EvdeCasteljau(), ON_EvBezier()
00390  */
00391 {
00392   ON_BOOL32 rc = false;
00393   double det, a, b;
00394   a = ds_o_ds*dt_o_dt;
00395   b = ds_o_dt*ds_o_dt;
00396   /* NOTE: a = |Du|^2 * |Dv|^2  and b = (Du o Dv)^2 are always >= 0 */
00397   det = a - b;
00398   if (ds_o_ds <= dt_o_dt* ON_EPSILON || dt_o_dt <= ds_o_ds* ON_EPSILON) {
00399     /* one of the paritals is (numerically) zero with respect
00400      * to the other partial - value of det is unreliable
00401      */
00402     rc = false;
00403   }
00404   else if (fabs(det) <= ((a > b) ? a : b)* ON_SQRT_EPSILON) {
00405     /* Du and Dv are (numerically) (anti) parallel - 
00406      * value of det is unreliable.
00407      */
00408     rc = false;
00409   }
00410   else {
00411     rc = true;
00412   }
00413   if (det_addr) *det_addr = det;
00414   return rc;
00415 }
00416 
00417 
00418 ON_BOOL32 
00419 ON_EvNormalPartials(
00420         const ON_3dVector& ds,
00421         const ON_3dVector& dt,
00422         const ON_3dVector& dss,
00423         const ON_3dVector& dst,
00424         const ON_3dVector& dtt,
00425         ON_3dVector& ns,
00426         ON_3dVector& nt
00427         )
00428 {
00429   ON_BOOL32 rc = false;
00430   const double ds_o_ds = ds*ds;
00431   const double ds_o_dt = ds*dt;
00432   const double dt_o_dt = dt*dt;
00433 
00434   rc = ON_EvJacobian( ds_o_ds, ds_o_dt, dt_o_dt, NULL );
00435   if (!rc) 
00436   {
00437     // degenerate Jacobian and unit surface normal is not well defined
00438     ns.Zero();
00439     nt.Zero();
00440   }
00441   else 
00442   {
00443     // If V: . -> R^3 is nonzero and C^2 and N = V/|V|, then 
00444     //
00445     //          V'       V o V'
00446     //   N' = -----  -  ------- * V.
00447     //         |V|       |V|^3
00448     //
00449     // When a surface has a non-degenerate Jacobian, V = ds X dt
00450     // and the derivatives of N may be computed from the first
00451     // and second partials.
00452     ON_3dVector V = ON_CrossProduct(ds,dt);
00453     double len = V.Length();
00454     double len3 = len*len*len;
00455     if (len < ON_EPSILON) {
00456       ns.Zero();
00457       nt.Zero();
00458       return false;
00459     }
00460 
00461     ns.x = dss.y*dt.z - dss.z*dt.y + ds.y*dst.z - ds.z*dst.y;
00462     ns.y = dss.z*dt.x - dss.x*dt.z + ds.z*dst.x - ds.x*dst.z;
00463     ns.z = dss.x*dt.y - dss.y*dt.x + ds.x*dst.y - ds.y*dst.x;
00464 
00465     nt.x = dst.y*dt.z - dst.z*dt.y + ds.y*dtt.z - ds.z*dtt.y;
00466     nt.y = dst.z*dt.x - dst.x*dt.z + ds.z*dtt.x - ds.x*dtt.z;
00467     nt.z = dst.x*dt.y - dst.y*dt.x + ds.x*dtt.y - ds.y*dtt.x;
00468 
00469     ns = ns/len - ((V*ns)/len3)*V;
00470     nt = nt/len - ((V*nt)/len3)*V;
00471   }
00472 
00473   return rc;
00474 }
00475 
00476 
00477 ON_BOOL32 
00478 ON_Pullback3dVector( // use to pull 3d vector back to surface parameter space
00479       const ON_3dVector& vector,   // 3d vector
00480       double distance,              // signed distance from vector location to closet point on surface
00481                                     // < 0 if point is below with respect to Du x Dv
00482       const ON_3dVector&  ds,      // surface first partials
00483       const ON_3dVector&  dt,
00484       const ON_3dVector&  dss,     // surface 2nd partials
00485       const ON_3dVector&  dst,     // (used only when dist != 0)
00486       const ON_3dVector&  dtt, 
00487       ON_2dVector&  pullback       // pullback
00488       )
00489 {
00490   ON_BOOL32 rc = false;
00491   //int bIsDegenerate = false;
00492   if (distance != 0.0) {
00493     ON_3dVector ns, nt;
00494     rc = ON_EvNormalPartials(ds,dt,dss,dst,dtt,ns,nt);
00495     if ( rc ) {
00496       // adjust ds and dt to take account of offset distance
00497       rc = ON_DecomposeVector( vector, ds + distance*ns, dt + distance*nt, &pullback.x, &pullback.y );
00498     }
00499   }
00500   else {
00501     rc = ON_DecomposeVector( vector, ds, dt, &pullback.x, &pullback.y );
00502   }
00503   return rc;
00504 }
00505 
00506 
00507 ON_BOOL32 
00508 ON_GetParameterTolerance(
00509         double t0, double t1, // domain
00510         double t,          // parameter in domain
00511         double* tminus, double* tplus// parameter tolerance (tminus, tplus) returned here
00512         )
00513 {
00514   const ON_BOOL32 rc = (t0 < t1) ? true : false;
00515   if ( rc ) {
00516     if ( t < t0 )
00517       t = t0;
00518     else if (t > t1 )
00519       t = t1;
00520     double dt = (t1-t0)*8.0* ON_SQRT_EPSILON + (fabs(t0) + fabs(t1))* ON_EPSILON;
00521     if ( dt >= t1-t0 )
00522       dt = 0.5*(t1-t0);
00523     const double tmin = t-dt;
00524     const double tmax = t+dt;
00525     if ( tminus )
00526       *tminus = tmin;
00527     if ( tplus )
00528       *tplus = tmax;
00529   }
00530 
00531   return rc;
00532 }
00533 
00534 
00535 ON_BOOL32
00536 ON_EvNormal(int limit_dir,
00537                 const ON_3dVector& Du, const ON_3dVector& Dv, 
00538                 const ON_3dVector& Duu, const ON_3dVector& Duv, const ON_3dVector& Dvv, 
00539                 ON_3dVector& N)
00540 {
00541   const double DuoDu = Du.LengthSquared();
00542   const double DuoDv = Du*Dv;
00543   const double DvoDv = Dv.LengthSquared();
00544   if ( ON_EvJacobian(DuoDu,DuoDv,DvoDv,NULL) ) {
00545     N = ON_CrossProduct(Du,Dv);
00546   }
00547   else {
00548     /* degenerate jacobian - try to compute normal using limit
00549      *
00550      * P,Du,Dv,Duu,Duv,Dvv = srf and partials evaluated at (u0,v0).
00551      * Su,Sv,Suu,Suv,Svv = partials evaluated at (u,v).
00552      * Assume that srf : R^2 -> R^3 is analytic near (u0,v0).
00553      *
00554      * srf(u0+u,v0+v) = srf(u0,v0) + u*Du + v*Dv 
00555      *                  + (1/2)*u^2*Duu + u*v*Duv + (1/2)v^2*Dvv
00556      *                  + cubic and higher order terms.
00557      *
00558      * Su X Sv = Du X Dv + u*(Du X Duv + Duu X Dv) + v*(Du X Dvv + Duv X Dv) 
00559      *           + quadratic and higher order terms
00560      * 
00561      * Set 
00562      * (1) A = (Du X Duv + Duu X Dv), 
00563      * (2) B = (Du X Dvv + Duv X Dv) and assume
00564      * Du X Dv = 0.  Then 
00565      *
00566      * |Su X Sv|^2 = u^2*AoA + 2uv*AoB + v^2*BoB + cubic and higher order terms
00567      *
00568      * If u = a*t, v = b*t and (a^2*AoA + 2ab*AoB + b^2*BoB) != 0, then
00569      * 
00570      *        Su X Sv                   a*A + b*B
00571      * lim   ---------  =  ----------------------------------
00572      * t->0  |Su X Sv|      sqrt(a^2*AoA + 2ab*AoB + b^2*BoB)
00573      *
00574      * All I need is the direction, so I just need to compute a*A + b*B as carefully
00575      * as possible.  Note that
00576      * (3)  a*A + b*B = Du X (a*Duv + b*Dvv)  - Dv X (a*Duu + b*Duv).
00577      * Formaula (3) requires fewer flops than using formulae (1) and (2) to 
00578      * compute a*A + b*B.  In addition, when |Du| << |Dv| or |Du| >> |Dv|,
00579      * formula (3) reduces the number of subtractions between numbers of
00580      * similar size.  Since the (nearly) zero first partial is the most common
00581      * is more common than the (nearly) (anti) parallel case, I'll use
00582      * formula (3).  If you're reading this because you're not getting
00583      * the right answer and you can't find any bugs, you might want to
00584      * try using formulae (1) and (2).
00585      *
00586      * The limit_dir argument determines which direction is used to compute the
00587      * limit.
00588      *                  |
00589      *   limit_dir == 2 |  limit_dir == 1
00590      *           \      |      /
00591      *            \     |     /
00592      *             \    |    /
00593      *              \   |   /
00594      *               \  |  /
00595      *                \ | /
00596      *                 \|/
00597      *   ---------------*--------------
00598      *                 /|\
00599      *                / | \
00600      *               /  |  \
00601      *              /   |   \
00602      *             /    |    \
00603      *            /     |     \
00604      *           /      |      \
00605      *   limit_dir == 3 |  limit_dir == 4
00606      *                  |
00607      *
00608      */
00609 
00610     double a,b;
00611     ON_3dVector V, Au, Av;
00612 
00613     switch(limit_dir) {
00614     case  2: /* from 2nd  quadrant to point */
00615       a = -1.0; b =  1.0; break;
00616     case  3: /* from 3rd  quadrant to point */
00617       a = -1.0; b = -1.0; break;
00618     case  4: /* from 4rth quadrant to point */
00619       a =  1.0; b = -1.0; break;
00620     default: /* from 1rst quadrant to point */
00621       a =  1.0; b =  1.0; break;
00622     }
00623 
00624     V = a*Duv + b*Dvv;
00625     Av.x = Du.y*V.z - Du.z*V.y;
00626     Av.y = Du.z*V.x - Du.x*V.z;
00627     Av.z = Du.x*V.y - Du.y*V.x;
00628 
00629     V = a*Duu + b*Duv;
00630     Au.x = V.y*Dv.z - V.z*Dv.y;
00631     Au.y = V.z*Dv.x - V.x*Dv.z;
00632     Au.z = V.x*Dv.y - V.y*Dv.x;
00633 
00634     N = Av + Au;
00635   }
00636   
00637   return N.Unitize();
00638 }
00639 
00640 bool ON_EvTangent( 
00641         const ON_3dVector& D1, // first derivative
00642         const ON_3dVector& D2, // second derivative
00643         ON_3dVector& T         // Unit tangent returned here
00644         )
00645 {
00646   // Evaluate unit tangent from first and second derivatives
00647   // T = D1 / |D1|
00648 
00649   bool rc = false;
00650   double d1 = D1.Length();
00651 
00652   if (d1 == 0.0) 
00653   {
00654     // Use L'hopital's rule to show that if the unit tanget
00655     // exists and the 1rst derivative is zero and the 2nd derivative is
00656     // nonzero, then the unit tangent is equal to +/-the unitized 
00657     // 2nd derivative.  The sign is equal to the sign of D1(s) o D2(s)
00658     // as s approaches the evaluation parameter.
00659     //
00660     d1 = D2.Length();
00661     if (d1 > 0.0) 
00662     {
00663       T = D2/d1;
00664       rc = true;
00665     }
00666     else 
00667     {
00668       T.Zero();
00669     }
00670   }
00671   else 
00672   {
00673     T = D1/d1;
00674     rc = true;
00675   }
00676   return rc;  
00677 }
00678 
00679 
00680 ON_BOOL32 
00681 ON_EvCurvature( 
00682         const ON_3dVector& D1, // first derivative
00683         const ON_3dVector& D2, // second derivative
00684         ON_3dVector& T,       // Unit tangent returned here
00685         ON_3dVector& K        // Curvature returned here
00686         )
00687 {
00688   // Evaluate unit tangent and curvature from first and second derivatives
00689   // T = D1 / |D1|
00690   // K = ( D2 - (D2 o T)*T )/( D1 o D1)
00691 
00692   ON_BOOL32 rc = false;
00693   double d1 = D1.Length();
00694 
00695   if (d1 == 0.0) 
00696   {
00697     // Use L'hopital's rule to show that if the unit tanget
00698     // exists and the 1rst derivative is zero and the 2nd derivative is
00699     // nonzero, then the unit tangent is equal to +/-the unitized 
00700     // 2nd derivative.  The sign is equal to the sign of D1(s) o D2(s)
00701     // as s approaches the evaluation parameter.
00702     //
00703     d1 = D2.Length();
00704     if (d1 > 0.0) {
00705       T = D2/d1;
00706     }
00707     else 
00708     {
00709       T.Zero();
00710     }
00711     K.Zero();
00712   }
00713   else 
00714   {
00715     T = D1/d1;
00716     const double negD2oT = -D2*T;
00717     d1 = 1.0/(d1*d1);
00718     K = d1*( D2 + negD2oT*T );
00719     rc = true;
00720   }
00721   return rc;  
00722 }
00723 
00724 bool ON_EvSectionalCurvature( 
00725     const ON_3dVector& S10, 
00726     const ON_3dVector& S01,
00727     const ON_3dVector& S20, 
00728     const ON_3dVector& S11, 
00729     const ON_3dVector& S02,
00730     const ON_3dVector& planeNormal,
00731     ON_3dVector& K 
00732     )
00733 {
00734   ON_3dVector M, D1, M1, D2;
00735   double a, b, e, pr;
00736   int rank;
00737 
00738   // Calculates the curvature of the intersection of the surface
00739   // and plane at the point were the surface partials were evaluated.
00740   // If D1 and D2 are the derivatives of any parametric curve,
00741   // then the curvature is
00742   //
00743   // K = (D2 - (D2oD1)/(D1oD1)*D1)/(D1oD1)
00744   //
00745   // So, the trick is to assign a parameterization to the intersection
00746   // curve and use the surface partials and plane normal
00747   // to calculate the curve's derivatives.  For computational reasons, 
00748   // I'm choosing the parameterization such that
00749   //
00750   // D1 = (Su X Sv) X sectionNormal.
00751   //
00752   // Then
00753   //
00754   // D2 = ((Suu*u' + Suv*v') X Sv  +  Su X (Suv*u' + Svv*v')) X sectionNormal,
00755   //
00756   // where the (unknown) intersection curve is srf(u(t),v(t)).  But, we
00757   // do know D1 can also be computed as
00758   // 
00759   // D1 = Su*u' + Sv*v'
00760   //
00761   // So, if Su and Sv are linearly independent, then we have
00762   // (Su X Sv) X sectionNormal = Su*u' + Sv*v' and can solve for u' and v'.
00763   //
00764 
00765 
00766   // M = Su X Sv  (surface normal = M/|M|)
00767   //M = ON_CrossProduct(S10,S01); 
00768   M.x = S10.y*S01.z - S01.y*S10.z;
00769   M.y = S10.z*S01.x - S01.z*S10.x;
00770   M.z = S10.x*S01.y - S01.x*S10.y;
00771 
00772   // D1 = 1st derivative of the intersection curve
00773   //D1 = ON_CrossProduct(M,sectionN);
00774   D1.x = M.y*planeNormal.z - planeNormal.y*M.z;
00775   D1.y = M.z*planeNormal.x - planeNormal.z*M.x;
00776   D1.z = M.x*planeNormal.y - planeNormal.x*M.y;
00777 
00778 
00779   // D1 is tangent to the surface.  Find a, b so that D1 = a*Su + b*Sv.
00780   rank = ON_Solve3x2( &S10.x, &S01.x, D1.x, D1.y, D1.z, &a, &b, &e, &pr );
00781   if ( rank < 2 )
00782   {
00783     K.x = 0.0;
00784     K.y = 0.0;
00785     K.z = 0.0;
00786     return false;
00787   }
00788 
00789   // M1 = derivative of M = (a*Suu + v*Suv) x Sv  +  Su x (a*Suv + b*Svv)
00790   //M1 = ON_CrossProduct(a*S20 + b*S11, S01) + ON_CrossProduct(S10, a*S11 + b*S02);
00791   
00792   D2.x = a*S20.x + b*S11.x;
00793   D2.y = a*S20.y + b*S11.y;
00794   D2.z = a*S20.z + b*S11.z;
00795   M.x = D2.y*S01.z - S01.y*D2.z;
00796   M.y = D2.z*S01.x - S01.z*D2.x;
00797   M.z = D2.x*S01.y - S01.x*D2.y;
00798   
00799   D2.x = a*S11.x + b*S02.x;
00800   D2.y = a*S11.y + b*S02.y;
00801   D2.z = a*S11.z + b*S02.z;
00802   M.x += S10.y*D2.z - D2.y*S10.z;
00803   M.y += S10.z*D2.x - D2.z*S10.x;
00804   M.z += S10.x*D2.y - D2.x*S10.y;
00805 
00806   // D2 = 2nd derivative of the intersection curve
00807   //D2 = ON_CrossProduct(M1,sectionN);
00808   D2.x = M.y*planeNormal.z - planeNormal.y*M.z;
00809   D2.y = M.z*planeNormal.x - planeNormal.z*M.x;
00810   D2.z = M.x*planeNormal.y - planeNormal.x*M.y;
00811 
00812   a = D1.x*D1.x + D1.y*D1.y + D1.z*D1.z;
00813 
00814   if ( !(a > ON_DBL_MIN) )
00815   {
00816     K.x = 0.0;
00817     K.y = 0.0;
00818     K.z = 0.0;
00819     return false;
00820   }
00821 
00822   a = 1.0/a;
00823   b = -a*(D2.x*D1.x + D2.y*D1.y + D2.z*D1.z);
00824   K.x = a*(D2.x + b*D1.x);
00825   K.y = a*(D2.y + b*D1.y);
00826   K.z = a*(D2.z + b*D1.z);
00827 
00828   return true;
00829 }
00830 
00831 ON_BOOL32 ON_IsContinuous(
00832   ON::continuity desired_continuity,
00833   ON_3dPoint Pa, ON_3dVector D1a, ON_3dVector D2a,
00834   ON_3dPoint Pb, ON_3dVector D1b, ON_3dVector D2b,
00835   double point_tolerance,
00836   double d1_tolerance,
00837   double d2_tolerance,
00838   double cos_angle_tolerance,
00839   double curvature_tolerance
00840   )
00841 {
00842   ON_3dVector Ta, Tb, Ka, Kb;
00843 
00844   switch( ON::ParametricContinuity(desired_continuity) )
00845   {
00846   case ON::unknown_continuity:
00847     break;
00848 
00849   case ON::C0_continuous:  
00850   case ON::C0_locus_continuous:  
00851     if ( !(Pa-Pb).IsTiny(point_tolerance) )
00852       return false;
00853     break;
00854 
00855   case ON::C1_continuous:
00856   case ON::C1_locus_continuous:
00857     if ( !(Pa-Pb).IsTiny(point_tolerance) || !(D1a-D1b).IsTiny(d1_tolerance) )
00858       return false;
00859     break;
00860 
00861   case ON::G1_continuous:
00862   case ON::G1_locus_continuous:
00863     Ta = D1a;
00864     if ( !Ta.Unitize() )
00865       ON_EvCurvature( D1a, D2a, Ta, Ka );
00866     Tb = D1b;
00867     if ( !Tb.Unitize() )
00868       ON_EvCurvature( D1b, D2b, Tb, Kb );
00869     if ( !(Pa-Pb).IsTiny(point_tolerance) || Ta*Tb < cos_angle_tolerance )
00870       return false;
00871     break;
00872 
00873   case ON::C2_continuous:
00874   case ON::C2_locus_continuous:
00875   case ON::Cinfinity_continuous:
00876     if ( !(Pa-Pb).IsTiny(point_tolerance) || !(D1a-D1b).IsTiny(d1_tolerance) || !(D2a-D2b).IsTiny(d2_tolerance) )
00877       return false;
00878     break;
00879 
00880   case ON::G2_continuous:
00881   case ON::G2_locus_continuous:
00882   case ON::Gsmooth_continuous:
00883     ON_EvCurvature( D1a, D2a, Ta, Ka );
00884     ON_EvCurvature( D1b, D2b, Tb, Kb );
00885     if ( !(Pa-Pb).IsTiny(point_tolerance) || Ta*Tb < cos_angle_tolerance )
00886       return false;
00887     if ( ON::Gsmooth_continuous == desired_continuity )
00888     {
00889       if ( !ON_IsGsmoothCurvatureContinuous(Ka,Kb,cos_angle_tolerance,curvature_tolerance) )
00890         return false;
00891     }
00892     else
00893     {
00894       if ( !ON_IsG2CurvatureContinuous(Ka,Kb,cos_angle_tolerance,curvature_tolerance) )
00895         return false;
00896     }
00897     break;
00898   }
00899 
00900   return true;
00901 }
00902 
00903 int 
00904 ON_SearchMonotoneArray(const double* array, int length, double t)
00905 /*****************************************************************************
00906 Find interval in an increasing array of doubles
00907  
00908 INPUT:
00909   array
00910     A monotone increasing (array[i] <= array[i+1]) array of length doubles.
00911   length (>=1)
00912     number of doubles in array
00913   t
00914     parameter
00915 OUTPUT:
00916   ON_GetdblArrayIndex()
00917     -1:         t < array[0]
00918      i:         (0 <= i <= length-2) array[i] <= t < array[i+1]
00919      length-1:  t == array[length-1]
00920      length:    t  > array[length-1]
00921 COMMENTS:
00922   If length < 1 or array is not monotone increasing, you will get a meaningless
00923   answer and may crash your program.
00924 EXAMPLE:
00925   // Given a "t", find the knots that define the span used to evaluate a
00926   // nurb at t; i.e., find "i" so that
00927   // knot[i] <= ... <= knot[i+order-2] 
00928   //   <= t < knot[i+order-1] <= ... <= knot[i+2*(order-1)-1]
00929   i = ON_GetdblArrayIndex(knot+order-2,cv_count-order+2,t);
00930   if (i < 0) i = 0; else if (i > cv_count - order) i = cv_count - order;
00931 RELATED FUNCTIONS:
00932   ON_
00933   ON_
00934 *****************************************************************************/
00935 
00936 {                 
00937   int 
00938     i, i0, i1;
00939 
00940   length--;
00941 
00942   /* Since t is frequently near the ends and bisection takes the
00943    * longest near the ends, trap those cases here.
00944    */
00945   if (t < array[0]) 
00946     return -1;
00947   if (t >= array[length])
00948     return (t > array[length]) ? length+1 : length;
00949   if (t < array[1])
00950     return 0;
00951   if (t >= array[length-1])
00952     return (length-1);
00953 
00954 
00955   i0 = 0;
00956   i1 = length;
00957   while (array[i0] == array[i0+1]) i0++;
00958   while (array[i1] == array[i1-1]) i1--;
00959   /* From now on we have 
00960    *  1.) array[i0] <= t < array[i1] 
00961    *  2.) i0 <= i < i1.
00962    * When i0+1 == i1, we have array[i0] <= t < array[i0+1] 
00963    * and i0 is the answer we seek.
00964    */
00965   while (i0+1 < i1) {
00966     i = (i0+i1)>>1;
00967     if (t < array[i]) {
00968       i1 = i;
00969       while (array[i1] == array[i1-1]) i1--;
00970     }
00971     else {
00972       i0 = i;
00973       while (array[i0] == array[i0+1]) i0++;
00974     }
00975   }
00976 
00977   return i0;
00978 }
00979 
00980 
00981 double 
00982 ON_BinomialCoefficient(int i, int j)
00983 {
00984 #define MAX_HALF_N 26
00985 static const double bc[((MAX_HALF_N-2)*(MAX_HALF_N-1))/2 + MAX_HALF_N - 2] =
00986  {15.0, 20.0, 28.0, 56.0, 70.0, 45.0, 120.0, 210.0, 252.0, 66.0,
00987   220.0, 495.0, 792.0, 924.0, 91.0, 364.0, 1001.0, 2002.0, 3003.0,
00988   3432.0, 120.0, 560.0, 1820.0, 4368.0, 8008.0, 11440.0, 12870.0,
00989   153.0, 816.0, 3060.0, 8568.0, 18564.0, 31824.0, 43758.0, 48620.0,
00990   190.0, 1140.0, 4845.0, 15504.0, 38760.0, 77520.0, 125970.0,
00991   167960.0, 184756.0, 231.0, 1540.0, 7315.0, 26334.0, 74613.0,
00992   170544.0, 319770.0, 497420.0, 646646.0, 705432.0, 276.0, 2024.0,
00993   10626.0, 42504.0, 134596.0, 346104.0, 735471.0, 1307504.0,
00994   1961256.0, 2496144.0, 2704156.0, 325.0, 2600.0, 14950.0, 65780.0,
00995   230230.0, 657800.0, 1562275.0, 3124550.0, 5311735.0, 7726160.0,
00996   9657700.0, 10400600.0, 378.0, 3276.0, 20475.0, 98280.0, 376740.0,
00997   1184040.0, 3108105.0, 6906900.0, 13123110.0, 21474180.0,
00998   30421755.0, 37442160.0, 40116600.0, 435.0, 4060.0, 27405.0,
00999   142506.0, 593775.0, 2035800.0, 5852925.0, 14307150.0, 30045015.0,
01000   54627300.0, 86493225.0, 119759850.0, 145422675.0, 155117520.0,
01001   496.0, 4960.0, 35960.0, 201376.0, 906192.0, 3365856.0,
01002   10518300.0, 28048800.0, 64512240.0, 129024480.0, 225792840.0,
01003   347373600.0, 471435600.0, 565722720.0, 601080390.0, 561.0,
01004   5984.0, 46376.0, 278256.0, 1344904.0, 5379616.0, 18156204.0,
01005   52451256.0, 131128140.0, 286097760.0, 548354040.0, 927983760.0,
01006   1391975640.0, 1855967520.0, 2203961430.0, 2333606220.0, 630.0,
01007   7140.0, 58905.0, 376992.0, 1947792.0, 8347680.0, 30260340.0,
01008   94143280.0, 254186856.0, 600805296.0, 1251677700.0, 2310789600.0,
01009   3796297200.0, 5567902560.0, 7307872110.0, 8597496600.0,
01010   9075135300.0, 703.0, 8436.0, 73815.0, 501942.0, 2760681.0,
01011   12620256.0, 48903492.0, 163011640.0, 472733756.0, 1203322288.0,
01012   2707475148.0, 5414950296.0, 9669554100.0, 15471286560.0,
01013   22239974430.0, 28781143380.0, 33578000610.0, 35345263800.0,
01014   780.0, 9880.0, 91390.0, 658008.0, 3838380.0, 18643560.0,
01015   76904685.0, 273438880.0, 847660528.0, 2311801440.0, 5586853480.0,
01016   12033222880.0, 23206929840.0, 40225345056.0, 62852101650.0,
01017   88732378800.0, 113380261800.0, 131282408400.0, 137846528820.0,
01018   861.0, 11480.0, 111930.0, 850668.0, 5245786.0, 26978328.0,
01019   118030185.0, 445891810.0, 1471442973.0, 4280561376.0,
01020   11058116888.0, 25518731280.0, 52860229080.0, 98672427616.0,
01021   166509721602.0, 254661927156.0, 353697121050.0, 446775310800.0,
01022   513791607420.0, 538257874440.0, 946.0, 13244.0, 135751.0,
01023   1086008.0, 7059052.0, 38320568.0, 177232627.0, 708930508.0,
01024   2481256778.0, 7669339132.0, 21090682613.0, 51915526432.0,
01025   114955808528.0, 229911617056.0, 416714805914.0, 686353797976.0,
01026   1029530696964.0, 1408831480056.0, 1761039350070.0,
01027   2012616400080.0, 2104098963720.0, 1035.0, 15180.0, 163185.0,
01028   1370754.0, 9366819.0, 53524680.0, 260932815.0, 1101716330.0,
01029   4076350421.0, 13340783196.0, 38910617655.0, 101766230790.0,
01030   239877544005.0, 511738760544.0, 991493848554.0, 1749695026860.0,
01031   2818953098830.0, 4154246671960.0, 5608233007146.0,
01032   6943526580276.0, 7890371113950.0, 8233430727600.0, 1128.0,
01033   17296.0, 194580.0, 1712304.0, 12271512.0, 73629072.0,
01034   377348994.0, 1677106640.0, 6540715896.0, 22595200368.0,
01035   69668534468.0, 192928249296.0, 482320623240.0, 1093260079344.0,
01036   2254848913647.0, 4244421484512.0, 7309837001104.0,
01037   11541847896480.0, 16735679449896.0, 22314239266528.0,
01038   27385657281648.0, 30957699535776.0, 32247603683100.0, 1225.0,
01039   19600.0, 230300.0, 2118760.0, 15890700.0, 99884400.0,
01040   536878650.0, 2505433700.0, 10272278170.0, 37353738800.0,
01041   121399651100.0, 354860518600.0, 937845656300.0, 2250829575120.0,
01042   4923689695575.0, 9847379391150.0, 18053528883775.0,
01043   30405943383200.0, 47129212243960.0, 67327446062800.0,
01044   88749815264600.0, 108043253365600.0, 121548660036300.0,
01045   126410606437752.0, 1326.0, 22100.0, 270725.0, 2598960.0,
01046   20358520.0, 133784560.0, 752538150.0, 3679075400.0,
01047   15820024220.0, 60403728840.0, 206379406870.0, 635013559600.0,
01048   1768966344600.0, 4481381406320.0, 10363194502115.0,
01049   21945588357420.0, 42671977361650.0, 76360380541900.0,
01050   125994627894135.0, 191991813933920.0, 270533919634160.0,
01051   352870329957600.0, 426384982032100.0, 477551179875952.0,
01052   495918532948104.0};
01053 
01054   int n, half_n, bc_i;
01055 
01056   if (i  < 0 || j  < 0) return  0.0;
01057   if (0 == i || 0 == j) return  1.0;
01058   n = i+j;
01059   if (1 == i || 1 == j) return (double)n;
01060   if (4 == n)           return  6.0;
01061   if (5 == n)           return 10.0;
01062 
01063   if (n%2)
01064     return ON_BinomialCoefficient(i-1,j)+ON_BinomialCoefficient(i,j-1);
01065 
01066   half_n = n >> 1;
01067   if (half_n > MAX_HALF_N)
01068     return ON_BinomialCoefficient(i-1,j)+ON_BinomialCoefficient(i,j-1);
01069   if (i > half_n)
01070     i = n - i;
01071   /* at this point we have n even,
01072    * MAX_HALF_N*2 >= n >= 6 and 1 < i <= n/2
01073    * and we grab the answer from the bc[] table.
01074    */
01075   half_n -= 2;
01076   bc_i = ((half_n*(half_n+1))>>1) + i - 3;
01077   return bc[bc_i];
01078 
01079 #undef MAX_HALF_N
01080 }
01081 
01082 ON_DECL
01083 double ON_TrinomialCoefficient( 
01084           int i,
01085           int j,
01086           int k
01087           )
01088 {
01089   return (ON_BinomialCoefficient(i,j+k)*ON_BinomialCoefficient(j,k));
01090 }
01091 
01092 
01093 ON_BOOL32 
01094 ON_IsValidPointList(
01095        int dim,
01096        int is_rat,
01097        int count,
01098        int stride,
01099        const float* p
01100        )
01101 {
01102   return ( dim > 0 && stride >= (is_rat?(dim+1):dim) && count >= 0 && p != NULL ) ? true : false;
01103 }
01104 
01105 
01106 ON_BOOL32 
01107 ON_IsValidPointList(
01108        int dim,
01109        int is_rat,
01110        int count,
01111        int stride,
01112        const double* p
01113        )
01114 {
01115   return ( dim > 0 && stride >= (is_rat?(dim+1):dim) && count >= 0 && p != NULL ) ? true : false;
01116 }
01117 
01118 
01119 ON_BOOL32 
01120 ON_IsValidPointGrid(
01121         int dim,
01122         ON_BOOL32 is_rat,
01123         int point_count0, int point_count1,
01124         int point_stride0, int point_stride1,
01125         const double* p
01126         )
01127 {
01128   if ( dim < 1 || point_count0 < 1 || point_count1 < 1 || p == NULL )
01129     return false;
01130   if ( is_rat )
01131     dim++;
01132   if ( point_stride0 < dim || point_stride1 < dim )
01133     return false;
01134   if ( point_stride0 <= point_stride1 ) {
01135     if ( point_stride1 < point_stride0*point_count0 )
01136       return false;
01137   }
01138   else {
01139     if ( point_stride0 < point_stride1*point_count1 )
01140       return false;
01141   }
01142   return true;
01143 }
01144 
01145 
01146 bool ON_ReversePointList(
01147        int dim,
01148        int is_rat,
01149        int count,
01150        int stride,
01151        double* p
01152        )
01153 {
01154   if ( !ON_IsValidPointList(dim,is_rat,count,stride,p) )
01155     return false;
01156   if ( is_rat )
01157     dim++;
01158   if ( count <= 1 )
01159     return true;
01160   const size_t ele_size = dim*sizeof(*p);
01161   void* t = onmalloc(ele_size);
01162   int i, j;
01163   for ( i = 0, j = (count-1)*stride; i < j; i += stride, j -= stride ) {
01164     memcpy( t,   p+i, ele_size );
01165     memcpy( p+i, p+j, ele_size );
01166     memcpy( p+j, t,   ele_size );
01167   }
01168   onfree(t);
01169   return true;
01170 }
01171 
01172 
01173 ON_BOOL32 
01174 ON_ReversePointGrid(
01175         int dim,
01176         ON_BOOL32 is_rat,
01177         int point_count0, int point_count1,
01178         int point_stride0, int point_stride1,
01179         double* p,
01180         int dir
01181         )
01182 {
01183   ON_BOOL32 rc = false;
01184   if ( !dir ) {
01185     rc = ON_ReversePointGrid( dim, is_rat, point_count1, point_count0, point_stride1, point_stride0, p, 1 );
01186   }
01187   else {
01188     int i;
01189     for ( i = 0; i < point_count0; i++ ) {
01190       if ( !ON_ReversePointList( dim, is_rat, point_count1, point_stride1, p + i*point_stride0 ) ) {
01191         rc = false;
01192         break;
01193       }
01194       else if (!i) {
01195         rc = true;
01196       }
01197     }
01198   }
01199   return rc;
01200 }
01201 
01202 
01203 bool 
01204 ON_SwapPointListCoordinates( int count, int stride, float* p,
01205                                    int i, int j )
01206 {
01207   float t;
01208   int k;
01209   if ( !ON_IsValidPointList(stride,0,count,stride,p) )
01210     return false;
01211   if ( i < 0 || j < 0 || i >= stride || j >= stride )
01212     return false;
01213   if ( i == j || count == 0 )
01214     return true;
01215   for ( k = 0; k < count; k++, p += stride ) {
01216     t = p[i]; 
01217     p[i] = p[j];
01218     p[j] = t;
01219   }
01220   return true;
01221 }
01222 
01223 
01224 bool 
01225 ON_SwapPointListCoordinates( int count, int stride, double* p,
01226                                              int i, int j )
01227 {
01228   double t;
01229   int k;
01230   if ( !ON_IsValidPointList(stride,0,count,stride,p) )
01231     return false;
01232   if ( i < 0 || j < 0 || i >= stride || j >= stride )
01233     return false;
01234   if ( i == j || count == 0 )
01235     return true;
01236   for ( k = 0; k < count; k++, p += stride ) {
01237     t = p[i]; 
01238     p[i] = p[j];
01239     p[j] = t;
01240   }
01241   return true;
01242 }
01243 
01244 
01245 ON_BOOL32 
01246 ON_SwapPointGridCoordinates(
01247         int point_count0, int point_count1,
01248         int point_stride0, int point_stride1,
01249         double* p,
01250         int i, int j // coordinates to swap
01251         )
01252 {
01253   ON_BOOL32 rc = false;
01254   if ( p ) {
01255     double t;
01256     int k, m;
01257     double* pk;
01258     for ( k = 0; k < point_count0; k++ ) {
01259       pk = p + k*point_stride0;
01260       for ( m = 0; m < point_count1; m++ ) {
01261         t = pk[i]; pk[i] = pk[j]; pk[j] = t;
01262         pk += point_stride1;
01263       }
01264     }
01265     rc = true;
01266   }
01267   return rc;
01268 }
01269 
01270 
01271 bool 
01272 ON_TransformPointList(
01273                   int dim, int is_rat, int count, 
01274                   int stride, float* point,
01275                   const ON_Xform& xform
01276                   )
01277 {
01278   bool rc = true;
01279   double x, y, z, w;
01280 
01281   if ( !ON_IsValidPointList( dim, is_rat, count, stride, point ) )
01282     return false;
01283   if ( xform.m_xform == NULL )
01284     return false;
01285   if (count == 0)
01286     return true;
01287 
01288   if (is_rat) {
01289     switch(dim) {
01290     case 1:
01291       while(count--) {
01292                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][3]*point[1];
01293                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][3]*point[1];
01294                                 point[0] = (float)x; point[1] = (float)w;
01295                                 point += stride;
01296       }
01297       break;
01298     case 2:
01299       while(count--) {
01300                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][3]*point[2];
01301                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][3]*point[2];
01302                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][3]*point[2];
01303                                 point[0] = (float)x; point[1] = (float)y; point[2] = (float)w;
01304                                 point += stride;
01305       }
01306       break;
01307     default: // dim >= 3
01308       while(count--) {
01309                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][2]*point[2] + xform.m_xform[0][3]*point[dim];
01310                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][2]*point[2] + xform.m_xform[1][3]*point[dim];
01311                                 z = xform.m_xform[2][0]*point[0] + xform.m_xform[2][1]*point[1] + xform.m_xform[2][2]*point[2] + xform.m_xform[2][3]*point[dim];
01312                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][2]*point[2] + xform.m_xform[3][3]*point[dim];
01313                                 point[0] = (float)x; point[1] = (float)y; point[2] = (float)z; point[dim] = (float)w;
01314                                 point += stride;
01315       }
01316       break;
01317     }
01318   }
01319   else {
01320     switch(dim) {
01321     case 1:
01322       while(count--) {
01323                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][3];
01324         if (w==0.0) {
01325           rc = false;
01326           w = 1.0;
01327         }
01328         else
01329                                   w = 1.0/w;
01330                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][3];
01331                                 point[0] = (float)(w*x);
01332                                 point += stride;
01333       }
01334       break;
01335     case 2:
01336       while(count--) {
01337                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][3];
01338         if (w==0.0) {
01339           rc = false;
01340           w = 1.0;
01341         }
01342         else
01343                                   w = 1.0/w;
01344                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][3];
01345                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][3];
01346                                 point[0] = (float)(w*x); point[1] = (float)(w*y);
01347                                 point += stride;
01348       }
01349       break;
01350     default: // dim = 3
01351       while(count--) {
01352                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][2]*point[2] + xform.m_xform[3][3];
01353         if (w==0.0)  {
01354           rc = false;
01355           w = 1.0;
01356         }
01357         else
01358                                   w = 1.0/w;
01359                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][2]*point[2] + xform.m_xform[0][3];
01360                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][2]*point[2] + xform.m_xform[1][3];
01361                                 z = xform.m_xform[2][0]*point[0] + xform.m_xform[2][1]*point[1] + xform.m_xform[2][2]*point[2] + xform.m_xform[2][3];
01362                                 point[0] = (float)(w*x); point[1] = (float)(w*y); point[2] = (float)(w*z);
01363                                 point += stride;
01364       }
01365       break;
01366     }
01367   }
01368   return rc;
01369 }
01370 
01371 
01372 bool 
01373 ON_TransformPointList(
01374                   int dim, int is_rat, int count, 
01375                   int stride, double* point,
01376                   const ON_Xform& xform
01377                   )
01378 {
01379   bool rc = true;
01380   double x, y, z, w;
01381 
01382   if ( !ON_IsValidPointList( dim, is_rat, count, stride, point ) )
01383     return false;
01384   if ( xform.m_xform == NULL )
01385     return false;
01386   if (count == 0)
01387     return true;
01388 
01389   if (is_rat) {
01390     switch(dim) {
01391     case 1:
01392       while(count--) {
01393                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][3]*point[1];
01394                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][3]*point[1];
01395                                 point[0] = x; point[1] = w;
01396                                 point += stride;
01397       }
01398       break;
01399     case 2:
01400       while(count--) {
01401                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][3]*point[2];
01402                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][3]*point[2];
01403                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][3]*point[2];
01404                                 point[0] = x; point[1] = y; point[2] = w;
01405                                 point += stride;
01406       }
01407       break;
01408     default: // dim >= 3
01409       while(count--) {
01410                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][2]*point[2] + xform.m_xform[0][3]*point[dim];
01411                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][2]*point[2] + xform.m_xform[1][3]*point[dim];
01412                                 z = xform.m_xform[2][0]*point[0] + xform.m_xform[2][1]*point[1] + xform.m_xform[2][2]*point[2] + xform.m_xform[2][3]*point[dim];
01413                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][2]*point[2] + xform.m_xform[3][3]*point[dim];
01414                                 point[0] = x; point[1] = y; point[2] = z; point[dim] = w;
01415                                 point += stride;
01416       }
01417       break;
01418     }
01419   }
01420   else {
01421     switch(dim) {
01422     case 1:
01423       while(count--) {
01424                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][3];
01425         if (w==0.0) {
01426           rc = false;
01427           w = 1.0;
01428         }
01429         else
01430                                   w = 1.0/w;
01431                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][3];
01432                                 point[0] = w*x;
01433                                 point += stride;
01434       }
01435       break;
01436     case 2:
01437       while(count--) {
01438                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][3];
01439         if (w==0.0) {
01440           rc = false;
01441           w = 1.0;
01442         }
01443         else
01444                                   w = 1.0/w;
01445                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][3];
01446                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][3];
01447                                 point[0] = w*x; point[1] = w*y;
01448                                 point += stride;
01449       }
01450       break;
01451     default: // dim = 3
01452       while(count--) {
01453                                 w = xform.m_xform[3][0]*point[0] + xform.m_xform[3][1]*point[1] + xform.m_xform[3][2]*point[2] + xform.m_xform[3][3];
01454         if (w==0.0)  {
01455           rc = false;
01456           w = 1.0;
01457         }
01458         else
01459                                   w = 1.0/w;
01460                                 x = xform.m_xform[0][0]*point[0] + xform.m_xform[0][1]*point[1] + xform.m_xform[0][2]*point[2] + xform.m_xform[0][3];
01461                                 y = xform.m_xform[1][0]*point[0] + xform.m_xform[1][1]*point[1] + xform.m_xform[1][2]*point[2] + xform.m_xform[1][3];
01462                                 z = xform.m_xform[2][0]*point[0] + xform.m_xform[2][1]*point[1] + xform.m_xform[2][2]*point[2] + xform.m_xform[2][3];
01463                                 point[0] = w*x; point[1] = w*y; point[2] = w*z;
01464                                 point += stride;
01465       }
01466       break;
01467     }
01468   }
01469   return rc;
01470 }
01471 
01472 
01473 ON_BOOL32 
01474 ON_TransformPointGrid(
01475                   int dim, int is_rat, 
01476                   int point_count0, int point_count1,
01477                   int point_stride0, int point_stride1,
01478                   double* point,
01479                   const ON_Xform& xform
01480                   )
01481 {
01482   ON_BOOL32 rc = false;
01483   int i;
01484   double* pt = point;
01485   for ( i = 0; i < point_count0; i++ ) {
01486     if ( !ON_TransformPointList( dim, is_rat, point_count1, point_stride1, pt, xform ) ) {
01487       rc = false;
01488     }
01489     else if ( !i ) {
01490       rc = true;
01491     }
01492     pt += point_stride0;
01493   }
01494   return rc;
01495 }
01496 
01497 
01498 ON_BOOL32 
01499 ON_TransformVectorList(
01500                   int dim, int count, 
01501                   int stride, float* vector,
01502                   const ON_Xform& xform
01503                   )
01504 {
01505   ON_BOOL32 rc = true;
01506   double x, y, z;
01507 
01508   if ( !ON_IsValidPointList( dim, 0, count, stride, vector ) )
01509     return false;
01510   if ( xform.m_xform == NULL )
01511     return false;
01512   if (count == 0)
01513     return true;
01514 
01515   switch(dim) {
01516   case 1:
01517     while(count--) {
01518                         x = xform.m_xform[0][0]*vector[0];
01519                         vector[0] = (float)x;
01520                         vector += stride;
01521     }
01522     break;
01523   case 2:
01524     while(count--) {
01525                         x = xform.m_xform[0][0]*vector[0] + xform.m_xform[0][1]*vector[1];
01526                         y = xform.m_xform[1][0]*vector[0] + xform.m_xform[1][1]*vector[1];
01527                         vector[0] = (float)x; vector[1] = (float)y;
01528                         vector += stride;
01529     }
01530     break;
01531   default: // dim >= 3
01532     while(count--) {
01533                         x = xform.m_xform[0][0]*vector[0] + xform.m_xform[0][1]*vector[1] + xform.m_xform[0][2]*vector[2];
01534                         y = xform.m_xform[1][0]*vector[0] + xform.m_xform[1][1]*vector[1] + xform.m_xform[1][2]*vector[2];
01535                         z = xform.m_xform[2][0]*vector[0] + xform.m_xform[2][1]*vector[1] + xform.m_xform[2][2]*vector[2];
01536                         vector[0] = (float)x; vector[1] = (float)y; vector[2] = (float)z;
01537                         vector += stride;
01538     }
01539     break;
01540   }
01541 
01542   return rc;
01543 }
01544 
01545 
01546 
01547 ON_BOOL32 
01548 ON_TransformVectorList(
01549                   int dim, int count, 
01550                   int stride, double* vector,
01551                   const ON_Xform& xform
01552                   )
01553 {
01554   ON_BOOL32 rc = true;
01555   double x, y, z;
01556 
01557   if ( !ON_IsValidPointList( dim, 0, count, stride, vector ) )
01558     return false;
01559   if ( xform.m_xform == NULL )
01560     return false;
01561   if (count == 0)
01562     return true;
01563 
01564   switch(dim) {
01565   case 1:
01566     while(count--) {
01567                         x = xform.m_xform[0][0]*vector[0];
01568                         vector[0] = x;
01569                         vector += stride;
01570     }
01571     break;
01572   case 2:
01573     while(count--) {
01574                         x = xform.m_xform[0][0]*vector[0] + xform.m_xform[0][1]*vector[1];
01575                         y = xform.m_xform[1][0]*vector[0] + xform.m_xform[1][1]*vector[1];
01576                         vector[0] = x; vector[1] = y;
01577                         vector += stride;
01578     }
01579     break;
01580   default: // dim >= 3
01581     while(count--) {
01582                         x = xform.m_xform[0][0]*vector[0] + xform.m_xform[0][1]*vector[1] + xform.m_xform[0][2]*vector[2];
01583                         y = xform.m_xform[1][0]*vector[0] + xform.m_xform[1][1]*vector[1] + xform.m_xform[1][2]*vector[2];
01584                         z = xform.m_xform[2][0]*vector[0] + xform.m_xform[2][1]*vector[1] + xform.m_xform[2][2]*vector[2];
01585                         vector[0] = x; vector[1] = y; vector[2] = z;
01586                         vector += stride;
01587     }
01588     break;
01589   }
01590 
01591   return rc;
01592 }
01593 
01594 bool ON_PointsAreCoincident(
01595     int dim,
01596     int is_rat,
01597     const double* pointA,
01598     const double* pointB
01599     )
01600 {
01601   double d, a, b, wa, wb;
01602   
01603   if ( dim < 1 || 0 == pointA || 0 == pointB )
01604     return false;
01605 
01606   if ( is_rat )
01607   {
01608     wa = pointA[dim];
01609     wb = pointB[dim];
01610     if ( 0.0 == wa || 0.0 == wb )
01611     {
01612       if ( 0.0 == wa && 0.0 == wb )
01613         return ON_PointsAreCoincident(dim,0,pointA,pointB);
01614       return false;
01615     }
01616     while(dim--)
01617     {
01618       a = *pointA++ / wa;
01619       b = *pointB++ / wb;
01620       d = fabs(a-b);
01621       if ( d <= ON_ZERO_TOLERANCE )
01622         continue;
01623       if ( d <= (fabs(a)+fabs(b))*ON_RELATIVE_TOLERANCE )
01624         continue;
01625       return false;
01626     }
01627   }
01628   else
01629   {
01630     while(dim--)
01631     {
01632       a = *pointA++;
01633       b = *pointB++;
01634       d = fabs(a-b);
01635       if ( d <= ON_ZERO_TOLERANCE )
01636         continue;
01637       if ( d <= (fabs(a)+fabs(b))*ON_RELATIVE_TOLERANCE )
01638         continue;
01639       return false;
01640     }
01641   }
01642 
01643   return true;
01644 }
01645 
01646 bool ON_PointsAreCoincident(
01647     int dim,
01648     int is_rat,
01649     int point_count,
01650     int point_stride,
01651     const double* points
01652     )
01653 {
01654   if ( 0 == points || point_count < 2 )
01655     return false;
01656   if ( point_stride < (is_rat?(dim+1):dim) )
01657     return false;
01658   if ( false == ON_PointsAreCoincident( dim, is_rat, points, points + ((point_count-1)*point_stride) ) )
01659     return false;
01660   if ( point_count > 2 )
01661   {
01662     point_count--;
01663     while ( point_count-- )
01664     {
01665       if ( false == ON_PointsAreCoincident(dim,is_rat,points,points + point_stride ) )
01666         return false;
01667       points += point_stride;
01668     }
01669   }
01670   return true;
01671 }
01672 
01673 int 
01674 ON_ComparePoint( // returns 
01675                               // -1: first < second
01676                               //  0: first == second
01677                               // +1: first > second
01678           int dim,
01679           ON_BOOL32 is_rat,
01680           const double* pointA,
01681           const double* pointB
01682           )
01683 {
01684   const double wA = (is_rat && pointA[dim] != 0.0) ? 1.0/pointA[dim] : 1.0;
01685   const double wB = (is_rat && pointB[dim] != 0.0) ? 1.0/pointB[dim] : 1.0;
01686   double a, b, tol;
01687   int i;
01688   for ( i = 0; i < dim; i++ ) {
01689     a = wA* *pointA++;
01690     b = wB* *pointB++;
01691     tol = (fabs(a) + fabs(b))* ON_RELATIVE_TOLERANCE;
01692     if ( tol <  ON_ZERO_TOLERANCE )
01693       tol =  ON_ZERO_TOLERANCE;
01694     if ( a < b-tol )
01695       return -1;
01696     if ( b < a-tol )
01697       return 1;
01698     if ( wA < wB- ON_SQRT_EPSILON )
01699       return -1;
01700     if ( wB < wA- ON_SQRT_EPSILON )
01701       return -1;
01702   }
01703   return 0;
01704 }
01705 
01706 
01707 int 
01708 ON_ComparePointList( // returns 
01709                               // -1: first < second
01710                               //  0: first == second
01711                               // +1: first > second
01712           int dim,
01713           ON_BOOL32 is_rat,
01714           int point_count,
01715           int point_strideA,
01716           const double* pointA,
01717           int point_strideB,
01718           const double* pointB
01719           )
01720 {
01721   int i, rc = 0, rc1 = 0;
01722   bool bDoSecondCheck = ( 1 == is_rat && dim <= 3 && point_count > 0 
01723                          && ON_IsValid(pointA[dim]) && 0.0 != pointA[dim]
01724                          && ON_IsValid(pointB[dim]) && 0.0 != pointB[dim]
01725                          );
01726   const double wA = bDoSecondCheck ? pointA[dim] : 1.0; 
01727   const double wB = bDoSecondCheck ? pointB[dim] : 1.0; 
01728   const double wAtol = wA*ON_ZERO_TOLERANCE;
01729   const double wBtol = wB*ON_ZERO_TOLERANCE;
01730   double A[3] = {0.0,0.0,0.0};
01731   double B[3] = {0.0,0.0,0.0};
01732   const size_t AB_size = dim*sizeof(A[0]);
01733 
01734   for ( i = 0; i < point_count && !rc; i++ ) 
01735   {
01736     rc = ON_ComparePoint( dim, is_rat, pointA, pointB );
01737     if (    rc && bDoSecondCheck 
01738          && fabs(wA - pointA[dim]) <= wAtol 
01739          && fabs(wB - pointB[dim]) <= wBtol )
01740     {
01741       if ( !rc1 )
01742         rc1 = rc;
01743       memcpy(A,pointA,AB_size);
01744       A[0] /= pointA[dim]; A[1] /= pointA[dim]; A[2] /= pointA[dim];
01745       memcpy(B,pointB,AB_size);
01746       B[0] /= pointB[dim]; B[1] /= pointB[dim]; B[2] /= pointB[dim];
01747       rc = ( 0 == ON_ComparePoint( dim, 0, A, B ) ) ? 0 : rc1;
01748     }
01749     pointA += point_strideA;
01750     pointB += point_strideB;
01751   }
01752 
01753   return rc;
01754 }
01755 
01756 
01757 ON_BOOL32 
01758 ON_IsPointListClosed(
01759        int dim,
01760        int is_rat,
01761        int count,
01762        int stride,
01763        const double* p
01764        )
01765 {
01766   ON_BOOL32 rc = false;
01767   if ( count >= 4 && 0 == ON_ComparePoint( dim, is_rat, p, p+stride*(count-1) ) ) 
01768   {
01769     // a bunch of points piled on top of each other is not considered to be closed.
01770     for ( int i = 1; i < count-1; i++ ) {
01771       if ( ON_ComparePoint( dim, is_rat, p, p+stride*i ) ) {
01772         rc = true;
01773         break;
01774       }
01775     }
01776   }
01777   return rc;
01778 }
01779 
01780 
01781 ON_BOOL32 
01782 ON_IsPointGridClosed(
01783         int dim,
01784         ON_BOOL32 is_rat,
01785         int point_count0, int point_count1,
01786         int point_stride0, int point_stride1,
01787         const double* p,
01788         int dir
01789        )
01790 {
01791   ON_BOOL32 rc = false;
01792   if ( point_count0>0 && point_count1>0 && p != NULL ) {
01793     int count, stride;
01794     const double* p0;
01795     const double* p1;
01796     if ( dir ) {
01797       p0 = p;
01798       p1 = p + (point_count1-1)*point_stride1;
01799       count = point_count0;
01800       stride = point_stride0;
01801     }
01802     else {
01803       p0 = p;
01804       p1 = p + (point_count0-1)*point_stride0;
01805       count = point_count1;
01806       stride = point_stride1;
01807     }
01808     rc = (0==ON_ComparePointList( dim, is_rat, count, stride, p0, stride, p1 ))?true:false;
01809   }
01810   return rc;
01811 }
01812 
01813 
01814 
01815 
01816 int
01817 ON_SolveQuadraticEquation(
01818        double a, double b, double c, 
01819        double *r0, double *r1
01820        )
01821 /* Find solutions of a quadratic equation
01822  *
01823  * INPUT:
01824  *   a, b, c  coefficients defining the quadratic equation
01825  *            a*t^2 + b*t + c = 0
01826  *   r0, r1   address of doubles
01827  * OUTPUT:
01828  *   ON_QuadraticEquation()
01829  *      0: successful - two distinct real roots (*r0 < *r1)
01830  *      1: successful - one real root (*r0 = *r1)
01831  *      2: successful - two complex conjugate roots (*r0 +/- (*r1)*sqrt(-1))
01832  *     -1: failure - a = 0, b != 0        (*r0 = *r1 = -c/b)
01833  *     -2: failure - a = 0, b  = 0 c != 0 (*r0 = *r1 = 0.0)
01834  *     -3: failure - a = 0, b  = 0 c  = 0 (*r0 = *r1 = 0.0)
01835  *
01836  * COMMENTS:
01837  *   The quadratic equation is solved using the formula
01838  *   roots = q/a, c/q, q = 0.5*(b + sgn(b)*sqrt(b^2 - 4ac)).
01839  *
01840  *   When |b^2 - 4*a*c| <= b*b*ON_EPSILON, the discriminant
01841  *   is numerical noise and is assumed to be zero.
01842  *
01843  *   If it is really important to have the best possible answer,
01844  *   you sould probably tune up the returned roots using
01845  *   Brent's algorithm.
01846  *
01847  * REFERENCE:
01848  *   Numerical Recipes in C, section 5.5
01849  *
01850  * RELATED FUNCTIONS:
01851  *   ON_CubicEquation()
01852  */
01853 {
01854   double q, x0, x1, y0, y1, y;
01855 
01856   if (a == 0.0) {
01857     if (b == 0.0) 
01858       {*r0 = *r1 = 0.0; return (c == 0.0) ? -3 : -2;}
01859     *r0 = *r1 = -c/b; return -1;
01860   }
01861 
01862   if (c == 0.0) {
01863     if (b == 0.0) 
01864       {*r0 = *r1 = 0.0; return 1;}
01865     b /= -a;
01866     if (b < 0.0) 
01867       {*r0=b;*r1=0.0;} 
01868     else
01869       {*r0=0.0;*r1=b;}
01870     return 0;
01871   }
01872 
01873   if (b == 0.0) {
01874     c /= -a;
01875     *r1 = sqrt(fabs(c));
01876     if (c < 0.0) 
01877       {*r0 = 0.0; return 2;}
01878     *r0 = -(*r1);
01879     return 0;
01880   }
01881   q = b*b - 4.0*a*c;
01882   if (fabs(q) <= b*b* ON_EPSILON) 
01883     q = 0.0; /* q is noise - set it to zero */
01884   if (q <= 0.0) {
01885     /* multiple real root or complex conjugate roots */
01886     *r0 = -0.5*b/a;
01887     if (q == 0.0) 
01888       {*r1 = *r0; return 1;}
01889 
01890     /* complex conjugate roots (probably) */
01891     *r1 = fabs(0.5*sqrt(fabs(q))/a); 
01892     x0 = *r0;
01893     x1 = *r1;
01894     y = (a*x0 + b)*x0 + c;            /* y = quadratic evaluated at -b/2a */
01895     if ((a > 0.0 && y <= 0.0) || (a < 0.0 && y >= 0.0))
01896       {*r1 = *r0; return 1;}
01897     y0 = y - a*x1*x1;                 /* y0 = real part of "zero" */
01898     y1 = (2.0*a*x0 + b)*x1;           /* y1 = imaginary part of "zero" */
01899     if (fabs(y) <= fabs(y0) || fabs(y) <= fabs(y1)) 
01900       {*r1 = *r0; return 1;}
01901     return 2;
01902   }
01903 
01904   /* distinct roots (probably) */
01905   q = 0.5*(fabs(b) + sqrt(q));
01906   if (b > 0.0) q = -q;
01907   x0 = q/a;
01908   x1 = c/q;
01909   if (x0 == x1) 
01910     {*r0 = *r1 = x0; return 1;}
01911 
01912   if (x0 > x1) 
01913     {y = x0; x0 = x1; x1 = y;}
01914 
01915   /* quick test to see if roots are numerically distinct from extrema */
01916   y = -0.5*b/a;
01917   if (x0 <= y && y <= x1) {
01918     y = (a*y + b)*y + c;              /* y = quadratic evaluated at -b/2a */
01919     y0 = (a*x0 + b)*x0 + c;
01920     y1 = (a*x1 + b)*x1 + c;
01921     if (fabs(y) <= fabs(y0) || fabs(y) <= fabs(y1)
01922         || (a > 0.0 && y > 0.0) || (a < 0.0 && y < 0.0))
01923       {*r0 = *r1 = -0.5*b/a; return 1;}
01924   }
01925 
01926   /* distinct roots */
01927   *r0 = x0;
01928   *r1 = x1;
01929   return 0;
01930 }
01931 
01932 
01933 ON_BOOL32
01934 ON_SolveTriDiagonal( int dim, int n, 
01935                           double* a, const double* b, double* c,
01936                           const double* d, double* X)
01937 /*****************************************************************************
01938 Solve a tridiagonal linear system of equations using backsubstution
01939  
01940 INPUT:
01941   dim   (>=1) dimension of X and d
01942   n     (>=2) number of equations
01943   a,b,c,d
01944         coefficients of the linear system. a and c are arrays of n-1 doubles.
01945         b and d are arrays of n doubles.  Note that "a", "b" and "d" are
01946         not modified. "c" is modified.
01947   X     array of n doubles 
01948 OUTPUT:
01949   ON_SolveTriDiagonal()  0: success
01950                         -1: failure - illegal input
01951                         -2: failure - zero pivot encountered
01952                                       (can happen even when matrix is
01953                                        non-singular)
01954 
01955   X     if ON_SolveTriDiagonal() returns 0, then X is the solution to
01956 
01957   b[0]   c[0]                                X[0]        d[0]
01958   a[0]   b[1]  c[1]                          X[1]        d[1]
01959          a[1]  b[2]  c[2]                  * X[2]     =  d[2]
01960                ....  ....  ....              ...         ...
01961                      a[n-3] b[n-2] c[n-2]    X[n-2]      d[n-2]
01962                             a[n-2] b[n-1]    X[n-1]      d[n-1]
01963 
01964 COMMENTS:
01965   If n <= 3, this function uses ON_Solve2x2() or ON_Solve3x3().  
01966   If n > 3, the system is solved in the fastest possible manner; 
01967   in particular,  no pivoting is performed, b[0] must be nonzero.
01968   If |b[i]| > |a[i-1]| + |c[i]|, then this function will succeed.
01969   The computation is performed in such a way that the output
01970   "X" pointer can be equal to the input "d" pointer; i.e., if the
01971   d array will not be used after the call to ON_SolveTriDiagonal(), then
01972   it is not necessary to allocate seperate space for X and d.
01973 EXAMPLE:
01974 REFERENCE:
01975   NRC, section 2.6
01976 RELATED FUNCTIONS:
01977   ON_Solve2x2
01978   ON_Solve3x3
01979   ON_SolveSVD
01980 *****************************************************************************/
01981 {
01982   double beta, g, q;
01983   int i, j;
01984   if (dim < 1 || n < 2 || !a || !b || !c || !d || !X)
01985     return -1;
01986 
01987   if (dim == 1) {
01988     /* standard tri-diagonal problem -  X and d are scalars */
01989     beta = *b++;
01990     if (beta == 0.0)
01991       return -2;
01992     beta = 1.0/beta;
01993     *X = *d++ *beta;
01994     i = n-1;
01995     while(i--) {
01996       g = (*c++ *= beta);
01997       beta = *b++ - *a * g;
01998       if (beta == 0.0) return -2;
01999       beta = 1.0/beta;
02000       X[1] = (*d++ - *a++ * *X)*beta;
02001       X++;      
02002     }
02003     X--;
02004     c--;
02005     i = n-1;
02006     while(i--) {
02007       *X -= *c-- * X[1];
02008       X--;
02009     }
02010   }
02011   else {
02012     /* X and d are vectors */
02013     beta = *b++;
02014     if (beta == 0.0)
02015       return -2;
02016     beta = 1.0/beta;
02017     j = dim;
02018     while(j--)
02019       *X++ = *d++ *beta;
02020     X -= dim;
02021     i = n-1;
02022     while(i--) {
02023       g = (*c++ *= beta);
02024       beta = *b++ - *a * g;
02025       if (beta == 0.0) return -2;
02026       beta = 1.0/beta;
02027       j = dim;
02028       q = *a++;
02029       while(j--) {
02030         X[dim] = (*d++ - q * *X)*beta;
02031         X++;      
02032       }
02033     }
02034     X--;
02035     c--;
02036     i = n-1;
02037     while(i--) {
02038       q = *c--;
02039       j = dim;
02040       while(j--) {
02041         *X -= q * X[dim];
02042         X--;
02043       }
02044     }
02045   }
02046 
02047   return 0;
02048 }
02049 
02050 
02051 int
02052 ON_Solve2x2( double m00, double m01, double m10, double m11, double d0, double d1,
02053                   double* x_addr, double* y_addr, double* pivot_ratio)
02054 /* Solve a 2x2 system of linear equations
02055  *
02056  * INPUT:
02057  *   m00, m01, m10, m11, d0, d1
02058  *      coefficients for the 2x2 the linear system:
02059  *   x_addr, y_addr
02060  *      addresses of doubles
02061  *   pivot_ratio
02062  *      address of double
02063  * OUTPUT:
02064  *   ON_Solve2x2() returns rank (0,1,2)
02065  *
02066  *   If ON_Solve2x2() is successful (return code 2), then
02067  *   the solution is returned in {*x_addr, *y_addr} and
02068  *   *pivot_ratio = min(|pivots|)/max(|pivots|).
02069  *
02070  *   WARNING: If the pivot ratio is small, then the matrix may
02071  *   be singular or ill conditioned.  You should test the results
02072  *   before you use them.
02073  *
02074  * COMMENTS:
02075  *      The system of 2 equations and 2 unknowns (x,y),
02076  *         m00*x + m01*y = d0
02077  *         m10*x + m11*y = d1,
02078  *      is solved using Gauss-Jordan elimination
02079  *      with full pivoting.
02080  * EXAMPLE:
02081  *      // Find the intersection of 2 2D lines where
02082  *      // P0, P1  are points on the lines and
02083  *      // D0, D1, are nonzero directions
02084  *      rc = ON_Solve2x2(D0[0],-D1[0],D0[1],-D1[1],P1[0]-P0[0],P1[1]-P0[1],
02085  *                       &x, &y,&pivot_ratio);
02086  *      switch(rc) {
02087  *      case  0: // P0 + x*D0 = P1 + y*D1 = intersection point
02088  *        if (pivot_ratio < 0.001) {
02089  *          // small pivot ratio - test answer before using ...
02090  *        }
02091  *        break;
02092  *      case -1: // both directions are zero!
02093  *        break;
02094  *      case -2: // parallel directions
02095  *        break;
02096  *      }
02097  *
02098  * REFERENCE:
02099  *      STRANG
02100  *
02101  * RELATED FUNCTIONS:
02102  *      ON_Solve3x2(), ON_Solve3x3
02103  */
02104 {
02105   int i = 0;
02106   double maxpiv, minpiv;
02107   double x = fabs(m00);
02108   double y = fabs(m01); if (y > x) {x = y; i = 1;}
02109   y = fabs(m10); if (y > x) {x = y; i = 2;}
02110   y = fabs(m11); if (y > x) {x = y; i = 3;}
02111   *pivot_ratio = *x_addr = *y_addr = 0.0;
02112   if (x == 0.0) 
02113     return 0; // rank = 0
02114   minpiv = maxpiv = x;
02115   if (i%2) {
02116     {double* tmp = x_addr; x_addr = y_addr; y_addr = tmp;}
02117     x = m00; m00 = m01; m01 = x;
02118     x = m10; m10 = m11; m11 = x;
02119   }
02120   if (i > 1) {
02121     x = d0; d0 = d1; d1 = x;
02122     x = m00; m00 = m10; m10 = x;
02123     x = m01; m01 = m11; m11 = x;
02124   }
02125   
02126   x = 1.0/m00;
02127   m01 *= x; d0 *= x;
02128   if (m10 != 0.0) {m11 -= m10*m01; d1 -= m10*d0;}
02129 
02130   if (m11 == 0.0) 
02131     return 1; // rank = 1
02132 
02133   y = fabs(m11);
02134   if (y > maxpiv) maxpiv = y; else if (y < minpiv) minpiv = y;
02135   
02136   d1 /= m11;
02137   if (m01 != 0.0)
02138     d0 -= m01*d1;
02139 
02140   *x_addr = d0;
02141   *y_addr = d1;
02142   *pivot_ratio = minpiv/maxpiv;
02143   return 2;  
02144 }
02145 
02146 
02147 int 
02148 ON_Solve3x2(const double col0[3], const double col1[3], 
02149                 double d0, double d1, double d2,
02150                 double* x_addr, double* y_addr, double* err_addr, double* pivot_ratio)
02151 /* Solve a 3x2 system of linear equations
02152  *
02153  * INPUT:
02154  *   col0, col1
02155  *      arrays of 3 doubles
02156  *   d0, d1, d2
02157  *      right hand column of system
02158  *   x_addr, y_addr, err_addr, pivot_ratio
02159  *      addresses of doubles
02160  * OUTPUT:
02161  *   TL_Solve3x2()
02162  *       2: successful
02163  *       0: failure - 3x2 matrix has rank 0
02164  *       1: failure - 3x2 matrix has rank 1
02165  *      If the return code is zero, then
02166  *        (*x_addr)*{col0} + (*y_addr)*{col1}
02167  *        + (*err_addr)*({col0 X col1}/|col0 X col1|)
02168  *        = {d0,d1,d2}.
02169  *      pivot_ratio = min(|pivots|)/max(|pivots|)  If this number
02170  *      is small, then the 3x2 matrix may be singular or ill 
02171  *      conditioned.
02172  * COMMENTS:
02173  *      The system of 3 equations and 2 unknowns (x,y),
02174  *              x*col0[0] + y*col1[1] = d0
02175  *              x*col0[0] + y*col1[1] = d1
02176  *              x*col0[0] + y*col1[1] = d2,
02177  *      is solved using Gauss-Jordan elimination
02178  *      with full pivoting.
02179  * EXAMPLE:
02180  *      // If A, B and T are 3D vectors, find a and b so that
02181  *      // T - a*A + b*B is perpendicular to both A and B.
02182  *      rc = TL_Solve3x3(A,B,T[0],T[1],T[2],&a,&b,&len);
02183  *      switch(rc) {
02184  *      case  0: // {x,y,z} = intersection point, len = T o (A X B / |A X B|)
02185  *        break;
02186  *      case -1: // both A and B are zero!
02187  *        break;
02188  *      case -2: // A and B are parallel, or one of A and B is zero.
02189  *        break;
02190  *      }
02191  * REFERENCE:
02192  *      STRANG
02193  * RELATED FUNCTIONS:
02194  *      ON_Solve2x2, ON_Solve3x3,
02195  */
02196 {
02197   /* solve 3x2 linear system using Gauss-Jordan elimination with
02198    * full pivoting.  Input columns not modified.
02199    * returns 0: rank 0, 1: rank 1, 2: rank 2
02200    *         *err = signed distance from (d0,d1,d2) to plane
02201    *                through origin with normal col0 X col1.
02202    */
02203   int i;
02204   double x, y;
02205   ON_3dVector c0, c1;
02206 
02207   *x_addr = *y_addr = *pivot_ratio = 0.0;
02208   *err_addr = ON_DBL_MAX;
02209   i = 0;
02210   x = fabs(col0[0]);
02211   y = fabs(col0[1]); if (y>x) {x = y; i = 1;}
02212   y = fabs(col0[2]); if (y>x) {x = y; i = 2;}
02213   y = fabs(col1[0]); if (y>x) {x = y; i = 3;}
02214   y = fabs(col1[1]); if (y>x) {x = y; i = 4;}
02215   y = fabs(col1[2]); if (y>x) {x = y; i = 5;}
02216   if (x == 0.0) return 0;
02217   *pivot_ratio = fabs(x);
02218   if (i >= 3) {
02219     /* swap columns */
02220     double* ptr = x_addr; x_addr = y_addr; y_addr = ptr;
02221     c0 = col1;
02222     c1 = col0;
02223   }
02224   else {
02225     c0 = col0;
02226     c1 = col1;
02227   }
02228 
02229   switch((i%=3)) {
02230   case 1: /* swap rows 0 and 1*/
02231     x=c0.y;c0.y=c0.x;c0.x=x;
02232     x=c1.y;c1.y=c1.x;c1.x=x;
02233     x=d1;d1=d0;d0=x;
02234     break;
02235   case 2: /* swap rows 0 and 2*/
02236     x=c0.z;c0.z=c0.x;c0.x=x;
02237     x=c1.z;c1.z=c1.x;c1.x=x;
02238     x=d2;d2=d0;d0=x;
02239     break;
02240   }
02241 
02242   c1.x /= c0.x; d0 /= c0.x;
02243   x = -c0.y; if (x != 0.0) {c1.y += x*c1.x; d1 += x*d0;}
02244   x = -c0.z; if (x != 0.0) {c1.z += x*c1.x; d2 += x*d0;}
02245 
02246   if (fabs(c1.y) > fabs(c1.z)) {
02247     if (fabs(c1.y) > *pivot_ratio)
02248       *pivot_ratio /= fabs(c1.y); 
02249     else 
02250       *pivot_ratio = fabs(c1.y)/ *pivot_ratio;
02251     d1 /= c1.y;
02252     x = -c1.x; if (x != 0.0) d0 += x*d1;
02253     x = -c1.z; if (x != 0.0) d2 += x*d1;
02254     *x_addr = d0;
02255     *y_addr = d1;
02256     *err_addr = d2;
02257   }
02258   else if (c1.z == 0.0) 
02259     return 1; /* 3x2 matrix has rank = 1 */
02260   else {
02261     if (fabs(c1.z) > *pivot_ratio)
02262       *pivot_ratio /= fabs(c1.z); 
02263     else 
02264       *pivot_ratio = fabs(c1.z)/ *pivot_ratio;
02265     d2 /= c1.z;
02266     x = -c1.x; if (x != 0.0) d0 += x*d2;
02267     x = -c1.y; if (x != 0.0) d1 += x*d2;
02268     *x_addr = d0;
02269     *err_addr = d1;
02270     *y_addr = d2;
02271   }
02272 
02273   return 2;
02274 }
02275 
02276 double ON_SolveNxN(bool bFullPivot, bool bNormalize, int n, double* M[], double B[], double X[])
02277 {
02278   if ( n <= 0 || 0 == M || 0 == B || 0 == X )
02279     return 0.0;
02280 
02281   int i,j,maxi,maxj,n0, Xdex_buffer[64];
02282   double x,minpivot=0.0,maxpivot=1.0,*p;
02283   int* Xdex = 0;
02284   
02285   if ( bNormalize )
02286   {
02287     for ( i = 0; i < n; i++ )
02288     {
02289       x = 0.0;
02290       for ( j = 0; j < n; j++ )
02291       {
02292         x += M[i][j]*M[i][j];
02293       }
02294       if ( x > 0.0 )
02295       {
02296         x = 1.0/sqrt(x);
02297         B[i] *= x;
02298         for ( j = 0; j < n; j++ )
02299           M[i][j] *= x;
02300       }
02301     }
02302   }
02303 
02304   if ( bFullPivot )
02305   {
02306     // The Xdex_buffer[] hoo haa is here to avoid an potentially time
02307     // consuming call to heap services when the matrix is small.
02308     // When n > 64 the numerical portion of the computation is 
02309     // long enough that the time to call onmalloc() is negligable.
02310     // (When n > 10-ish, this calculation is likely to return junk
02311     // unless you have a special case matrix, in which case this
02312     // function will be much slower than one that is designed to
02313     // takes advantage of the special case.
02314     Xdex = (n <= (int)((sizeof(Xdex_buffer)/sizeof(Xdex_buffer[0]))))
02315          ? &Xdex_buffer[0] 
02316          : (int*)onmalloc(n*sizeof(Xdex[0]));
02317     for ( i = 0; i < n; i++ )
02318       Xdex[i] = i;
02319   }
02320 
02321   // reduce system of equations to upper triangular
02322   for (n0=0; n0<n; n0++)
02323   {
02324     // find pivot = biggest entry in sub-matrix
02325     maxi=n0;
02326     maxj=n0;
02327     x=0.0;
02328     for(j=n0;j<n;j++) 
02329     {
02330       for(i=n0;i<n;i++)  
02331       {
02332         if ( fabs(M[i][j]) > x )
02333         {
02334           x=fabs(M[i][j]);
02335           maxi=i;
02336           maxj=j;
02337         }
02338       }
02339       if ( !bFullPivot )
02340         break;
02341     }
02342     if ( 0.0==x )
02343     {
02344       // system of equations is degenerate
02345       // Return -(rank of M) (M has rank n0)
02346       if ( 0 != Xdex && Xdex != &Xdex_buffer[0] )
02347         onfree(Xdex);
02348       return -n0;
02349     }
02350     else if ( 0==n0 )
02351     {
02352       minpivot=x;
02353       maxpivot=x;
02354     }
02355     else if (x < minpivot)
02356       minpivot=x;
02357     else if  (x > maxpivot)
02358       maxpivot=x;
02359 
02360     // put pivot in M[n0][n0]
02361     if ( n0 != maxi )
02362     {
02363       // swap rows n0 and maxi
02364       p = M[n0];M[n0]=M[maxi];M[maxi]=p;
02365       x=B[n0];B[n0]=B[maxi];B[maxi]=x;
02366     }
02367     if ( n0 != maxj )
02368     {
02369       // swap columns n0 and maxj
02370       for (i=0;i<n;i++)
02371       {
02372         x=M[i][n0];M[i][n0]=M[i][maxj];M[i][maxj]=x;
02373       }
02374       j=Xdex[n0];Xdex[n0]=Xdex[maxj];Xdex[maxj]=j;
02375     }
02376 
02377     // divide row n0 by M[n0][n0] to unitize M[n0][n0]
02378     x = 1.0/M[n0][n0];
02379     //M[n0][n0] = 1.0; // cosmetic because M[n0][n0] will never be used again
02380     B[n0] *= x;
02381     for (j=n0+1;j<n;j++) 
02382       M[n0][j] *= x;
02383 
02384     // For each i > n0, replace row[i] with
02385     // row[i] - M[i][n0]*row[n0] to zero out M[i>n0][n0]
02386     for ( i = n0+1; i < n; i++ )
02387     {
02388       x = -M[i][n0];
02389       if ( 0.0 != x )
02390       {
02391         //M[i][n0] = 0.0; // cosmetic because M[i][n0] will never be used again
02392         B[i] += x*B[n0];
02393         for ( j = n0+1; j < n; j++ )
02394         {
02395           M[i][j] += x*M[n0][j];
02396         }
02397       }
02398     }
02399   }
02400 
02401   // At this point M is upper triangular with 1's
02402   // on its diagonal.  Backsolve.
02403   for ( j = n-1; j >= 0; j-- )
02404   {
02405     for ( i = 0; i < j; i++ )
02406     {
02407       x = -M[i][j];
02408       if ( x != 0 )
02409       {
02410         B[i] += x*B[j];
02411         //M[i][j] += x; // cosmetic because M[i][j] will never be used again
02412       }
02413     }
02414   }
02415 
02416   // solution is now in B
02417   if ( bFullPivot )
02418   {
02419     for(i=0;i<n;i++)
02420       X[Xdex[i]] = B[i];
02421 
02422     if ( 0 != Xdex && Xdex != &Xdex_buffer[0] )
02423       onfree(Xdex);
02424   }
02425   else
02426   {
02427     memcpy(&X[0],&B[0],n*sizeof(X[0]));
02428   }
02429 
02430   return minpivot/maxpivot;
02431 }
02432 
02433 
02434 int
02435 ON_Solve4x4(const double row0[4], const double row1[4], const double row2[4], const double row3[4],
02436                 double d0, double d1, double d2, double d3,
02437                 double* x_addr, double* y_addr, double* z_addr, double* w_addr,
02438                 double* pivot_ratio)
02439 {
02440   /* Solve a 4x4 linear system using Gauss-Jordan elimination 
02441    * with full pivoting.
02442    */
02443   int i, j;
02444   double *p0, *p1, *p2, *p3, *p;
02445   double x, y, workarray[20], maxpiv, minpiv;
02446 
02447   const int sizeof_row = 4*sizeof(row0[4]);
02448 
02449   *pivot_ratio = *x_addr = *y_addr = *z_addr = *w_addr = 0.0;
02450   x = fabs(row0[0]); i=j=0;
02451   y = fabs(row0[1]); if (y>x) {x=y;j=1;}
02452   y = fabs(row0[2]); if (y>x) {x=y;j=2;}
02453   y = fabs(row0[3]); if (y>x) {x=y;j=3;}
02454 
02455   y = fabs(row1[0]); if (y>x) {x=y;i=1;j=0;}
02456   y = fabs(row1[1]); if (y>x) {x=y;i=1;j=1;}
02457   y = fabs(row1[2]); if (y>x) {x=y;i=1;j=2;}
02458   y = fabs(row1[3]); if (y>x) {x=y;i=1;j=3;}
02459 
02460   y = fabs(row2[0]); if (y>x) {x=y;i=2;j=0;}
02461   y = fabs(row2[1]); if (y>x) {x=y;i=2;j=1;}
02462   y = fabs(row2[2]); if (y>x) {x=y;i=2;j=2;}
02463   y = fabs(row2[3]); if (y>x) {x=y;i=2;j=3;}
02464 
02465   y = fabs(row3[0]); if (y>x) {x=y;i=3;j=0;}
02466   y = fabs(row3[1]); if (y>x) {x=y;i=3;j=1;}
02467   y = fabs(row3[2]); if (y>x) {x=y;i=3;j=2;}
02468   y = fabs(row3[3]); if (y>x) {x=y;i=3;j=3;}
02469 
02470   if (x == 0.0) 
02471     return 0; // rank = 0
02472 
02473   maxpiv = minpiv = x;
02474   p0 = workarray;
02475   p1 = p0+5;
02476   p2 = p1+5;
02477   p3 = p2+5;
02478   switch(i) 
02479   {
02480   case 1: /* swap rows 0 and 1 */
02481     memcpy(p0,row1,sizeof_row); p0[4] = d1;
02482     memcpy(p1,row0,sizeof_row); p1[4] = d0;
02483     memcpy(p2,row2,sizeof_row); p2[4] = d2;
02484     memcpy(p3,row3,sizeof_row); p3[4] = d3;
02485     break;
02486   case 2: /* swap rows 0 and 2 */
02487     memcpy(p0,row2,sizeof_row); p0[4] = d2;
02488     memcpy(p1,row1,sizeof_row); p1[4] = d1;
02489     memcpy(p2,row0,sizeof_row); p2[4] = d0;
02490     memcpy(p3,row3,sizeof_row); p3[4] = d3;
02491     break;
02492   case 3: /* swap rows 0 and 3 */
02493     memcpy(p0,row3,sizeof_row); p0[4] = d3;
02494     memcpy(p1,row1,sizeof_row); p1[4] = d1;
02495     memcpy(p2,row2,sizeof_row); p2[4] = d2;
02496     memcpy(p3,row0,sizeof_row); p3[4] = d0;
02497     break;
02498   default:
02499     memcpy(p0,row0,sizeof_row); p0[4] = d0;
02500     memcpy(p1,row1,sizeof_row); p1[4] = d1;
02501     memcpy(p2,row2,sizeof_row); p2[4] = d2;
02502     memcpy(p3,row3,sizeof_row); p3[4] = d3;
02503     break;
02504   }
02505 
02506   switch(j) 
02507   {
02508   case 1: /* swap columns 0 and 1 */
02509     p = x_addr; x_addr = y_addr; y_addr = p;
02510     x = p0[0]; p0[0]=p0[1]; p0[1]=x;
02511     x = p1[0]; p1[0]=p1[1]; p1[1]=x;
02512     x = p2[0]; p2[0]=p2[1]; p2[1]=x;
02513     x = p3[0]; p3[0]=p3[1]; p3[1]=x;
02514     break;
02515   case 2: /* swap columns 0 and 2 */
02516     p = x_addr; x_addr = z_addr; z_addr = p;
02517     x = p0[0]; p0[0]=p0[2]; p0[2]=x;
02518     x = p1[0]; p1[0]=p1[2]; p1[2]=x;
02519     x = p2[0]; p2[0]=p2[2]; p2[2]=x;
02520     x = p3[0]; p3[0]=p3[2]; p3[2]=x;
02521     break;
02522   case 3: /* swap columns 0 and 3 */
02523     p = x_addr; x_addr = w_addr; w_addr = p;
02524     x = p0[0]; p0[0]=p0[3]; p0[3]=x;
02525     x = p1[0]; p1[0]=p1[3]; p1[3]=x;
02526     x = p2[0]; p2[0]=p2[3]; p2[3]=x;
02527     x = p3[0]; p3[0]=p3[3]; p3[3]=x;
02528     break;
02529   }
02530 
02532   //
02533   //  p0 = M * * *   *
02534   //  p1 = ~ * * *   *
02535   //  p2 = ~ * * *   *
02536   //  p3 = ~ * * *   *
02537   //
02538   x = 1.0/p0[0];
02539   /* debugger set p0[0] = 1 */
02540   p0[1] *= x; p0[2] *= x; p0[3] *= x; p0[4] *= x;
02541 
02542   x = -p1[0];
02543   /* debugger set p1[0] = 0 */
02544   if (x != 0.0) 
02545   {
02546     p1[1] += x*p0[1]; p1[2] += x*p0[2]; p1[3] += x*p0[3]; p1[4] += x*p0[4];
02547   }
02548 
02549   x = -p2[0];
02550   if (x != 0.0) 
02551   {
02552     /* debugger set p2[0] = 0 */
02553     p2[1] += x*p0[1]; p2[2] += x*p0[2]; p2[3] += x*p0[3]; p2[4] += x*p0[4];
02554   }
02555 
02556   x = -p3[0];
02557   if (x != 0.0) 
02558   {
02559     /* debugger set p3[0] = 0 */
02560     p3[1] += x*p0[1]; p3[2] += x*p0[2]; p3[3] += x*p0[3]; p3[4] += x*p0[4];
02561   }
02562 
02564   //
02565   //  p0 = 1 * * *   *
02566   //  p1 = 0 * * *   *
02567   //  p2 = 0 * * *   *
02568   //  p3 = 0 * * *   *
02569   //
02570   x = fabs(p1[1]); i=j=0;
02571   y = fabs(p1[2]); if (y>x) {x=y;j=1;}
02572   y = fabs(p1[3]); if (y>x) {x=y;j=2;}
02573 
02574   y = fabs(p2[1]); if (y>x) {x=y;i=1;j=0;}
02575   y = fabs(p2[2]); if (y>x) {x=y;i=1;j=1;}
02576   y = fabs(p2[3]); if (y>x) {x=y;i=1;j=2;}
02577 
02578   y = fabs(p3[1]); if (y>x) {x=y;i=2;j=0;}
02579   y = fabs(p3[2]); if (y>x) {x=y;i=2;j=1;}
02580   y = fabs(p3[3]); if (y>x) {x=y;i=2;j=2;}
02581   if (x == 0.0) 
02582   {
02583     *x_addr = p0[4];
02584     return 1; // rank = 1;
02585   }
02586 
02587   if (x > maxpiv) maxpiv = x; else if (x < minpiv) minpiv = x;
02588   if ( 1 == j )
02589   {
02590     /* swap columns 1 and 2 */
02591     x = p0[1]; p0[1] = p0[2]; p0[2] = x;
02592     x = p1[1]; p1[1] = p1[2]; p1[2] = x;
02593     x = p2[1]; p2[1] = p2[2]; p2[2] = x;
02594     x = p3[1]; p3[1] = p3[2]; p3[2] = x;
02595     p = y_addr; y_addr = z_addr; z_addr = p;
02596   }
02597   else if ( 2 == j )
02598   {
02599     /* swap columns 1 and 3 */
02600     x = p0[1]; p0[1] = p0[3]; p0[3] = x;
02601     x = p1[1]; p1[1] = p1[3]; p1[3] = x;
02602     x = p2[1]; p2[1] = p2[3]; p2[3] = x;
02603     x = p3[1]; p3[1] = p3[3]; p3[3] = x;
02604     p = y_addr; y_addr = w_addr; w_addr = p;
02605   }
02606 
02607   if (1 == i) 
02608   {
02609     /* swap rows 1 and 2 */
02610     p = p1; p1 = p2; p2 = p;
02611   }
02612   else if (2 == i) 
02613   {
02614     /* swap rows 1 and 3 */
02615     p = p1; p1 = p3; p3 = p;
02616   }
02617 
02619   //
02620   //  p0 = 1 * * *   *
02621   //  p1 = 0 M * *   *
02622   //  p2 = 0 ~ * *   *
02623   //  p3 = 0 ~ * *   *
02624   //
02625   x = 1.0/p1[1];
02626   /* debugger set p1[1] = 1 */
02627   p1[2] *= x; p1[3] *= x; p1[4] *= x;
02628 
02629   x = -p2[1];
02630   if (x != 0.0) 
02631   {
02632     /* debugger set p2[1] = 0 */
02633     p2[2] += x*p1[2]; p2[3] += x*p1[3]; p2[4] += x*p1[4];
02634   }
02635 
02636   x = -p3[1];
02637   if (x != 0.0) 
02638   {
02639     /* debugger set p3[1] = 0 */
02640     p3[2] += x*p1[2]; p3[3] += x*p1[3]; p3[4] += x*p1[4];
02641   }
02642 
02644   //
02645   //  p0 = 1 * * *   *
02646   //  p1 = 0 1 * *   *
02647   //  p2 = 0 0 * *   *
02648   //  p3 = 0 0 * *   *
02649   //
02650   x = fabs(p2[2]);i=j=0;
02651   y = fabs(p2[3]);if (y>x) {x=y;j=1;}
02652   y = fabs(p3[2]);if (y>x) {x=y;i=1;j=0;}
02653   y = fabs(p3[3]);if (y>x) {x=y;i=j=1;}
02654   if (x == 0.0) 
02655   {
02656     *y_addr = p2[4];
02657     *x_addr = p0[4] - p0[1]*(*y_addr);
02658     return 2; // rank = 2;
02659   }
02660   if (x > maxpiv) maxpiv = x; else if (x < minpiv) minpiv = x;
02661   if (j) 
02662   {
02663     /* swap columns 2 and 3 */
02664     x = p0[2]; p0[2] = p0[3]; p0[3] = x;
02665     x = p1[2]; p1[2] = p1[3]; p1[3] = x;
02666     x = p2[2]; p2[2] = p2[3]; p2[3] = x;
02667     x = p3[2]; p3[2] = p3[3]; p3[3] = x;
02668     p = z_addr; z_addr = w_addr; w_addr = p;
02669   }
02670 
02671   if (i) 
02672   {
02673     /* swap rows 2 and 3 */
02674     p = p2;
02675     p2 = p3;
02676     p3 = p;
02677   }
02678 
02680   //
02681   //  p0 = 1 * * *   *
02682   //  p1 = 0 1 * *   *
02683   //  p2 = 0 0 M *   *
02684   //  p3 = 0 0 ~ *   *
02685   //
02686 
02687   /* debugger set p2[2] = 1 */
02688   x = 1.0/p2[2]; p2[3] *= x; p2[4] *= x;
02689   x = - p3[2];
02690   if (x != 0.0) 
02691   {
02692     /* debugger set p3[2] = 0 */
02693     p3[3] += x*p2[3]; p3[4] += x*p2[4];
02694   }
02696   //
02697   //  p0 = 1 * * *   *
02698   //  p1 = 0 1 * *   *
02699   //  p2 = 0 0 1 *   *
02700   //  p3 = 0 0 0 *   *
02701   //
02702 
02703   x = fabs(p3[3]);
02704   if ( x == 0.0 )
02705   {
02706     *z_addr = p2[4];
02707     *y_addr = p1[4] - p1[2]*(*z_addr);
02708     *x_addr = p0[4] - p0[1]*(*y_addr) - p0[2]*(*z_addr);
02709     return 3; // rank = 3
02710   }
02711   if (x > maxpiv) maxpiv = x; else if (x < minpiv) minpiv = x;
02712 
02713   // backsolve in a that optimizers can use cache/registers
02714   p3[4] /= p3[3];
02715   p2[4] -= p2[3]*p3[4];
02716   p1[4] -= p1[2]*p2[4] + p1[3]*p3[4];
02717   p0[4] -= p1[4]*p0[1] + p2[4]*p0[2] + p3[4]*p0[3];
02718 
02719   // return answer
02720   *x_addr = p0[4];
02721   *y_addr = p1[4];
02722   *z_addr = p2[4];
02723   *w_addr = p3[4];
02724   *pivot_ratio = minpiv/maxpiv;
02725 
02726   return 4; // rank = 4
02727 }
02728 
02729 
02730 
02731 
02732 int
02733 ON_Solve3x3(const double row0[3], const double row1[3], const double row2[3],
02734                 double d0, double d1, double d2,
02735                 double* x_addr, double* y_addr, double* z_addr,
02736                 double* pivot_ratio)
02737 {
02738   /* Solve a 3x3 linear system using Gauss-Jordan elimination 
02739    * with full pivoting.
02740    */
02741   int i, j;
02742   double* p0;
02743   double* p1;
02744   double* p2;
02745   double x, y, workarray[12], maxpiv, minpiv;
02746 
02747   const int sizeof_row = 3*sizeof(row0[0]);
02748 
02749   *pivot_ratio = *x_addr = *y_addr = *z_addr = 0.0;
02750   x = fabs(row0[0]); i=j=0;
02751   y = fabs(row0[1]); if (y>x) {x=y;j=1;}
02752   y = fabs(row0[2]); if (y>x) {x=y;j=2;}
02753   y = fabs(row1[0]); if (y>x) {x=y;i=1;j=0;}
02754   y = fabs(row1[1]); if (y>x) {x=y;i=1;j=1;}
02755   y = fabs(row1[2]); if (y>x) {x=y;i=1;j=2;}
02756   y = fabs(row2[0]); if (y>x) {x=y;i=2;j=0;}
02757   y = fabs(row2[1]); if (y>x) {x=y;i=2;j=1;}
02758   y = fabs(row2[2]); if (y>x) {x=y;i=2;j=2;}
02759   if (x == 0.0) 
02760     return 0;
02761   maxpiv = minpiv = fabs(x);
02762   p0 = workarray;
02763   switch(i) {
02764   case 1: /* swap rows 0 and 1 */
02765     memcpy(p0,row1,sizeof_row); p0[3] = d1; p0 += 4;
02766     memcpy(p0,row0,sizeof_row); p0[3] = d0; p0 += 4;
02767     memcpy(p0,row2,sizeof_row); p0[3] = d2;
02768     break;
02769   case 2: /* swap rows 0 and 2 */
02770     memcpy(p0,row2,sizeof_row); p0[3] = d2; p0 += 4;
02771     memcpy(p0,row1,sizeof_row); p0[3] = d1; p0 += 4;
02772     memcpy(p0,row0,sizeof_row); p0[3] = d0;
02773     break;
02774   default:
02775     memcpy(p0,row0,sizeof_row); p0[3] = d0; p0 += 4;
02776     memcpy(p0,row1,sizeof_row); p0[3] = d1; p0 += 4;
02777     memcpy(p0,row2,sizeof_row); p0[3] = d2;
02778     break;
02779   }
02780   switch(j) {
02781   case 1: /* swap columns 0 and 1 */
02782     p0 = x_addr; x_addr = y_addr; y_addr = p0;
02783     p0 = &workarray[0]; 
02784     x = p0[0]; p0[0]=p0[1]; p0[1]=x; p0 += 4;
02785     x = p0[0]; p0[0]=p0[1]; p0[1]=x; p0 += 4;
02786     x = p0[0]; p0[0]=p0[1]; p0[1]=x;
02787     break;
02788   case 2: /* swap columns 0 and 2 */
02789     p0 = x_addr; x_addr = z_addr; z_addr = p0;
02790     p0 = &workarray[0]; 
02791     x = p0[0]; p0[0]=p0[2]; p0[2]=x; p0 += 4;
02792     x = p0[0]; p0[0]=p0[2]; p0[2]=x; p0 += 4;
02793     x = p0[0]; p0[0]=p0[2]; p0[2]=x;
02794     break;
02795   }
02796 
02797   x = 1.0/workarray[0];
02798   /* debugger set workarray[0] = 1 */
02799   p0 = p1 = workarray + 1;
02800   *p1++ *= x; *p1++ *= x; *p1++ *= x;
02801   x = -(*p1++);
02802   /* debugger set workarray[4] = 0 */
02803   if (x == 0.0) 
02804     p1 += 3;
02805   else 
02806     {*p1++ += x*(*p0++); *p1++ += x*(*p0++); *p1++ += x*(*p0); p0 -= 2;}
02807   x = -(*p1++);
02808   /* debugger set workarray[8] = 0 */
02809   if (x != 0.0)
02810     {*p1++ += x*(*p0++); *p1++ += x*(*p0++); *p1++ += x*(*p0); p0 -= 2;}
02811 
02812   x = fabs(workarray[ 5]);i=j=0;
02813   y = fabs(workarray[ 6]);if (y>x) {x=y;j=1;}
02814   y = fabs(workarray[ 9]);if (y>x) {x=y;i=1;j=0;}
02815   y = fabs(workarray[10]);if (y>x) {x=y;i=j=1;}
02816   if (x == 0.0) 
02817     return 1; // rank = 1;
02818   y = fabs(x);
02819   if (y > maxpiv) maxpiv = y; else if (y < minpiv) minpiv = y;
02820   if (j) {
02821     /* swap columns 1 and 2 */
02822     p0 = workarray+1;
02823     p1 = p0+1;
02824     x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4;
02825     x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4;
02826     x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4;
02827     p0 = y_addr; y_addr = z_addr; z_addr = p0;
02828   }
02829 
02830   if (i) {
02831     /* pivot is in row 2 */
02832     p0 = workarray+1;
02833     p1 = p0 + 8;
02834     p2 = p0 + 4;
02835   }
02836   else {
02837     /* pivot is in row 1 */
02838     p0 = workarray+1;
02839     p1 = p0 + 4;
02840     p2 = p0 + 8;
02841   }
02842 
02843   /* debugger set workarray[5+4*i] = 1 */
02844   x = 1.0/(*p1++); *p1++ *= x; *p1 *= x; p1--;
02845   x = -(*p0++);
02846   /* debugger set p0[-1] = 0 */
02847   if (x != 0.0) {*p0++ += x*(*p1++); *p0 += x*(*p1); p0--; p1--;}
02848   x = -(*p2++);
02849   /* debugger set p2[-1] = 0 */
02850   if (x != 0.0) {*p2++ += x*(*p1++); *p2 += x*(*p1); p2--; p1--;}
02851   x = *p2++;
02852   if (x == 0.0) 
02853     return 2; // rank = 2;
02854   y = fabs(x);
02855   if (y > maxpiv) maxpiv = y; else if (y < minpiv) minpiv = y;
02856   /* debugger set p2[-1] = 1 */
02857   *p2 /= x;
02858   x = -(*p1++);  if (x != 0.0) *p1 += x*(*p2);
02859   /* debugger set p1[-1] = 0 */
02860   x = -(*p0++);  if (x != 0.0) *p0 += x*(*p2);
02861   /* debugger set p0[-1] = 0 */
02862   *x_addr = workarray[3];
02863   if (i) {
02864     *y_addr = workarray[11];
02865     *z_addr = workarray[7];
02866   }
02867   else {
02868     *y_addr = workarray[7];
02869     *z_addr = workarray[11];
02870   }
02871   *pivot_ratio = minpiv/maxpiv;
02872   return 3;
02873 }
02874 
02875 
02876 
02877 struct tagON_SORT_CONTEXT
02878 {
02879   void* users_context;
02880   const unsigned char* qdata;
02881   int (*compar2)(const void*,const void*);
02882   int (*compar3)(const void*,const void*,void*);
02883 };
02884 
02885 static int qicompar2(void* p, const void* a, const void* b)
02886 {
02887   return ((struct tagON_SORT_CONTEXT*)p)->compar2(
02888     ((struct tagON_SORT_CONTEXT*)p)->qdata + *((unsigned int*)a),
02889     ((struct tagON_SORT_CONTEXT*)p)->qdata + *((unsigned int*)b)
02890     );
02891 }
02892 
02893 static int qicompar3(void* p, const void* a, const void* b)
02894 {
02895   return ((struct tagON_SORT_CONTEXT*)p)->compar3(
02896     ((struct tagON_SORT_CONTEXT*)p)->qdata + *((unsigned int*)a),
02897     ((struct tagON_SORT_CONTEXT*)p)->qdata + *((unsigned int*)b),
02898     ((struct tagON_SORT_CONTEXT*)p)->users_context
02899     );
02900 }
02901 
02902 void
02903 ON_Sort( ON::sort_algorithm method, 
02904          int* index, 
02905          const void* data, 
02906          size_t count, 
02907          size_t sizeof_element, 
02908          int (*compar)(const void*,const void*)
02909          )
02910 /*****************************************************************************
02911 Sort an index
02912  
02913 INPUT:
02914   method
02915     ON::quick_sort (generally best) or ON::heap_sort
02916   data
02917     pointer passed to compar() function
02918   index, count
02919     index is array of count integers. This function takes care of initializing 
02920     the input array.
02921   int compar(void* data, int i, int j)   ( 0 <= i,j < count )
02922     function that returns 
02923       -1: datum i is less than datum j
02924        0: datum i is equal to datum j
02925        1: datum i is greater than datum j
02926 OUTPUT:
02927   index
02928     Sorted so that datum at index[i] is less than or equal to datum at index[i+1]  
02929 COMMENTS:
02930   If you want the data sorted in place, then use ON_qsort() or ON_hsort().
02931 EXAMPLE:
02932   // ...
02933 REFERENCE:
02934   KNUTH
02935 */
02936 {
02937   tagON_SORT_CONTEXT context;
02938   unsigned int* idx;
02939   const void* tmp;
02940   unsigned int i, j, k, tmpi, icount, isizeof_element;
02941   
02942   if (count < 1 || 0 == index || sizeof_element <= 0) 
02943   {
02944     return;
02945   }
02946   if (1 == count) 
02947   {
02948     index[0]=0;
02949     return;
02950   }
02951 
02952   isizeof_element = (unsigned int)sizeof_element; // (int) converts 64 bit size_t
02953   icount = (unsigned int)count;
02954   idx = (unsigned int*)index; // convert to unsigned int
02955   
02956   for ( i = 0; icount--; i += isizeof_element )
02957   {
02958     *idx++ = i;
02959   }
02960 
02961   memset(&context,0,sizeof(context));
02962   context.qdata = (const unsigned char*)data;
02963   context.compar2 = compar;
02964   idx    = (unsigned int*)index; // convert to unsigned int
02965   if ( ON::quick_sort == method )
02966   {
02967     ON_qsort(idx,count,sizeof(idx[0]),qicompar2,&context);
02968   }
02969   else
02970   {
02971     // heap sort
02972     icount = (unsigned int)count;
02973 
02974     k = icount >> 1;
02975     icount--;
02976     for (;;) 
02977     {
02978       if (k) 
02979       {
02980         tmpi = idx[--k];
02981         tmp  = context.qdata + tmpi;
02982       }
02983       else 
02984       {
02985         tmpi = idx[icount];
02986         tmp  = context.qdata + tmpi;
02987         idx[icount] = idx[0];
02988         if (!(--icount)) 
02989         {
02990           idx[0] = tmpi;
02991           break;
02992         }
02993       }
02994       i = k;
02995       j = (k<<1) + 1;
02996       while (j <= icount) 
02997       {
02998         if (j < icount && context.compar2(context.qdata + idx[j], context.qdata + idx[j+1]) < 0) 
02999         {
03000           j++;
03001         }
03002         if (context.compar2(tmp,context.qdata + idx[j]) < 0) 
03003         {
03004           idx[i] = idx[j];
03005           i = j;
03006           j = (j<<1) + 1;
03007         } 
03008         else 
03009         {
03010           j = icount + 1;
03011         }
03012       }
03013       idx[i] = tmpi;
03014     }
03015   }
03016 
03017   for (i = (unsigned int)count; i--; idx++ )
03018   {
03019     *idx /= isizeof_element;
03020   }
03021 }
03022 
03023 void
03024 ON_Sort( ON::sort_algorithm method, 
03025          int* index, 
03026          const void* data, 
03027          size_t count, 
03028          size_t sizeof_element, 
03029          int (*compar)(const void*,const void*,void*),
03030          void* p
03031          )
03032 {
03033   tagON_SORT_CONTEXT context;
03034   unsigned int* idx;
03035   const void* tmp;
03036   unsigned int i, j, k, tmpi, icount, isizeof_element;
03037   
03038   if (count < 1 || 0 == index || sizeof_element <= 0) 
03039   {
03040     return;
03041   }
03042   if (1 == count) 
03043   {
03044     index[0]=0;
03045     return;
03046   }
03047 
03048   isizeof_element = (unsigned int)sizeof_element; // (int) converts 64 bit size_t
03049   icount = (unsigned int)count;
03050   idx = (unsigned int*)index; // convert to unsigned int
03051   
03052   for ( i = 0; icount--; i += isizeof_element )
03053   {
03054     *idx++ = i;
03055   }
03056 
03057   memset(&context,0,sizeof(context));
03058   context.users_context = p;
03059   context.qdata = (const unsigned char*)data;
03060   context.compar3 = compar;
03061   idx    = (unsigned int*)index; // convert to unsigned int
03062   if ( ON::quick_sort == method )
03063   {
03064     ON_qsort(idx,count,sizeof(idx[0]),qicompar3,&context);
03065   }
03066   else
03067   {
03068     // heap sort
03069     icount = (unsigned int)count;
03070 
03071     k = icount >> 1;
03072     icount--;
03073     for (;;) 
03074     {
03075       if (k) 
03076       {
03077         tmpi = idx[--k];
03078         tmp  = context.qdata + tmpi;
03079       }
03080       else 
03081       {
03082         tmpi = idx[icount];
03083         tmp  = context.qdata + tmpi;
03084         idx[icount] = idx[0];
03085         if (!(--icount)) 
03086         {
03087           idx[0] = tmpi;
03088           break;
03089         }
03090       }
03091       i = k;
03092       j = (k<<1) + 1;
03093       while (j <= icount) 
03094       {
03095         if (j < icount && context.compar3(context.qdata + idx[j], context.qdata + idx[j+1], context.users_context) < 0) 
03096         {
03097           j++;
03098         }
03099         if (context.compar3(tmp,context.qdata + idx[j], context.users_context) < 0) 
03100         {
03101           idx[i] = idx[j];
03102           i = j;
03103           j = (j<<1) + 1;
03104         } 
03105         else 
03106         {
03107           j = icount + 1;
03108         }
03109       }
03110       idx[i] = tmpi;
03111     }
03112   }
03113 
03114   for (i = (unsigned int)count; i--; idx++ )
03115   {
03116     *idx /= isizeof_element;
03117   }
03118 }
03119 
03120 static void ON_hsort_str(char **e, size_t nel)
03121 {
03122   size_t
03123     i_end,k;
03124   char
03125     *e_tmp;
03126 
03127   if (nel < 2) return;
03128   k = nel >> 1;
03129   i_end = nel-1;
03130   for (;;) {
03131     if (k) {
03132       --k;
03133       e_tmp = e[k];
03134     } else {
03135       e_tmp = e[i_end];
03136       e[i_end] = e[0];
03137       if (!(--i_end)) {
03138         e[0] = e_tmp;
03139         break;
03140       }
03141     }
03142     { size_t i, j;
03143       i = k;
03144       j = (k<<1) + 1;
03145       while (j <= i_end) {
03146         if (j < i_end && strcmp(e[j],e[j + 1])<0) j++;
03147         if (strcmp(e_tmp,e[j])<0) {
03148           e[i] = e[j];
03149           i = j;
03150           j = (j<<1) + 1;
03151         } else j = i_end + 1;
03152       }
03153       e[i] = e_tmp;
03154     }
03155   }
03156 }
03157 
03158 const int* ON_BinarySearchIntArray( int key, const int* base, size_t nel )
03159 {
03160   if (nel > 0 && base )
03161   {
03162     size_t i;
03163     int d;
03164 
03165     // The end tests are not necessary, but they
03166     // seem to provide overall speed improvement
03167     // for the types of searches that call this
03168     // function.
03169     d = key-base[0];
03170     if ( d < 0 )
03171       return 0;
03172     if ( !d )
03173       return base;
03174 
03175     d = key-base[nel-1];
03176     if ( d > 0 )
03177       return 0;
03178     if ( !d )
03179       return (base + (nel-1));
03180 
03181     while ( nel > 0 )
03182     {
03183       i = nel/2;
03184       d = key - base[i];
03185       if ( d < 0 )
03186       {
03187         nel = i;
03188       }
03189       else if ( d > 0 )
03190       {
03191         i++;
03192         base += i;
03193         nel -= i;
03194       }
03195       else
03196       {
03197         return base+i;
03198       }
03199     }
03200   }
03201   return 0;
03202 }
03203 
03204 const unsigned int* ON_BinarySearchUnsignedIntArray( unsigned int key, const unsigned int* base, size_t nel )
03205 {
03206   if (nel > 0 && base )
03207   {
03208     size_t i;
03209     unsigned int d;
03210 
03211     // The end tests are not necessary, but they
03212     // seem to provide overall speed improvement
03213     // for the types of searches that call this
03214     // function.
03215     d = base[0];
03216     if ( key < d )
03217       return 0;
03218     if ( key == d )
03219       return base;
03220 
03221     d = base[nel-1];
03222     if ( key > d )
03223       return 0;
03224     if ( key == d )
03225       return (base + (nel-1));
03226 
03227     while ( nel > 0 )
03228     {
03229       i = nel/2;
03230       d = base[i];
03231       if ( key < d )
03232       {
03233         nel = i;
03234       }
03235       else if ( key > d )
03236       {
03237         i++;
03238         base += i;
03239         nel -= i;
03240       }
03241       else
03242       {
03243         return base+i;
03244       }
03245     }
03246   }
03247   return 0;
03248 }
03249 
03250 const double* ON_BinarySearchDoubleArray( double key, const double* base, size_t nel )
03251 {
03252   if (nel > 0 && base )
03253   {
03254     size_t i;
03255     double d;
03256 
03257     // The end tests are not necessary, but they
03258     // seem to provide overall speed improvement
03259     // for the types of searches that call this
03260     // function.
03261     d = key-base[0];
03262     if ( d < 0.0 )
03263       return 0;
03264     if ( 0.0 == d )
03265       return base;
03266 
03267     d = key-base[nel-1];
03268     if ( d > 0.0 )
03269       return 0;
03270     if ( 0.0 == d )
03271       return (base + (nel-1));
03272 
03273     while ( nel > 0 )
03274     {
03275       i = nel/2;
03276       d = key - base[i];
03277       if ( d < 0.0 )
03278       {
03279         nel = i;
03280       }
03281       else if ( d > 0.0 )
03282       {
03283         i++;
03284         base += i;
03285         nel -= i;
03286       }
03287       else
03288       {
03289         return base+i;
03290       }
03291     }
03292   }
03293   return 0;
03294 }
03295 
03296 
03297 int ON_Compare2dex( const ON_2dex* a, const ON_2dex* b)
03298 {
03299   int d;
03300   if ( 0 == (d = (a->i - b->i)) )
03301   {
03302     d = a->j - b->j;
03303   }
03304   return d;
03305 }
03306 
03307 
03308 int ON_Compare3dex( const ON_3dex* a, const ON_3dex* b)
03309 {
03310   int d;
03311   if ( 0 == (d = (a->i - b->i)) )
03312   {
03313     if ( 0 == (d = a->j - b->j) )
03314       d = a->k - b->k;
03315   }
03316   return d;
03317 }
03318 
03319 
03320 int ON_Compare4dex( const ON_4dex* a, const ON_4dex* b)
03321 {
03322   int d;
03323   if ( 0 == (d = (a->i - b->i)) )
03324   {
03325     if ( 0 == (d = a->j - b->j) )
03326     {
03327       if ( 0 == (d = a->k - b->k) )
03328         d = a->l - b->l;
03329     }
03330   }
03331   return d;
03332 }
03333 
03334 static int compar_string(const void* pa, const void* pb)
03335 {
03336   const char* sa = (const char*)pa;
03337   const char* sb = (const char*)pb;
03338   if ( !sa ) {
03339     return (sb) ? -1 : 0;
03340   }
03341   else if ( !sb ) {
03342     return 1;
03343   }
03344   return strcmp(sa,sb);
03345 }
03346 
03347 void
03348 ON_SortStringArray(
03349         ON::sort_algorithm method,
03350         char** e,   // array of strings
03351         size_t nel    // length of array
03352         )
03353 {
03354   if ( nel > 1 )
03355   {
03356     switch ( method ) 
03357     {
03358     case ON::heap_sort:
03359       ON_hsort_str( e, nel );
03360       break;
03361     case ON::quick_sort:
03362     default:
03363       ON_qsort( e, nel, sizeof(*e), compar_string );
03364       break;
03365     }
03366   }
03367 }
03368 
03369 
03370 /*
03371 Description:
03372   Use the quotient rule to compute derivatives of a one parameter
03373   rational function F(t) = X(t)/W(t), where W is a scalar
03374   and F and X are vectors of dimension dim.
03375 Parameters:
03376   dim - [in]
03377   der_count - [in] number of derivative (>=0)
03378   v_stride - [in] (>= dim+1)
03379   v - [in/out]
03380     v[] is an array of length (der_count+1)*v_stride.
03381     The input v[] array contains  derivatives of the numerator and
03382     denominator functions in the order (X, W), (Xt, Wt), (Xtt, Wtt), ...
03383     In general, the (dim+1) coordinates of the d-th derivative 
03384     are in (v[n],...,v[n+dim]) where n = d*v_stride.
03385     In the output v[] array the derivatives of X are replaced with
03386     the derivatives of F and the derivatives of W are divided by
03387     w = v[dim].
03388 Returns:
03389   True if input is valid; i.e., v[dim] != 0.
03390 See Also:
03391   ON_EvaluateQuotientRule2
03392   ON_EvaluateQuotientRule3
03393 */
03394 
03395 bool ON_EvaluateQuotientRule( int dim, int der_count, int v_stride, double *v )
03396 {
03397   /*
03398   The quotient rule says the n-th derivative is
03399 
03400             (n)       (n)          (n)             (n-1)    (1)              (1)    (n-1)
03401           f  (t) =  x   (t)  -  (w  (t)*f(t) + n*w    (t)*f  (t) + ... + n*w  (t)*f    (t))
03402                     ---------------------------------------------------------------------
03403                                              w(t)
03404 
03405 
03406                                                                         (i)   (j)
03407          (The missing summands look like  ON_BinomialCoefficient(i,j)*w   * f    )
03408   */
03409 
03410   double
03411     wt, w2, *f, *x, *w;
03412   int
03413     i, j, n, df;
03414 
03415   wt = v[dim];
03416   if (wt == 0.0)
03417     return false;
03418   wt = 1.0/wt;
03419   i = (der_count+1)*v_stride;
03420   x = v;
03421   while(i--) *x++ *= wt;
03422 
03423   if (der_count) {
03424     // 1rst derivative - faster special case 
03425     f = v;            // f = func(t)
03426     x = v + v_stride; // x = numerator'(t)/w
03427     wt = -x[dim];     // wt = -denominator'(t)/w
03428     j = dim; while (j--) *x++ +=  wt* *f++;
03429     if (der_count> 1) {
03430       // 2nd derivative - faster special case 
03431       f = v + v_stride;
03432       x = f + v_stride;
03433       // v = func(t), f = func'(t), x = numerator''(t)/w, 
03434       // * wt = -2*denominator'(t)/w, w2 = denominator''(t)/w
03435       wt *= 2.0;
03436       w2 = -x[dim];
03437       j = dim; while(j--) *x++ += w2* *v++ + wt* *f++;
03438       if (der_count>2) {
03439         df = v_stride-dim;
03440         // higher derivatives use slower loop
03441         v -= dim;
03442         x = v + v_stride*2;
03443         for (n = 3; n <= der_count; n++) {
03444           // computing n-th derivative
03445           f = v;
03446           x += v_stride; // x = numerator^(n)/weight 
03447           w = v + n*v_stride + dim;
03448           for (i = 0; i < n; i++) {
03449             // f = value of i-th derivative 
03450             // w = ((n-i)-th derivative of denominator)/weight
03451             wt = -ON_BinomialCoefficient(n-i,i) * *w;
03452             w -= v_stride;
03453             j = dim; while (j--) *x++ += *f++ * wt;
03454             x -= dim;
03455             f += df;
03456           }
03457         }
03458       }
03459     }
03460   }
03461 
03462   return true;
03463 }
03464 
03465 /*
03466 Description:
03467   Use the quotient rule to compute partial derivatives of a two parameter
03468   rational function F(s,t) = X(s,t)/W(s,t), where W is a scalar
03469   and F and X are vectors of dimension dim.
03470 Parameters:
03471   dim - [in]
03472   der_count - [in] number of derivative (>=0)
03473   v_stride - [in] (>= dim+1)
03474   v - [in/out]
03475     v[] is an array of length (der_count+2)*(der_count+1)*v_stride.
03476     The input array contains derivatives of the numerator and denominator
03477                 functions in the order X, W, Xs, Ws, Xt, Wt, Xss, Wss, Xst, Wst, Xtt, Wtt, ...
03478     In general, the (i,j)-th derivatives are in the (dim+1) entries of v[]
03479                 v[k], ..., answer[k+dim], where k = ((i+j)*(i+j+1)/2 + j)*v_stride.
03480     In the output v[] array the derivatives of X are replaced with
03481     the derivatives of F and the derivatives of W are divided by
03482     w = v[dim].
03483 Returns:
03484   True if input is valid; i.e., v[dim] != 0.
03485 See Also:
03486   ON_EvaluateQuotientRule
03487   ON_EvaluateQuotientRule3
03488 */
03489 
03490 bool ON_EvaluateQuotientRule2( int dim, int der_count, int v_stride, double *v )
03491 {
03492   double
03493     F, Fs, Ft, ws, wt, wss, wtt, wst, *f, *x;
03494   int
03495     i, j, n, q, ii, jj, Fn;
03496 
03497   // comment notation:
03498   //  X = value of numerator
03499   //  W = value of denominator
03500   //  F = X/W
03501   //  Xs = partial w.r.t. 1rst parameter
03502   //  Xt = partial w.r.t. 2nd parameter 
03503   //  ...
03504   // 
03505 
03506         // divide everything by the weight
03507   F = v[dim];
03508   if (F == 0.0)
03509     return false;
03510   F = 1.0/F;
03511   if ( v_stride > dim+1 )
03512   {
03513     i = ((der_count+1)*(der_count+2)>>1);
03514     x = v;
03515     j = dim+1;
03516     q = v_stride-j;
03517     while(i--)
03518     {
03519       jj = j;
03520       while(jj--)
03521         *x++ *= F;
03522       x += q;
03523     }
03524   }
03525   else
03526   {
03527     i = (((der_count+1)*(der_count+2))>>1)*v_stride;
03528     x = v;
03529     while(i--) *x++ *= F;
03530   }
03531 
03532   if (der_count) 
03533   {
03534                 // first derivatives
03535     f = v;                    // f = F
03536     x = v + v_stride;         // x = Xs/w, x[v_stride] = Xt/w
03537     ws = -x[dim];             // ws = -Ws/w
03538     wt = -x[dim+v_stride];    // wt = -Wt/w
03539     j = dim; 
03540                 while (j--) 
03541     {
03542                         F = *f++;
03543                         *x += ws*F;
03544                         x[v_stride] += wt*F;
03545                         x++;
03546                 }
03547 
03548     if (der_count> 1) 
03549     {
03550       // 2nd derivatives
03551       f+= (v_stride-dim);           // f = Fs, f[cvdim] = Ft 
03552       x = v + 3*v_stride;     // x = Xss, x[v_stride] = Xst, x[2*v_stride] = Xtt
03553       wss = -x[dim];          // wss = -wss/W
03554                         wst = -x[v_stride+dim]; // wst = -Wst/W 
03555                         n = 2*v_stride;
03556       wtt = -x[n+dim];        // wtt = -Wtt/w
03557       j = dim; 
03558                         while(j--) 
03559       {
03560                                 F = *v++;
03561                                 Ft = f[v_stride];
03562                                 Fs = *f++;
03563                                 *x          += wss*F + 2.0*ws*Fs;     // Dss
03564                                 x[v_stride] += wst*F + wt*Fs + ws*Ft; // Dst
03565                                 x[n]        += wtt*F + 2.0*wt*Ft;     // Dtt
03566                                 x++;
03567                         }
03568 
03569       if (der_count>2) 
03570       {
03571                                 // general loop for higher derivatives 
03572         v -= dim; // restore v pointer to input value
03573         f = v + 6*v_stride;    // f = Xsss
03574         for ( n = 3; n <= der_count; n++ ) 
03575         {
03576           for ( j = 0; j <= n; j++ ) 
03577           {
03578             // f = Ds^i Dt^j
03579             // 13 Jan 2005 Dale Lear bug fix - added missing a binomial coefficients.
03580             i = n-j;
03581             for ( ii = 0; ii <= i; ii++ ) 
03582             {
03583               ws = ON_BinomialCoefficient(ii,i-ii);
03584               for ( jj = ii?0:1; jj <= j; jj++ ) 
03585               {
03586                 q = ii+jj;
03587                 Fn = ((q*(q+1))/2 + jj)*v_stride+dim;
03588                 // wt = -(i choose ii)*(j choose jj)*W(ii,jj)
03589                 wt = -ws*ON_BinomialCoefficient(jj,j-jj)*v[Fn];
03590                 q = n-q;
03591                 Fn = ((q*(q+1))/2 + j-jj)*v_stride; // X(i-ii,j-jj) = v[Fn]
03592                 for ( q = 0; q < dim; q++ )
03593                   f[q] += wt*v[Fn+q];                
03594               }
03595             }
03596             f += v_stride;
03597           }
03598         }
03599       }
03600     }
03601   }
03602 
03603   return true;
03604 }
03605 
03606 
03607 /*
03608 Description:
03609   Use the quotient rule to compute partial derivatives of a 3 parameter
03610   rational function F(r,s,t) = X(r,s,t)/W(r,s,t), where W is a scalar
03611   and F and X are vectors of dimension dim.
03612 Parameters:
03613   dim - [in]
03614   der_count - [in] number of derivative (>=0)
03615   v_stride - [in] (>= dim+1)
03616   v - [in/out]
03617     v[] is an array of length 
03618     v_stride*(der_count+1)*(der_count+2)*(der_count+3)/6.
03619     The input v[] array contains  derivatives of the numerator and
03620     denominator functions in the order (X, W), (Xr, Wr), (Xs, Ws),
03621     (Xt, Wt), (Xrr, Wrr), (Xrs, Wrs), (Xrt, Wrt), (Xss, Wss), 
03622     (Xst, Wst), (Xtt, Wtt), ...
03623     In general, the (dim+1) coordinates of the input derivative 
03624     (Dr^i Ds^j Dt^k, i+j+k=d) are at v[n], ..., v[n+dim] where 
03625 
03626           n = v_stride*( d*(d+1)*(d+2)/6 + (j+k)*(j+k+1)/2 + k).
03627 
03628     In the output v[] array the derivatives of X are replaced with
03629     the derivatives of F and the derivatives of W are divided by
03630     w = v[dim].
03631 Returns:
03632   True if input is valid; i.e., v[dim] != 0.
03633 See Also:
03634   ON_EvaluateQuotientRule
03635   ON_EvaluateQuotientRule2
03636 */
03637 
03638 bool ON_EvaluateQuotientRule3( int dim, int der_count, int v_stride, double *v )
03639 {
03640   double
03641     F, Fr, Fs, Ft, wr, ws, wt, wrr, wrs, wrt, wss, wst, wtt, *f, *x;
03642   int
03643     i, j, k, n;
03644 
03645   // comment notation:
03646   //   X = value of numerator
03647   //   W = value of denominator
03648   //   F = X/W
03649   //   Xr = partial w.r.t. 1st parameter
03650   //   Xs = partial w.r.t. 2nd parameter
03651   //   Xt = partial w.r.t. 3rd parameter 
03652   //   ...
03653 
03654         // divide everything by the weight
03655   F = v[dim];
03656   if (F == 0.0)
03657     return false;
03658   F = 1.0/F;
03659   n = der_count+1;
03660   i = v_stride*n*(n+1)*(n+2)/6;
03661   x = v;
03662   while(i--) 
03663     *x++ *= F;
03664 
03665   if (der_count) 
03666   {
03667                 // first derivatives
03668     f = v;                   // f = F
03669     x = v + v_stride;        // x = Xr/w, x[v_stride] = Xs/w
03670     wr = -x[dim];            // wr = -Wr/w
03671     ws = -x[dim+v_stride];   // ws = -Ws/w
03672     wt = -x[dim+2*v_stride]; // wt = -Wt/w
03673     j = dim; 
03674                 while (j--) 
03675     {
03676                         F = *f++;
03677                         x[0]          += wr*F;
03678                         x[v_stride]   += ws*F;
03679                         x[2*v_stride] += wt*F;
03680                         x++;
03681                 }
03682 
03683     if (der_count> 1) 
03684     {
03685       // 2nd derivatives
03686       f = v;                // f = F, f[v_stride] = Fr, f[2*v_stride] = Fs, f[3*v_stride] = Ft
03687       x = v + 4*v_stride;   // x = Xrr, x[v_strde] = Xrs, ...
03688       wrr = -x[dim];
03689                         wrs = -x[dim+v_stride];
03690                         wrt = -x[dim+2*v_stride];
03691                         wss = -x[dim+3*v_stride];
03692                         wst = -x[dim+4*v_stride];
03693                         wtt = -x[dim+5*v_stride];
03694       j = dim; 
03695                         while(j--) 
03696       {
03697                                 Fr = f[v_stride];
03698                                 Fs = f[2*v_stride];
03699                                 Ft = f[3*v_stride];
03700                                 F = *f++;
03701                                 x[0]          += wrr*F + 2.0*wr*Fr;     // Drr
03702                                 x[v_stride]   += wrs*F + wr*Fs + ws*Fr; // Drs
03703                                 x[2*v_stride] += wrt*F + wr*Ft + wt*Fr; // Drt
03704                                 x[3*v_stride] += wss*F + 2.0*ws*Fs;     // Dss
03705                                 x[4*v_stride] += wst*F + ws*Ft + wt*Fs; // Dst
03706                                 x[5*v_stride] += wtt*F + 2.0*wt*Ft;     // Dtt
03707                                 x++;
03708                         }
03709 
03710       if ( der_count > 2 )
03711       {
03712         int ii, jj, kk, Fn, q;
03713         f = v + 10*v_stride; // f = Xrrr
03714         for ( n = 3; n <= der_count; n++ ) 
03715         {
03716           for ( i = n; i >= 0; i-- ) 
03717           {
03718             for ( j = n-i; j >= 0; j-- )
03719             {
03720               k = n-i-j;
03721               // f = Dr^i Ds^j Dt^k
03722               for ( ii = 0; ii <= i; ii++ ) 
03723               {
03724                 wr = ON_BinomialCoefficient(ii,i-ii);
03725                 for ( jj = 0; jj <= j; jj++ ) 
03726                 {
03727                   ws = wr*ON_BinomialCoefficient(jj,j-jj);
03728                   for ( kk = (ii||jj)?0:1; kk <= k; kk++ ) 
03729                   {
03730                     q = ii+jj+kk;
03731                     Fn=q-ii;
03732                     Fn = v_stride*( q*(q+1)*(q+2)/6  +  Fn*(Fn+1)/2  +  kk )+dim;
03733                     // wt = -(i choose ii)*(j choose jj)*(k choose kk)*W(ii,jj,kk)
03734                     wt = -ws*ON_BinomialCoefficient(kk,k-kk)*v[Fn];
03735                     q = n-q;
03736                     Fn = q-i+ii;
03737                     // F(i-ii,j-jj,k-kk) = v[Fn]
03738                     Fn = v_stride*( q*(q+1)*(q+2)/6  +  Fn*(Fn+1)/2  +  k-kk );
03739                     for ( q = 0; q < dim; q++ )
03740                       f[q] += wt*v[Fn+q];
03741                   }
03742                 }
03743               }
03744               f += v_stride;
03745             }
03746           }
03747         }
03748       }
03749     }
03750   }
03751 
03752   return true;
03753 }
03754 
03755 //#define ON_TEST_EV_KAPPAS
03756 
03757 #if defined(ON_TEST_EV_KAPPAS)
03758 
03759 void ON_EPC_WARNING(const char* msg)
03760 {
03761   ON_Warning(__FILE__,__LINE__,msg);
03762 }
03763 
03764 #else
03765 
03766 #define ON_EPC_WARNING(msg) 
03767 
03768 #endif
03769 
03770 ON_BOOL32 ON_EvPrincipalCurvatures( 
03771         const ON_3dVector& Ds,
03772         const ON_3dVector& Dt,
03773         const ON_3dVector& Dss,
03774         const ON_3dVector& Dst,
03775         const ON_3dVector& Dtt,
03776         const ON_3dVector& N, // unit normal (use TL_EvNormal())
03777         double* gauss,        // = Gaussian curvature = kappa1*kappa2
03778         double* mean,         // = mean curvature = (kappa1+kappa2)/2
03779         double* kappa1,       // = largest principal curvature value (may be negative)
03780         double* kappa2,       // = smallest principal curvature value (may be negative)
03781         ON_3dVector& K1,      // kappa1 unit principal curvature direction
03782         ON_3dVector& K2       // kappa2 unit principal curvature direction
03783                               // output K1,K2,N is right handed frame
03784         )
03785 {
03786   const double l = N.x*Dss.x + N.y*Dss.y + N.z*Dss.z;
03787   const double m = N.x*Dst.x + N.y*Dst.y + N.z*Dst.z;
03788   const double n = N.x*Dtt.x + N.y*Dtt.y + N.z*Dtt.z;
03789 
03790         return ON_EvPrincipalCurvatures(  Ds, Dt, l, m, n, N, 
03791                                                                                         gauss, mean,  kappa1, kappa2,   K1,  K2 );   
03792 }
03793 
03794 ON_BOOL32 ON_EvPrincipalCurvatures( 
03795         const ON_3dVector& Ds,
03796         const ON_3dVector& Dt,
03797         double l,                                                       // Second fundamental form coefficients
03798         double m,
03799         double n,
03800         const ON_3dVector& N, // unit normal (use TL_EvNormal())
03801         double* gauss,        // = Gaussian curvature = kappa1*kappa2
03802         double* mean,         // = mean curvature = (kappa1+kappa2)/2
03803         double* kappa1,       // = largest principal curvature value (may be negative)
03804         double* kappa2,       // = smallest principal curvature value (may be negative)
03805         ON_3dVector& K1,      // kappa1 unit principal curvature direction
03806         ON_3dVector& K2       // kappa2 unit principal curvature direction
03807                               // output K1,K2,N is right handed frame
03808         )
03809 {
03810 
03811   //return ON_WRONG_EvPrincipalCurvatures( Ds, Dt, Dss, Dst, Dtt, N, gauss, mean, kappa1, kappa2, K1, K2 );
03812 
03813   //double e, f, g, l, m, n, jac, trace, det;
03814   double x, k1, k2;
03815 
03816   const double e = Ds.x*Ds.x + Ds.y*Ds.y + Ds.z*Ds.z;
03817   const double f = Ds.x*Dt.x + Ds.y*Dt.y + Ds.z*Dt.z;
03818   const double g = Dt.x*Dt.x + Dt.y*Dt.y + Dt.z*Dt.z;
03819 
03820 
03821   if (gauss) *gauss = 0.0;
03822   if (mean) *mean = 0.0;
03823   if (kappa1) *kappa1 = 0.0;
03824   if (kappa2) *kappa2 = 0.0;
03825   K1.x = K1.y = K1.z = 0.0;
03826   K2.x = K2.y = K2.z = 0.0;
03827 
03828   const double jac = e*g - f*f;
03829   if ( jac == 0.0 )
03830     return false;
03831   x = 1.0/jac;
03832   const double det   = (l*n - m*m)*x;           // = Gaussian curvature
03833   const double trace = (g*l - 2.0*f*m + e*n)*x; // = 2*(mean curvature)
03834   if (gauss) *gauss = det;
03835   if (mean) *mean = 0.5*trace;
03836 
03837   {
03838     // solve  k^2 - trace*k + det = 0 to get principal curvatures
03839     //    k = 0.5*(trace +/-sqrt(trace*trace - 4.0*det)
03840     x = trace*trace;
03841     double tol = fabs(det)*1.0e-12; // 31 March 2008 Dale Lear - added tol to fix 32091
03842     if ( x < 4.0*det-tol ) 
03843     {
03844       if ( det <= ON_EPSILON )
03845       {
03846         k1 = k2 = 0.0;
03847         if (gauss) *gauss = 0.0;
03848         if (mean) *mean = 0.0;
03849       }
03850       else
03851       {
03852         return false;
03853       }
03854     }
03855     else if ( 0.0 == x )
03856     {
03857       if ( det > 0.0 )
03858         return false;
03859       k1 = sqrt(-det);
03860       k2 = -k1;
03861     }
03862     else
03863     {
03864       x = 4.0*det/x;
03865       if (x > 1.0)
03866         x = 1.0;
03867       // k1 = root with largest absolute value
03868       k1 = 0.5*fabs(trace)*(1.0 + sqrt(1.0 - x));
03869       if ( trace < 0.0 )
03870         k1 = -k1;
03871       // Calculate k2 this way instead of using
03872       // the numerically sloppy difference in the
03873       // standard quadratic formula.
03874       k2 = det/k1;
03875       if ( fabs(k2) > fabs(k1) )
03876       {
03877         // numerical nonsense
03878         k2 = (det < 0.0) ? -k1 : k1;
03879       }
03880     }
03881 
03882     if ( kappa1 )
03883       *kappa1 = k1;
03884     if ( kappa2 )
03885       *kappa2 = k2;
03886 
03887 #if defined(ON_TEST_EV_KAPPAS)
03888     double gggg = k1*k2;
03889     double tttt = k1+k2;
03890     if ( fabs(gggg - det) > 1.0e-4*fabs(det) )
03891     {
03892       ON_EPC_WARNING("ON_EvPrincipalCurvatures() Det(shape op) != gaussian");
03893     }
03894     if ( fabs(tttt - trace) > 1.0e-4*fabs(trace) )
03895     {
03896       ON_EPC_WARNING("ON_EvPrincipalCurvatures() Trace(shape op) != trace");
03897     }
03898 
03899     double zzz1 = k1*k1 - trace*k1 + det;
03900     double zzz2 = k2*k2 - trace*k2 + det;
03901     if ( fabs(zzz1) > (fabs(trace)+fabs(det))*1e-10 )
03902     {
03903       ON_EPC_WARNING("ON_EvPrincipalCurvatures() k1 is bogus");
03904     }
03905     if ( fabs(zzz2) > (fabs(trace)+fabs(det))*1e-10 )
03906     {
03907       ON_EPC_WARNING("ON_EvPrincipalCurvatures() k2 is bogus");
03908     }
03909 #endif
03910 
03911 
03912     {
03913       int bUmbilic = true;
03914       if ( fabs(k1-k2) > 1.0e-6*(fabs(k1) + fabs(k2)) ) 
03915       {
03916         // different principle curvatures - see if we can get an answer
03917         int ki, bFixK1, bFixK2;
03918         double a, b, c, d, kk, x, y, len1, len2, E[2];
03919 
03920         bUmbilic = false;
03921 
03922         // use ShapeOp(Ds) = d(N) along s, etc., to figure out that with respect
03923         // to Du,Dv basis for tangent space, ShapeOp is given by the matrix
03924         //
03925         //            a  b
03926         // ShapeOp = 
03927         //            c  d
03928         // 
03929         // where and a,b,c,d are ..
03930 
03931         x = 1.0/jac;
03932         a = (g*l - f*m)*x;
03933         b = (g*m - f*n)*x;
03934         c = (e*m - f*l)*x;
03935         d = (e*n - f*m)*x;
03936 
03937 #if defined(ON_TEST_EV_KAPPAS)
03938         //det   = (l*n - m*m)*x;           // = Gaussian curvature
03939         //trace = (g*l - 2.0*f*m + e*n)*x; // = 2*(mean curvature)
03940         double ggg = a*d - b*c;
03941         double ttt = a+d;
03942         if ( fabs(ggg - det) > 1.0e-4*fabs(det) )
03943         {
03944           ON_EPC_WARNING("ON_EvPrincipalCurvatures() Det(shape op) != gaussian");
03945         }
03946         if ( fabs(ttt - trace) > 1.0e-4*fabs(trace) )
03947         {
03948           ON_EPC_WARNING("ON_EvPrincipalCurvatures() Trace(shape op) != trace");
03949         }
03950 #endif
03951 
03952         //  Since I'm looking for eigen vectors, I can ignore scale factor "x".
03953         // So I need to solve
03954         //
03955         //          a   b 
03956         //                *  Di = ki*Di
03957         //          c   d
03958         //
03959         // and set Ki = Di[0]*Ds + Di[1] *Dt;
03960         //
03961         len1 = len2 = 0.0;
03962         for ( ki = 0; ki < 2; ki++ ) 
03963         {
03964           //                   a-ki   b
03965           // The matrix
03966           //                    c    d-ki
03967           //
03968           // should have rank = 1.  This means (a-ki, b) and
03969           // (c,d-ki) must be linearly dependent.   The code
03970           // below sets (x,y) = the (best) average of the two 
03971           // vectors, and sets Ki = y*Ds - x*Dt.
03972           kk = (ki) ? k2 : k1;
03973 
03974 #if defined(ON_TEST_EV_KAPPAS)
03975           x = (a-kk)*(d-kk) - b*c; // kk = eigen value of ShapeOp means
03976                                    // x should be zero
03977           if ( fabs(x) > 1.0e-8 )
03978           {
03979             if ( 0==ki )
03980             {
03981               ON_EPC_WARNING("ON_EvPrincipalCurvatures() |det(shape op - [k1,k1])| > 1.0e-8");
03982             }
03983             else
03984             {
03985               ON_EPC_WARNING("ON_EvPrincipalCurvatures() |det(shape op - [k2,k2])| > 1.0e-8");
03986             }
03987           }
03988 #endif
03989 
03990 
03991           if ( (a-kk)*c + b*(d-kk) >= 0.0 ) 
03992           {
03993             x = (a-kk+c);
03994             y = (b+d-kk);
03995           }
03996           else {
03997             x = (a-kk-c);
03998             y = (b-d+kk);
03999           }
04000 
04001           E[0] = -y; E[1] = x;
04002           
04003 #if defined(ON_TEST_EV_KAPPAS)
04004           // debugging check: should have shapeE[] = kk*E[]
04005           double shapeE[2];
04006           shapeE[0] = a*E[0] + b*E[1];
04007           shapeE[1] = c*E[0] + d*E[1];
04008 
04009           x = shapeE[0] - kk*E[0];
04010           y = shapeE[1] - kk*E[1];
04011 
04012           if ( fabs(x) > 1.0e-8 || fabs(y) > 1.0e-8 )
04013           {
04014             if ( 0==ki )
04015             {
04016               ON_EPC_WARNING("ON_EvPrincipalCurvatures() (shape op k1 eigenvector is noisy).");
04017             }
04018             else
04019             {
04020               ON_EPC_WARNING("ON_EvPrincipalCurvatures() (shape op k2 eigenvector is noisy).");
04021             }
04022           }
04023 #endif
04024 
04025           if ( ki == 0 )
04026           {
04027             K1 = E[0]*Ds + E[1]*Dt;
04028             len1 = K1.Length();
04029             if ( len1 > 0.0 ) 
04030               K1 *= (1.0/len1);
04031           }
04032           else if ( ki == 1 )
04033           {
04034             K2 = E[0]*Ds + E[1]*Dt;
04035             len2 = K2.Length();
04036             if ( len2 > 0.0 ) 
04037               K2 *= (1.0/len2);
04038           }
04039         }
04040 
04041         bFixK1 = bFixK2 = false;
04042         {
04043           // make sure K1 and K2 are perp to N.
04044           x = K1*N;
04045           if ( fabs(x) >= 1.0e-4 )
04046           {
04047             ON_EPC_WARNING("ON_EvPrincipalCurvatures() K1*N > 1.0e-4.");
04048             bFixK1 = true;
04049           }
04050 
04051           x = K2*N;
04052           if ( fabs(x) >= 1.0e-4 )
04053           {
04054             ON_EPC_WARNING("ON_EvPrincipalCurvatures() K2*N > 1.0e-4.");
04055             bFixK2 = true;
04056           }
04057         }
04058 
04059         if ( !bFixK1 && !bFixK2 ) 
04060         {
04061           // make sure K1 and K2 are perp.
04062           x = K1*K2;
04063           if ( fabs(x) >= 1.0e-4 ) 
04064           {
04065 #if defined(ON_TEST_EV_KAPPAS)
04066             {
04067               static bool bSecondTry = false;
04068               if ( !bSecondTry )
04069               {
04070                 ON_EPC_WARNING("ON_EvPrincipalCurvatures() K1*K2 > 0.1.");
04071 
04072                 // 15 July 2005 - Dale Lear
04073                 //        There is a bug in converting the 2d eigenvectors
04074                 //        Into 3d K1 and K2.  I'll look into it later.
04075                 bSecondTry = true;
04076                 double ggg, mmm, kkk1, kkk2;
04077                 ON_3dVector KKK1, KKK2;
04078                 ON_EvPrincipalCurvatures(Ds,Dt,Dss,Dst,Dtt,N,
04079                                         &ggg,&mmm,&kkk1,&kkk2,KKK1,KKK2);
04080                 bSecondTry = false;
04081               }
04082             }
04083 #endif
04084             if ( len1 < len2 )
04085               bFixK1 = true;
04086             else
04087               bFixK2 = true;
04088           }
04089         }
04090 
04091         if ( bFixK1 || bFixK2 ) 
04092         {
04093           if ( bFixK1 && bFixK2 ) 
04094           {
04095             bUmbilic = true;
04096           }
04097           else if ( bFixK1 ) 
04098           {
04099             K1 = ON_CrossProduct( K2, N );
04100             K1.Unitize();
04101           }
04102           else if ( bFixK2 ) 
04103           {
04104             K2 = ON_CrossProduct( N, K1 );
04105             K2.Unitize();
04106           }
04107         }
04108       }
04109 
04110       if ( bUmbilic ) {
04111         // equal principle curvatures
04112         if ( e >= g ) {
04113           // Ds is longest derivative
04114           K1 = Ds;
04115           K1.Unitize();
04116         }
04117         else {
04118           // Dt is longest derivative
04119           K1 = Dt;
04120           K1.Unitize();
04121         }
04122         K2 = ON_CrossProduct( N, K1 );
04123         K2.Unitize();
04124       }
04125     }
04126   }
04127   return true;
04128 }
04129 
04130 
04131 ON_3dVector 
04132 ON_NormalCurvature( 
04133     const ON_3dVector& S10, const ON_3dVector& S01,
04134     const ON_3dVector& S20, const ON_3dVector& S11, const ON_3dVector& S02,
04135     const ON_3dVector& UnitNormal, const ON_3dVector& UnitTangent )
04136 /*****************************************************************************
04137 Evaluate normal curvature from surface derivatives and direction
04138  
04139 INPUT:
04140   S10, S01
04141     surface 1st partial derivatives
04142   S20, S11, S02
04143     surface 2nd partial derivatives
04144   SrfUnitNormal
04145     Unit normal to surface
04146   CrvUnitTangent
04147     3d unit tangent to the surface
04148 OUTPUT:
04149    The return value is the normal curvature vector for the surface in the direction of UnitTangent
04150    Normal curvature vector ((anti)parallel to UnitNormal).
04151    The scalar normal curvature is equal to NormalCurvature o UnitNormal.
04152 *****************************************************************************/
04153 {
04154   ON_3dVector NormalCurvature, D2, T, K;
04155   double a, b, d, e, pr;
04156   int rc;
04157 
04158   a = b = 0.0;
04159   // solve T = a*S10 + b*S01
04160   rc = ON_Solve3x2( S10, S01, UnitTangent.x, UnitTangent.y, UnitTangent.z, 
04161                     &a, &b, &e, &pr );
04162   if (rc < 2) {
04163     NormalCurvature.Zero();
04164   }
04165   else {
04166     // compute 2nd derivative of 3d curve(t) = srf( u0 + a*t, v0 + b*t ) at t=0
04167     D2 = a*a*S20 + 2.0*a*b*S11 + b*b*S02;
04168 
04169     // compute curvature of 3d curve(t) = srf( u0 + a*t, v0 + b*t ) at t=0
04170     ON_EvCurvature( UnitTangent, D2, T, K );
04171 
04172     // get component of K parallel to N
04173     d = K*UnitNormal;
04174     NormalCurvature = d*UnitNormal;
04175   }
04176 
04177   return NormalCurvature;
04178 }
04179 
04180 
04181 
04182 bool 
04183 ON_GetPolylineLength(
04184     int dim, 
04185     ON_BOOL32 is_rat,
04186     int count, 
04187     int stride, 
04188     const double* P, 
04189     double* length )
04190 /* first point  = {P[0], ... }
04191  * second point = {P[stride], ... }
04192  */
04193 {
04194 #define SUM_SIZE 128
04195   double       L, d, dd, w0, w1; 
04196         const double *p0, *p1;
04197   double       *sum;
04198         int          j, i, sumi;
04199 
04200   if (length)
04201                 *length = 0.0;
04202 
04203         if (stride == 0) stride = (dim+is_rat);
04204   if ( dim < 1 || count < 2 || stride < ((is_rat)?dim+1:dim) || !P || !length )
04205     return false;
04206 
04207 
04208         p1 = P;
04209   L = 0.0;
04210 
04211   sumi = count/SUM_SIZE;
04212   sumi++;
04213   sum = (double*)alloca( sumi*sizeof(*sum) );
04214   sumi = 0;
04215 
04216   if (is_rat) {
04217     w1 = p1[dim];
04218     if (w1 == 0.0) {
04219       ON_ERROR("ON_GetPolylineLength: Zero weight");
04220       return false;
04221     }
04222     w1 = 1.0/w1;
04223     for ( i = 1; i < count; i++ ) {
04224       w0 = w1;
04225                         p0 = p1;
04226                         p1 = p1 + stride;
04227       w1 = p1[dim];
04228       if (w1 == 0.0) {
04229         ON_ERROR("ON_GetPolylineLength: Zero weight");
04230         return false;
04231       }
04232       w1 = 1.0/w1;
04233                         dd = 0.0;
04234       for (j = 0; j < dim; j++) {
04235         d = w0* p0[j] - w1*p1[j];
04236         dd += d*d;
04237       }
04238       L += sqrt(dd);
04239       if ( !(i % SUM_SIZE) ) {
04240         sum[sumi++] = L;
04241         L = 0.0;
04242       }
04243     }
04244   }
04245   else {
04246     for (i = 1; i < count; i++) {
04247                         p0 = p1;
04248                         p1 = p1 + stride;
04249       dd = 0.0;
04250       for (j = 0; j < dim; j++) {
04251         d = p1[j] - p0[j];
04252         dd += d*d;
04253       }
04254       L += sqrt(dd);
04255       if ( !(i % SUM_SIZE) ) {
04256         sum[sumi++] = L;
04257         L = 0.0;
04258       }
04259     } 
04260   }
04261 
04262   for (i = 0; i < sumi; i++) {
04263     L += sum[i];
04264   }
04265 
04266   *length = L;
04267 
04268   return true;
04269 #undef SUM_SIZE
04270 }
04271 
04272 
04273 
04274 ON_Evaluator::ON_Evaluator( 
04275                            int parameter_count,
04276                            int value_count,
04277                            const ON_Interval* domain,
04278                            const bool* periodic
04279                            )
04280              : m_parameter_count(parameter_count),
04281                m_value_count(value_count>0?value_count:parameter_count)
04282 {
04283   int i;
04284 
04285   if (domain)
04286   {
04287     m_domain.Reserve(m_parameter_count);
04288     for ( i = 0; i < parameter_count; i++ )
04289       m_domain.Append(domain[i]);
04290 
04291     if (periodic )
04292     {
04293       for ( i = 0; i < parameter_count; i++ )
04294       {
04295         if  (periodic[i])
04296         {
04297           m_bPeriodicParameter.Reserve(m_parameter_count);
04298           for ( i = 0; i < m_parameter_count; i++ )
04299           {
04300             m_bPeriodicParameter.Append(periodic[i]?true:false);
04301           }
04302           break;
04303         }
04304       }
04305     }
04306   }
04307 }
04308 
04309 ON_Evaluator::~ON_Evaluator()
04310 {
04311 }
04312 
04313 
04314 int ON_Evaluator::EvaluateHessian(
04315      const double* parameters,
04316      double* value,
04317      double* gradient,
04318      double** hessian
04319      )
04320 {
04321   if ( m_parameter_count==1)
04322   {
04323     if ( 0 != gradient )
04324     {
04325       // we have enough information to get the 
04326       // value and the gradient
04327       Evaluate(parameters,value,&gradient);
04328     }
04329 
04330     if ( 0 != hessian )
04331     {
04332       // zero out the hessian matrix
04333       int i;
04334       for ( i = 0; i < m_parameter_count; i++ )
04335       {
04336         memset(hessian[0],0,m_parameter_count*sizeof(hessian[i][0]));
04337       }
04338     }
04339   }
04340 
04341   // -1 indicates that this evaluator does not support hessian evaluation.
04342   return -1;
04343 }
04344 
04345 
04346 bool ON_Evaluator::FiniteDomain() const
04347 {
04348   return ((m_parameter_count==m_domain.Count() && m_parameter_count>0) 
04349           ? true
04350           :false
04351           );
04352 }
04353 
04354 bool ON_Evaluator::Periodic(
04355   int parameter_index
04356   ) const
04357 {
04358   return ((m_parameter_count==m_bPeriodicParameter.Count() && m_parameter_count>0) 
04359           ? m_bPeriodicParameter[parameter_index] 
04360           : false
04361           );
04362 }
04363 
04364 ON_Interval ON_Evaluator::Domain(
04365   int parameter_index
04366   ) const
04367 {
04368   return ((m_parameter_count==m_domain.Count() && m_parameter_count>0) 
04369           ? m_domain[parameter_index] 
04370           : ON_Interval(-1.0e300,1.0e300)
04371           );
04372 }
04373 
04374 
04375  
04376 double ON_Max(double a, double b){
04377   return (a<b)? b:a;
04378 }
04379 
04380  
04381 float ON_Max(float a, float b){
04382   return (a<b)? b:a;
04383 }
04384 
04385  
04386 int ON_Max(int a, int b){
04387   return (a<b)? b:a;
04388 }
04389 
04390  
04391 double ON_Min(double a, double b){
04392   return (a<b)? a:b;
04393 }
04394 
04395  
04396 float ON_Min(float a, float b){
04397   return (a<b)? a:b;
04398 }
04399 
04400  
04401 int ON_Min(int a, int b){
04402   return (a<b)? a:b;
04403 }
04404 
04405 int ON_Round(double x)
04406 {
04407   // 12 March 2009 Dale Lear
04408   //   Added error checking to this function.
04409   //   Do not call this function in any opennurbs code, tl code
04410   //   or any other code that does critical calculations.
04411 
04412   if (!ON_IsValid(x))
04413   {
04414     ON_ERROR("ON_Round - invalid input");
04415     return 0;
04416   }
04417 
04418   // Do not use "INT_MAX" - the define is not portable
04419   // and this code needs to compile and execute the same
04420   // way on all OSs.
04421   if ( fabs(x) >= 2147483647.0 )
04422   {
04423     ON_ERROR("ON_Round - integer overflow");
04424     return (x > 0.0) ? 2147483647 : -2147483647;
04425   }
04426 
04427   return (x>=0.0) ? ((int)(x+0.5)) : -((int)(0.5-x));
04428 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:01