opennurbs_math.cpp
Go to the documentation of this file.
```00001 /* \$NoKeywords: \$ */
00002 /*
00003 //
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   {
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,
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
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  *
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   }
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]
00922   If length < 1 or array is not monotone increasing, you will get a meaningless
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
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:
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  *
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
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,
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:
02061  *   pivot_ratio
02063  * OUTPUT:
02064  *   ON_Solve2x2() returns rank (0,1,2)
02065  *
02066  *   If ON_Solve2x2() is successful (return code 2), then
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  *
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;}
02112   if (x == 0.0)
02113     return 0; // rank = 0
02114   minpiv = maxpiv = x;
02115   if (i%2) {
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
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,
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
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
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.
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
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 */
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;
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;
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,
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
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 */
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 */
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 */
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   {
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;
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;
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   {
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;
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   {
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
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,
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
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 */
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 */
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;
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 */
02863   if (i) {
02866   }
02867   else {
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]
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.
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.
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.
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
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,
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
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