src/Extrema/Extrema_ExtElC.cxx

   1 // Copyright (c) 1995-1999 Matra Datavision
   2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   3 //
   4 // This file is part of Open CASCADE Technology software library.
   5 //
   6 // This library is free software; you can redistribute it and / or modify it
   7 // under the terms of the GNU Lesser General Public version 2.1 as published
   8 // by the Free Software Foundation, with special exception defined in the file
   9 // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
  10 // distribution for complete text of the license and disclaimer of any warranty.
  11 //
  12 // Alternatively, this file may be used under the terms of Open CASCADE
  13 // commercial license or contractual agreement.
  14
  15 #include <stdio.h>
  16
  17 #include <Extrema_ExtElC.ixx>
  18 #include <StdFail_InfiniteSolutions.hxx>
  19 #include <StdFail_NotDone.hxx>
  20 #include <ElCLib.hxx>
  21 #include <math_TrigonometricFunctionRoots.hxx>
  22 #include <math_DirectPolynomialRoots.hxx>
  23 #include <Standard_OutOfRange.hxx>
  24 #include <Standard_NotImplemented.hxx>
  25 #include <Precision.hxx>
  26 #include <Extrema_ExtPElC.hxx>
  27 #include <IntAna_QuadQuadGeo.hxx>
  28 #include <Extrema_ExtPElC.hxx>
  29
  30 #include <gp_Pln.hxx>
  31 #include <gp_Ax3.hxx>
  32 #include <gp_Ax2.hxx>
  33 #include <gp_Pnt.hxx>
  34 #include <gp_Dir.hxx>
  35 #include <gp_Ax1.hxx>
  36
  37 static
  38   void RefineDir(gp_Dir& aDir);
  39
  40 //=======================================================================
  41 //class    : ExtremaExtElC_TrigonometricRoots
  42 //purpose  :
  43 //==  Classe Interne (Donne des racines classees d un polynome trigo)
  44 //==  Code duplique avec IntAna_IntQuadQuad.cxx (lbr le 26 mars 98)
  45 //==  Solution fiable aux problemes de coefficients proches de 0
  46 //==  avec essai de rattrapage si coeff<1.e-10 (jct le 27 avril 98)
  47 //=======================================================================
  48 class ExtremaExtElC_TrigonometricRoots {
  49  private:
  50   Standard_Real Roots[4];
  51   Standard_Boolean done;
  52   Standard_Integer NbRoots;
  53   Standard_Boolean infinite_roots;
  54  public:
  55   ExtremaExtElC_TrigonometricRoots(const Standard_Real CC,
  56                                    const Standard_Real SC,
  57                                    const Standard_Real C,
  58                                    const Standard_Real S,
  59                                    const Standard_Real Cte,
  60                                    const Standard_Real Binf,
  61                                    const Standard_Real Bsup);
  62   //
  63   Standard_Boolean IsDone() {
  64     return done;
  65   }
  66   //
  67   Standard_Boolean IsARoot(Standard_Real u) {
  68     Standard_Real PIpPI, aEps;
  69     //
  70     aEps=RealEpsilon();
  71     PIpPI = M_PI + M_PI;
  72     for(Standard_Integer i=0 ; i<NbRoots; i++) {
  73       if(Abs(u - Roots[i])<=aEps) {
  74         return Standard_True ;
  75       }
  76       if(Abs(u - Roots[i]-PIpPI)<=aEps) {
  77         return Standard_True;
  78       }
  79     }
  80     return Standard_False;
  81   }
  82   //
  83   Standard_Integer NbSolutions() {
  84     if(!done) {
  85       StdFail_NotDone::Raise();
  86     }
  87     return NbRoots;
  88   }
  89   //
  90   Standard_Boolean InfiniteRoots() {
  91     if(!done) {
  92       StdFail_NotDone::Raise();
  93     }
  94     return infinite_roots;
  95   }
  96   //
  97   Standard_Real Value(const Standard_Integer& n) {
  98     if((!done)||(n>NbRoots)) {
  99       StdFail_NotDone::Raise();
 100     }
 101     return Roots[n-1];
 102   }
 103 };
 104 //=======================================================================
 105 //function : ExtremaExtElC_TrigonometricRoots
 106 //purpose  :
 107 //=======================================================================
 108 ExtremaExtElC_TrigonometricRoots::
 109   ExtremaExtElC_TrigonometricRoots(const Standard_Real CC,
 110                                    const Standard_Real SC,
 111                                    const Standard_Real C,
 112                                    const Standard_Real S,
 113                                    const Standard_Real Cte,
 114                                    const Standard_Real Binf,
 115                                    const Standard_Real Bsup)
 116 {
 117   Standard_Integer i, nbessai;
 118   Standard_Real cc ,sc, c, s, cte;
 119   //
 120   nbessai = 1;
 121   cc = CC;
 122   sc = SC;
 123   c = C;
 124   s = S;
 125   cte = Cte;
 126   done=Standard_False;
 127   while (nbessai<=2 && !done) {
 128     //-- F= AA*CN*CN+2*BB*CN*SN+CC*CN+DD*SN+EE;
 129     math_TrigonometricFunctionRoots MTFR(cc,sc,c,s,cte,Binf,Bsup);
 130     //
 131     if(MTFR.IsDone()) {
 132       done=Standard_True;
 133       if(MTFR.InfiniteRoots()) {
 134         infinite_roots=Standard_True;
 135       }
 136       else { //else #1
 137         Standard_Boolean Triee;
 138         Standard_Integer j, SvNbRoots;
 139         Standard_Real aTwoPI, aMaxCoef, aPrecision;
 140         //
 141         aTwoPI=M_PI+M_PI;
 142         NbRoots=MTFR.NbSolutions();
 143         for(i=0;i<NbRoots;++i) {
 144           Roots[i]=MTFR.Value(i+1);
 145           if(Roots[i]<0.) {
 146             Roots[i]=Roots[i]+aTwoPI;
 147           }
 148           if(Roots[i]>aTwoPI) {
 149             Roots[i]=Roots[i]-aTwoPI;
 150           }
 151         }
 152         //
 153         //-- La recherche directe donne n importe quoi.
 154         aMaxCoef = Max(CC,SC);
 155         aMaxCoef = Max(aMaxCoef,C);
 156         aMaxCoef = Max(aMaxCoef,S);
 157         aMaxCoef = Max(aMaxCoef,Cte);
 158         aPrecision = Max(1.e-8, 1.e-12*aMaxCoef);
 159
 160         SvNbRoots=NbRoots;
 161         for(i=0; i<SvNbRoots; ++i) {
 162           Standard_Real y;
 163           Standard_Real co=cos(Roots[i]);
 164           Standard_Real si=sin(Roots[i]);
 165           y=co*(CC*co + (SC+SC)*si + C) + S*si + Cte;
 166           // modified by OCC  Tue Oct  3 18:43:00 2006
 167           if(Abs(y)>aPrecision) {
 168             NbRoots--;
 169             Roots[i]=1000.0;
 170           }
 171         }
 172         //
 173         do {
 174           Standard_Real t;
 175           //
 176           Triee=Standard_True;
 177           for(i=1, j=0; i<SvNbRoots; ++i, ++j) {
 178             if(Roots[i]<Roots[j]) {
 179               Triee=Standard_False;
 180               t=Roots[i];
 181               Roots[i]=Roots[j];
 182               Roots[j]=t;
 183             }
 184           }
 185         }
 186         while(!Triee);
 187         //
 188         infinite_roots=Standard_False;
 189         if(NbRoots==0) { //--!!!!! Detect case Pol = Cte ( 1e-50 ) !!!!
 190           if((Abs(CC) + Abs(SC) + Abs(C) + Abs(S)) < 1e-10) {
 191             if(Abs(Cte) < 1e-10)  {
 192               infinite_roots=Standard_True;
 193             }
 194           }
 195         }
 196       } // else #1
 197     } // if(MTFR.IsDone()) {
 198     else {
 199         // try to set very small coefficients to ZERO
 200       if (Abs(CC)<1e-10) {
 201         cc = 0.0;
 202       }
 203       if (Abs(SC)<1e-10) {
 204         sc = 0.0;
 205       }
 206       if (Abs(C)<1e-10) {
 207         c = 0.0;
 208       }
 209       if (Abs(S)<1e-10){
 210         s = 0.0;
 211       }
 212       if (Abs(Cte)<1e-10){
 213         cte = 0.0;
 214       }
 215       nbessai++;
 216     }
 217   } // while (nbessai<=2 && !done) {
 218 }
 219
 220 //=======================================================================
 221 //function : Extrema_ExtElC
 222 //purpose  :
 223 //=======================================================================
 224 Extrema_ExtElC::Extrema_ExtElC ()
 225 {
 226   myDone = Standard_False;
 227 }
 228 //=======================================================================
 229 //function : Extrema_ExtElC
 230 //purpose  :
 231 //=======================================================================
 232 Extrema_ExtElC::Extrema_ExtElC (const gp_Lin& C1,
 233                                 const gp_Lin& C2,
 234                                 const Standard_Real)
 235 // Function:
 236 //   Find min distance between 2 straight lines.
 237
 238 // Method:
 239 //   Let D1 and D2, be 2 directions of straight lines C1 and C2.
 240 //   2 cases are considered:
 241 //   1- if Angle(D1,D2) < AngTol, straight lines are parallel.
 242 //      The distance is the distance between a point of C1 and the straight line C2.
 243 //   2- if Angle(D1,D2) > AngTol:
 244 //      Let P1=C1(u1) and P2=C2(u2) be 2 solution points:
 245 //      Then, ( P1P2.D1 = 0. (1)
 246 //             ( P1P2.D2 = 0. (2)
 247 //   Let O1 and O2 be the origins of C1 and C2;
 248 //   THen, (1) <=> (O1P2-u1*D1).D1 = 0.       as O1P1 = u1*D1
 249 //           <=> u1 = O1P2.D1               as D1.D1 = 1.
 250 //          (2) <=> (P1O2+u2*D2).D2 = 0.       as O2P2 = u2*D2
 251 //           <=> u2 = O2P1.D2               as D2.D2 = 1.
 252 //           <=> u2 = (O2O1+O1P1).D2
 253 //           <=> u2 = O2O1.D2+((O1P2.T1)T1).T2) as O1P1 = u1*T1 = (O1P2.T1)T1
 254 //           <=> u2 = O2O1.D2+(((O1O2+O2P2).D1)D1).D2)
 255 //           <=> u2 = O2O1.D2+((O1O2.D1)D1).D2)+(O2P2.D1)(D1.D2)
 256 //           <=> u2 = ((O1O2.D1)D1-O1O2).D2 + u2*(D2.D1)(D1.D2)
 257 //           <=> u2 = (((O1O2.D1)D1-O1O2).D2) / 1.-(D1.D2)**2
 258 {
 259   myDone = Standard_False;
 260   myNbExt = 0;
 261
 262   gp_Dir D1 = C1.Position().Direction();
 263   gp_Dir D2 = C2.Position().Direction();
 264   // MSV 16/01/2000: avoid "divide by zero"
 265   Standard_Real D1DotD2 = D1.Dot(D2);
 266   Standard_Real aSin = 1.-D1DotD2*D1DotD2;
 267   if (aSin < gp::Resolution() ||
 268       D1.IsParallel(D2, Precision::Angular())) {
 269     myIsPar = Standard_True;
 270     mySqDist[0] = C2.SquareDistance(C1.Location());
 271     mySqDist[1] = mySqDist[0];
 272   }
 273   else {
 274     myIsPar = Standard_False;
 275     gp_Pnt O1 = C1.Location();
 276     gp_Pnt O2 = C2.Location();
 277     gp_Vec O1O2 (O1,O2);
 278     Standard_Real U2 = (D1.XYZ()*(O1O2.Dot(D1))-(O1O2.XYZ())).Dot(D2.XYZ());
 279     if( Precision::IsInfinite(U2) ) {
 280       myIsPar = Standard_True;
 281       mySqDist[0] = C2.SquareDistance(C1.Location());
 282       mySqDist[1] = mySqDist[0];
 283     }
 284     else {
 285       U2 /= aSin;
 286       if( Precision::IsInfinite(U2) ) {
 287         myIsPar = Standard_True;
 288         mySqDist[0] = C2.SquareDistance(C1.Location());
 289     mySqDist[1] = mySqDist[0];
 290       }
 291       else {
 292         gp_Pnt P2(ElCLib::Value(U2,C2));
 293         Standard_Real U1 = (gp_Vec(O1,P2)).Dot(D1);
 294         if( Precision::IsInfinite(U1) ) {
 295           myIsPar = Standard_True;
 296           mySqDist[0] = C2.SquareDistance(C1.Location());
 297       mySqDist[1] = mySqDist[0];
 298         }
 299         else {
 300           gp_Pnt P1(ElCLib::Value(U1,C1));
 301           mySqDist[myNbExt] = P1.SquareDistance(P2);
 302           myPoint[myNbExt][0] = Extrema_POnCurv(U1,P1);
 303           myPoint[myNbExt][1] = Extrema_POnCurv(U2,P2);
 304           myNbExt = 1;
 305         }
 306       }
 307     }
 308   }
 309   myDone = Standard_True;
 310 }
 311 //=======================================================================
 312 //function : Extrema_ExtElC
 313 //purpose  :
 314 // Find extreme distances between straight line C1 and circle C2.
 315 //
 316 //Method:
 317 //   Let P1=C1(u1) and P2=C2(u2) be two solution points
 318 //        D the direction of straight line C1
 319 //      T tangent at point P2;
 320 //  Then, ( P1P2.D = 0. (1)
 321 //         ( P1P2.T = 0. (2)
 322 //  Let O1 and O2 be the origins of C1 and C2;
 323 //  Then, (1) <=> (O1P2-u1*D).D = 0.         as O1P1 = u1*D
 324 //           <=> u1 = O1P2.D                as D.D = 1.
 325 //         (2) <=> P1O2.T = 0.                as O2P2.T = 0.
 326 //             <=> ((P2O1.D)D+O1O2).T = 0.    as P1O1 = -u1*D = (P2O1.D)D
 327 //           <=> (((P2O2+O2O1).D)D+O1O2).T = 0.
 328 //           <=> ((P2O2.D)(D.T)+((O2O1.D)D-O2O1).T = 0.
 329 //  We are in the reference of the circle; let:
 330 //         Cos = Cos(u2) and Sin = Sin(u2),
 331 //         P2 (R*Cos,R*Sin,0.),
 332 //         T (-R*Sin,R*Cos,0.),
 333 //       D (Dx,Dy,Dz),
 334 //       V (Vx,Vy,Vz) = (O2O1.D)D-O2O1;
 335 //  Then, the equation by Cos and Sin is as follows:
 336 //    -(2*R*R*Dx*Dy)   * Cos**2  +       A1
 337 //   R*R*(Dx**2-Dy**2) * Cos*Sin +    2* A2
 338 //         R*Vy        * Cos     +       A3
 339 //      -R*Vx        * Sin     +       A4
 340 //      R*R*Dx*Dy                = 0.    A5
 341 //Use the algorithm math_TrigonometricFunctionRoots to solve this equation.
 342 //=======================================================================
 343 Extrema_ExtElC::Extrema_ExtElC (const gp_Lin& C1,
 344                                 const gp_Circ& C2,
 345                                 const Standard_Real)
 346 {
 347   Standard_Real Dx,Dy,Dz,aRO2O1, aTolRO2O1;
 348   Standard_Real R, A1, A2, A3, A4, A5, aTol;
 349   gp_Dir x2, y2, z2, D, D1;
 350   //
 351   myIsPar = Standard_False;
 352   myDone = Standard_False;
 353   myNbExt = 0;
 354
 355   // Calculate T1 in the reference of the circle ...
 356   D = C1.Direction();
 357   D1 = D;
 358   x2 = C2.XAxis().Direction();
 359   y2 = C2.YAxis().Direction();
 360   z2 = C2.Axis().Direction();
 361   Dx = D.Dot(x2);
 362   Dy = D.Dot(y2);
 363   Dz = D.Dot(z2);
 364   //
 365   D.SetCoord(Dx, Dy, Dz);
 366   RefineDir(D);
 367   D.Coord(Dx, Dy, Dz);
 368   //
 369   // Calcul de V dans le repere du cercle:
 370   gp_Pnt O1 = C1.Location();
 371   gp_Pnt O2 = C2.Location();
 372   gp_Vec O2O1 (O2,O1);
 373   //
 374   aTolRO2O1=gp::Resolution();
 375   aRO2O1=O2O1.Magnitude();
 376   if (aRO2O1 > aTolRO2O1) {
 377     gp_Dir aDO2O1;
 378     //
 379     O2O1.Multiply(1./aRO2O1);
 380     aDO2O1.SetCoord(O2O1.Dot(x2), O2O1.Dot(y2), O2O1.Dot(z2));
 381     RefineDir(aDO2O1);
 382     O2O1.SetXYZ(aRO2O1*aDO2O1.XYZ());
 383   }
 384   else {
 385     O2O1.SetCoord(O2O1.Dot(x2), O2O1.Dot(y2), O2O1.Dot(z2));
 386   }
 387   //
 388   gp_XYZ Vxyz = (D.XYZ()*(O2O1.Dot(D)))-O2O1.XYZ();
 389   //
 390   //modified by NIZNHY-PKV Tue Mar 20 10:36:38 2012
 391   /*
 392   R = C2.Radius();
 393   A5 = R*R*Dx*Dy;
 394   A1 = -2.*A5;
 395   A2 = R*R*(Dx*Dx-Dy*Dy)/2.;
 396   A3 = R*Vxyz.Y();
 397   A4 = -R*Vxyz.X();
 398   //
 399   aTol=1.e-12;
 400   //
 401
 402   if(fabs(A5) <= aTol) {
 403     A5 = 0.;
 404   }
 405   if(fabs(A1) <= aTol) {
 406     A1 = 0.;
 407   }
 408   if(fabs(A2) <= aTol) {
 409     A2 = 0.;
 410   }
 411   if(fabs(A3) <= aTol) {
 412     A3 = 0.;
 413   }
 414   if(fabs(A4) <= aTol) {
 415     A4 = 0.;
 416   }
 417   */
 418   //
 419   aTol=1.e-12;
 420   // Calculate the coefficients of the equation by Cos and Sin ...
 421   // [divided by R]
 422   R = C2.Radius();
 423   A5 = R*Dx*Dy;
 424   A1 = -2.*A5;
 425   A2 = 0.5*R*(Dx*Dx-Dy*Dy);// /2.;
 426   A3 = Vxyz.Y();
 427   A4 = -Vxyz.X();
 428   //
 429   if (A1>=-aTol && A1<=aTol) {
 430     A1 = 0.;
 431   }
 432   if (A2>=-aTol && A2<=aTol) {
 433     A2 = 0.;
 434   }
 435   if (A3>=-aTol && A3<=aTol) {
 436     A3 = 0.;
 437   }
 438   if (A4>=-aTol && A4<=aTol) {
 439     A4 = 0.;
 440   }
 441   if (A5>=-aTol && A5<=aTol) {
 442     A5 = 0.;
 443   }
 444   //modified by NIZNHY-PKV Tue Mar 20 10:36:40 2012t
 445   //
 446   ExtremaExtElC_TrigonometricRoots Sol(A1, A2, A3, A4, A5, 0., M_PI+M_PI);
 447   if (!Sol.IsDone()) {
 448     return;
 449   }
 450   if (Sol.InfiniteRoots()) {
 451     myIsPar = Standard_True;
 452     mySqDist[0] = R*R;
 453     myDone = Standard_True;
 454     return;
 455   }
 456   // Storage of solutions ...
 457   Standard_Integer NoSol, NbSol;
 458   Standard_Real U1,U2;
 459   gp_Pnt P1,P2;
 460   //
 461   NbSol = Sol.NbSolutions();
 462   for (NoSol=1; NoSol<=NbSol; ++NoSol) {
 463     U2 = Sol.Value(NoSol);
 464     P2 = ElCLib::Value(U2,C2);
 465     U1 = (gp_Vec(O1,P2)).Dot(D1);
 466     P1 = ElCLib::Value(U1,C1);
 467     mySqDist[myNbExt] = P1.SquareDistance(P2);
 468     //modified by NIZNHY-PKV Wed Mar 21 08:11:33 2012f
 469     //myPoint[myNbExt][0] = Extrema_POnCurv(U1,P1);
 470     //myPoint[myNbExt][1] = Extrema_POnCurv(U2,P2);
 471     myPoint[myNbExt][0].SetValues(U1,P1);
 472     myPoint[myNbExt][1].SetValues(U2,P2);
 473     //modified by NIZNHY-PKV Wed Mar 21 08:11:36 2012t
 474     myNbExt++;
 475   }
 476   myDone = Standard_True;
 477 }
 478 //=======================================================================
 479 //function : Extrema_ExtElC
 480 //purpose  :
 481 //=======================================================================
 482 Extrema_ExtElC::Extrema_ExtElC (const gp_Lin& C1,
 483                                 const gp_Elips& C2)
 484 {
 485 /*-----------------------------------------------------------------------------
 486 Function:
 487   Find extreme distances between straight line C1 and ellipse C2.
 488
 489 Method:
 490   Let P1=C1(u1) and P2=C2(u2) two solution points
 491         D the direction of straight line C1
 492         T the tangent to point P2;
 493   Then, ( P1P2.D = 0. (1)
 494          ( P1P2.T = 0. (2)
 495   Let O1 and O2 be the origins of C1 and C2;
 496   Then, (1) <=> (O1P2-u1*D).D = 0.        as O1P1 = u1*D
 497              <=> u1 = O1P2.D              as D.D = 1.
 498          (2) <=> P1O2.T = 0.              as O2P2.T = 0.
 499              <=> ((P2O1.D)D+O1O2).T = 0.  as P1O1 = -u1*D = (P2O1.D)D
 500              <=> (((P2O2+O2O1).D)D+O1O2).T = 0.
 501              <=> ((P2O2.D)(D.T)+((O2O1.D)D-O2O1).T = 0.
 502   We are in the reference of the ellipse; let:
 503          Cos = Cos(u2) and Sin = Sin(u2),
 504          P2 (MajR*Cos,MinR*Sin,0.),
 505          T (-MajR*Sin,MinR*Cos,0.),
 506          D (Dx,Dy,Dz),
 507          V (Vx,Vy,Vz) = (O2O1.D)D-O2O1;
 508   Then,  get the following equation by Cos and Sin:
 509     -(2*MajR*MinR*Dx*Dy)             * Cos**2  +
 510    (MajR*MajR*Dx**2-MinR*MinR*Dy**2) * Cos*Sin +
 511          MinR*Vy                     * Cos     +
 512        - MajR*Vx                     * Sin     +
 513       MinR*MajR*Dx*Dy                = 0.
 514   Use algorithm math_TrigonometricFunctionRoots to solve this equation.
 515 -----------------------------------------------------------------------------*/
 516   myIsPar = Standard_False;
 517   myDone = Standard_False;
 518   myNbExt = 0;
 519
 520 // Calculate T1 the reference of the ellipse ...
 521   gp_Dir D = C1.Direction();
 522   gp_Dir D1 = D;
 523   gp_Dir x2, y2, z2;
 524   x2 = C2.XAxis().Direction();
 525   y2 = C2.YAxis().Direction();
 526   z2 = C2.Axis().Direction();
 527   Standard_Real Dx = D.Dot(x2);
 528   Standard_Real Dy = D.Dot(y2);
 529   Standard_Real Dz = D.Dot(z2);
 530   D.SetCoord(Dx,Dy,Dz);
 531
 532 // Calculate V ...
 533   gp_Pnt O1 = C1.Location();
 534   gp_Pnt O2 = C2.Location();
 535   gp_Vec O2O1 (O2,O1);
 536   O2O1.SetCoord(O2O1.Dot(x2), O2O1.Dot(y2), O2O1.Dot(z2));
 537   gp_XYZ Vxyz = (D.XYZ()*(O2O1.Dot(D)))-O2O1.XYZ();
 538
 539 // Calculate the coefficients of the equation by Cos and Sin ...
 540   Standard_Real MajR = C2.MajorRadius();
 541   Standard_Real MinR = C2.MinorRadius();
 542   Standard_Real A5 = MajR*MinR*Dx*Dy;
 543   Standard_Real A1 = -2.*A5;
 544   Standard_Real R2 = MajR*MajR;
 545   Standard_Real r2 = MinR*MinR;
 546   Standard_Real A2 =(R2*Dx*Dx -r2*Dy*Dy -R2 +r2)/2.0;
 547   Standard_Real A3 = MinR*Vxyz.Y();
 548   Standard_Real A4 = -MajR*Vxyz.X();
 549   //
 550   Standard_Real aEps=1.e-12;
 551   //
 552   if(fabs(A5) <= aEps) A5 = 0.;
 553   if(fabs(A1) <= aEps) A1 = 0.;
 554   if(fabs(A2) <= aEps) A2 = 0.;
 555   if(fabs(A3) <= aEps) A3 = 0.;
 556   if(fabs(A4) <= aEps) A4 = 0.;
 557   //
 558   ExtremaExtElC_TrigonometricRoots Sol(A1,A2,A3,A4,A5,0.,M_PI+M_PI);
 559   if (!Sol.IsDone()) { return; }
 560
 561 // Storage of solutions ...
 562   gp_Pnt P1,P2;
 563   Standard_Real U1,U2;
 564   Standard_Integer NbSol = Sol.NbSolutions();
 565   for (Standard_Integer NoSol = 1; NoSol <= NbSol; NoSol++) {
 566     U2 = Sol.Value(NoSol);
 567     P2 = ElCLib::Value(U2,C2);
 568     U1 = (gp_Vec(O1,P2)).Dot(D1);
 569     P1 = ElCLib::Value(U1,C1);
 570     mySqDist[myNbExt] = P1.SquareDistance(P2);
 571     myPoint[myNbExt][0] = Extrema_POnCurv(U1,P1);
 572     myPoint[myNbExt][1] = Extrema_POnCurv(U2,P2);
 573     myNbExt++;
 574   }
 575   myDone = Standard_True;
 576 }
 577
 578 //=======================================================================
 579 //function : Extrema_ExtElC
 580 //purpose  :
 581 //=======================================================================
 582 Extrema_ExtElC::Extrema_ExtElC (const gp_Lin& C1,
 583                                 const gp_Hypr& C2)
 584 {
 585 /*-----------------------------------------------------------------------------
 586 Function:
 587   Find extrema between straight line C1 and hyperbola C2.
 588
 589 Method:
 590   Let P1=C1(u1) and P2=C2(u2) be two solution points
 591         D the direction of straight line C1
 592         T the tangent at point P2;
 593   Then, ( P1P2.D = 0. (1)
 594         ( P1P2.T = 0. (2)
 595   Let O1 and O2 be the origins of C1 and C2;
 596   Then, (1) <=> (O1P2-u1*D).D = 0.         as O1P1 = u1*D
 597              <=> u1 = O1P2.D               as D.D = 1.
 598          (2) <=> (P1O2 + O2P2).T= 0.
 599              <=> ((P2O1.D)D+O1O2 + O2P2).T = 0.  as P1O1 = -u1*D = (P2O1.D)D
 600              <=> (((P2O2+O2O1).D)D+O1O2 + O2P2).T = 0.
 601              <=> (P2O2.D)(D.T)+((O2O1.D)D-O2O1).T + O2P2.T= 0.
 602   We are in the reference of the hyperbola; let:
 603          by writing P (R* Chu, r* Shu, 0.0)
 604          and Chu = (v**2 + 1)/(2*v) ,
 605              Shu = (V**2 - 1)/(2*v)
 606
 607          T(R*Shu, r*Chu)
 608          D (Dx,Dy,Dz),
 609          V (Vx,Vy,Vz) = (O2O1.D)D-O2O1;
 610
 611   Then we obtain the following equation by v:
 612          (-2*R*r*Dx*Dy - R*R*Dx*Dx-r*r*Dy*Dy + R*R + r*r)     * v**4  +
 613          (2*R*Vx + 2*r*Vy)                                    * v**3  +
 614          (-2*R*Vx + 2*r*Vy)                                   * v     +
 615          (-2*R*r*Dx*Dy - (R*R*Dx*Dx-r*r*Dy*Dy + R*R + r*r))  = 0
 616
 617
 618   Use the algorithm math_DirectPolynomialRoots to solve this equation.
 619 -----------------------------------------------------------------------------*/
 620   myIsPar = Standard_False;
 621   myDone = Standard_False;
 622   myNbExt = 0;
 623
 624 // Calculate T1 in the reference of the hyperbola...
 625   gp_Dir D = C1.Direction();
 626   gp_Dir D1 = D;
 627   gp_Dir x2, y2, z2;
 628   x2 = C2.XAxis().Direction();
 629   y2 = C2.YAxis().Direction();
 630   z2 = C2.Axis().Direction();
 631   Standard_Real Dx = D.Dot(x2);
 632   Standard_Real Dy = D.Dot(y2);
 633   Standard_Real Dz = D.Dot(z2);
 634   D.SetCoord(Dx,Dy,Dz);
 635
 636 // Calculate V ...
 637   gp_Pnt O1 = C1.Location();
 638   gp_Pnt O2 = C2.Location();
 639   gp_Vec O2O1 (O2,O1);
 640   O2O1.SetCoord(O2O1.Dot(x2), O2O1.Dot(y2), O2O1.Dot(z2));
 641   gp_XYZ Vxyz = (D.XYZ()*(O2O1.Dot(D)))-O2O1.XYZ();
 642   Standard_Real Vx = Vxyz.X();
 643   Standard_Real Vy = Vxyz.Y();
 644
 645 // Calculate coefficients of the equation by v
 646   Standard_Real R = C2.MajorRadius();
 647   Standard_Real r = C2.MinorRadius();
 648   Standard_Real a = -2*R*r*Dx*Dy;
 649   Standard_Real b = -R*R*Dx*Dx - r*r*Dy*Dy + R*R + r*r;
 650   Standard_Real A1 = a + b;
 651   Standard_Real A2 = 2*R*Vx + 2*r*Vy;
 652   Standard_Real A4 = -2*R*Vx + 2*r*Vy;
 653   Standard_Real A5 = a - b;
 654
 655   math_DirectPolynomialRoots Sol(A1,A2,0.0,A4, A5);
 656   if (!Sol.IsDone()) { return; }
 657
 658 // Store solutions ...
 659   gp_Pnt P1,P2;
 660   Standard_Real U1,U2, v;
 661   Standard_Integer NbSol = Sol.NbSolutions();
 662   for (Standard_Integer NoSol = 1; NoSol <= NbSol; NoSol++) {
 663     v = Sol.Value(NoSol);
 664     if (v > 0.0) {
 665       U2 = Log(v);
 666       P2 = ElCLib::Value(U2,C2);
 667       U1 = (gp_Vec(O1,P2)).Dot(D1);
 668       P1 = ElCLib::Value(U1,C1);
 669       mySqDist[myNbExt] = P1.SquareDistance(P2);
 670       myPoint[myNbExt][0] = Extrema_POnCurv(U1,P1);
 671       myPoint[myNbExt][1] = Extrema_POnCurv(U2,P2);
 672       myNbExt++;
 673     }
 674   }
 675   myDone = Standard_True;
 676 }
 677 //=======================================================================
 678 //function : Extrema_ExtElC
 679 //purpose  :
 680 //=======================================================================
 681 Extrema_ExtElC::Extrema_ExtElC (const gp_Lin& C1,
 682                                 const gp_Parab& C2)
 683 {
 684 /*-----------------------------------------------------------------------------
 685 Function:
 686   Find extreme distances between straight line C1 and parabole C2.
 687
 688 Method:
 689    Let P1=C1(u1) and P2=C2(u2) be two solution points
 690         D the direction of straight line C1
 691         T the tangent to point P2;
 692   Then, ( P1P2.D = 0. (1)
 693         ( P1P2.T = 0. (2)
 694   Let O1 and O2 be the origins of C1 and C2;
 695   Then, (1) <=> (O1P2-u1*D).D = 0.         as O1P1 = u1*D
 696              <=> u1 = O1P2.D               as D.D = 1.
 697          (2) <=> (P1O2 + O2P2).T= 0.
 698              <=> ((P2O1.D)D+O1O2 + O2P2).T = 0.  as P1O1 = -u1*D = (P2O1.D)D
 699              <=> (((P2O2+O2O1).D)D+O1O2 + O2P2).T = 0.
 700              <=> (P2O2.D)(D.T)+((O2O1.D)D-O2O1).T + O2P2.T = 0.
 701   We are in the reference of the parabola; let:
 702          P2 (y*y/(2*p), y, 0)
 703          T (y/p, 1, 0)
 704          D (Dx,Dy,Dz),
 705          V (Vx,Vy,Vz) = (O2O1.D)D-O2O1;
 706
 707   Then, get the following equation by y:
 708      ((1-Dx*Dx)/(2*p*p))            *  y*y*y  +        A1
 709      (-3*Dx*Dy/(2*p))               *  y*y    +        A2
 710      (1-Dy*Dy + Vx/p)               *  y      +        A3
 711         Vy                          = 0.               A4
 712
 713   Use the algorithm math_DirectPolynomialRoots to solve this equation.
 714 -----------------------------------------------------------------------------*/
 715   myIsPar = Standard_False;
 716   myDone = Standard_False;
 717   myNbExt = 0;
 718
 719 // Calculate T1 in the reference of the parabola...
 720   gp_Dir D = C1.Direction();
 721   gp_Dir D1 = D;
 722   gp_Dir x2, y2, z2;
 723   x2 = C2.XAxis().Direction();
 724   y2 = C2.YAxis().Direction();
 725   z2 = C2.Axis().Direction();
 726   Standard_Real Dx = D.Dot(x2);
 727   Standard_Real Dy = D.Dot(y2);
 728   Standard_Real Dz = D.Dot(z2);
 729   D.SetCoord(Dx,Dy,Dz);
 730
 731 // Calculate V ...
 732   gp_Pnt O1 = C1.Location();
 733   gp_Pnt O2 = C2.Location();
 734   gp_Vec O2O1 (O2,O1);
 735   O2O1.SetCoord(O2O1.Dot(x2), O2O1.Dot(y2), O2O1.Dot(z2));
 736   gp_XYZ Vxyz = (D.XYZ()*(O2O1.Dot(D)))-O2O1.XYZ();
 737
 738 // Calculate coefficients of the equation by y
 739   Standard_Real P = C2.Parameter();
 740   Standard_Real A1 = (1-Dx*Dx)/(2.0*P*P);
 741   Standard_Real A2 = (-3.0*Dx*Dy/(2.0*P));
 742   Standard_Real A3 = (1 - Dy*Dy + Vxyz.X()/P);
 743   Standard_Real A4 = Vxyz.Y();
 744
 745   math_DirectPolynomialRoots Sol(A1,A2,A3,A4);
 746   if (!Sol.IsDone()) { return; }
 747
 748 // Storage of solutions ...
 749   gp_Pnt P1,P2;
 750   Standard_Real U1,U2;
 751   Standard_Integer NbSol = Sol.NbSolutions();
 752   for (Standard_Integer NoSol = 1; NoSol <= NbSol; NoSol++) {
 753     U2 = Sol.Value(NoSol);
 754     P2 = ElCLib::Value(U2,C2);
 755     U1 = (gp_Vec(O1,P2)).Dot(D1);
 756     P1 = ElCLib::Value(U1,C1);
 757     mySqDist[myNbExt] = P1.SquareDistance(P2);
 758     myPoint[myNbExt][0] = Extrema_POnCurv(U1,P1);
 759     myPoint[myNbExt][1] = Extrema_POnCurv(U2,P2);
 760     myNbExt++;
 761   }
 762   myDone = Standard_True;
 763 }
 764 //=======================================================================
 765 //function : Extrema_ExtElC
 766 //purpose  :
 767 //=======================================================================
 768 Extrema_ExtElC::Extrema_ExtElC (const gp_Circ& C1,
 769                                 const gp_Circ& C2)
 770 {
 771   Standard_Boolean bIsSamePlane, bIsSameAxe;
 772   Standard_Real aTolD, aTolD2, aTolA, aD2, aDC2;
 773   gp_Pnt aPc1, aPc2;
 774   gp_Dir aDc1, aDc2;
 775   //
 776   myIsPar = Standard_False;
 777   myDone = Standard_False;
 778   myNbExt = 0;
 779   //
 780   aTolA=Precision::Angular();
 781   aTolD=Precision::Confusion();
 782   aTolD2=aTolD*aTolD;
 783   //
 784   aPc1=C1.Location();
 785   aDc1=C1.Axis().Direction();
 786
 787   aPc2=C2.Location();
 788   aDc2=C2.Axis().Direction();
 789   gp_Pln aPlc1(aPc1, aDc1);
 790   //
 791   aD2=aPlc1.SquareDistance(aPc2);
 792   bIsSamePlane=aDc1.IsParallel(aDc2, aTolA) && aD2<aTolD2;
 793   if (!bIsSamePlane) {
 794     return;
 795   }
 796   //
 797   aDC2=aPc1.SquareDistance(aPc2);
 798   bIsSameAxe=aDC2<aTolD2;
 799   //
 800   if(bIsSameAxe) {
 801     myIsPar = Standard_True;
 802     Standard_Real dR = C1.Radius() - C2.Radius();
 803     Standard_Real dC = C1.Location().Distance(C2.Location());
 804     mySqDist[0] = dR*dR + dC*dC;
 805     dR = C1.Radius() + C2.Radius();
 806     mySqDist[1] = dR*dR + dC*dC;
 807     myDone = Standard_True;
 808   }
 809   else {
 810     Standard_Boolean bIn, bOut;
 811     Standard_Integer j1, j2;
 812     Standard_Real aR1, aR2, aD12, aT11, aT12, aT21, aT22;
 813     gp_Circ aC1, aC2;
 814     gp_Pnt aP11, aP12, aP21, aP22;
 815     //
 816     myDone = Standard_True;
 817     //
 818     aR1=C1.Radius();
 819     aR2=C2.Radius();
 820     //
 821     j1=0;
 822     j2=1;
 823     aC1=C1;
 824     aC2=C2;
 825     if (aR2>aR1) {
 826       j1=1;
 827       j2=0;
 828       aC1=C2;
 829       aC2=C1;
 830     }
 831     //
 832     aR1=aC1.Radius(); // max radius
 833     aR2=aC2.Radius(); // min radius
 834     //
 835     aPc1=aC1.Location();
 836     aPc2=aC2.Location();
 837     //
 838     aD12=aPc1.Distance(aPc2);
 839     gp_Vec aVec12(aPc1, aPc2);
 840     gp_Dir aDir12(aVec12);
 841     //
 842     // 1. Four common solutions
 843     myNbExt=4;
 844     //
 845     aP11.SetXYZ(aPc1.XYZ()-aR1*aDir12.XYZ());
 846     aP12.SetXYZ(aPc1.XYZ()+aR1*aDir12.XYZ());
 847     aP21.SetXYZ(aPc2.XYZ()-aR2*aDir12.XYZ());
 848     aP22.SetXYZ(aPc2.XYZ()+aR2*aDir12.XYZ());
 849     //
 850     aT11=ElCLib::Parameter(aC1, aP11);
 851     aT12=ElCLib::Parameter(aC1, aP12);
 852     aT21=ElCLib::Parameter(aC2, aP21);
 853     aT22=ElCLib::Parameter(aC2, aP22);
 854     //
 855     // P11, P21
 856     myPoint[0][j1].SetValues(aT11, aP11);
 857     myPoint[0][j2].SetValues(aT21, aP21);
 858     mySqDist[0]=aP11.SquareDistance(aP21);
 859     // P11, P22
 860     myPoint[1][j1].SetValues(aT11, aP11);
 861     myPoint[1][j2].SetValues(aT22, aP22);
 862     mySqDist[1]=aP11.SquareDistance(aP22);
 863     //
 864     // P12, P21
 865     myPoint[2][j1].SetValues(aT12, aP12);
 866     myPoint[2][j2].SetValues(aT21, aP21);
 867     mySqDist[2]=aP12.SquareDistance(aP21);
 868     //
 869     // P12, P22
 870     myPoint[3][j1].SetValues(aT12, aP12);
 871     myPoint[3][j2].SetValues(aT22, aP22);
 872     mySqDist[3]=aP12.SquareDistance(aP22);
 873     //
 874     // 2. Check for intersections
 875     bOut=aD12>(aR1+aR2+aTolD);
 876     bIn =aD12<(aR1-aR2-aTolD);
 877     if (!bOut && !bIn) {
 878       Standard_Boolean bNbExt6;
 879       Standard_Real aAlpha, aBeta, aT[2], aVal, aDist2;
 880       gp_Pnt aPt, aPL1, aPL2;
 881       gp_Dir aDLt;
 882       //
 883       aAlpha=0.5*(aR1*aR1-aR2*aR2+aD12*aD12)/aD12;
 884       aVal=aR1*aR1-aAlpha*aAlpha;
 885       if (aVal<0.) {// see pkv/900/L4 for details
 886         aVal=-aVal;
 887       }
 888       aBeta=Sqrt(aVal);
 889       //aBeta=Sqrt(aR1*aR1-aAlpha*aAlpha);
 890       //--
 891       aPt.SetXYZ(aPc1.XYZ()+aAlpha*aDir12.XYZ());
 892       //
 893       aDLt=aDc1^aDir12;
 894       aPL1.SetXYZ(aPt.XYZ()+aBeta*aDLt.XYZ());
 895       aPL2.SetXYZ(aPt.XYZ()-aBeta*aDLt.XYZ());
 896       //
 897       aDist2=aPL1.SquareDistance(aPL2);
 898       bNbExt6=aDist2>aTolD2;
 899       //
 900       myNbExt=5;// just in case. see pkv/900/L4 for details
 901       aT[j1]=ElCLib::Parameter(aC1, aPL1);
 902       aT[j2]=ElCLib::Parameter(aC2, aPL1);
 903       myPoint[4][j1].SetValues(aT[j1], aPL1);
 904       myPoint[4][j2].SetValues(aT[j2], aPL1);
 905       mySqDist[4]=0.;
 906       //
 907       if (bNbExt6) {
 908         myNbExt=6;
 909         aT[j1]=ElCLib::Parameter(aC1, aPL2);
 910         aT[j2]=ElCLib::Parameter(aC2, aPL2);
 911         myPoint[5][j1].SetValues(aT[j1], aPL2);
 912         myPoint[5][j2].SetValues(aT[j2], aPL2);
 913         mySqDist[5]=0.;
 914       }
 915       //
 916     }// if (!bOut || !bIn) {
 917   }// else
 918 }
 919 //=======================================================================
 920 //function : Extrema_ExtElC
 921 //purpose  :
 922 //=======================================================================
 923 Extrema_ExtElC::Extrema_ExtElC (const gp_Circ&, const gp_Elips&)
 924 {
 925   Standard_NotImplemented::Raise();
 926 }
 927 //=======================================================================
 928 //function : Extrema_ExtElC
 929 //purpose  :
 930 //=======================================================================
 931 Extrema_ExtElC::Extrema_ExtElC (const gp_Circ&, const gp_Hypr&)
 932 {
 933   Standard_NotImplemented::Raise();
 934 }
 935 //=======================================================================
 936 //function : Extrema_ExtElC
 937 //purpose  :
 938 //=======================================================================
 939 Extrema_ExtElC::Extrema_ExtElC (const gp_Circ&, const gp_Parab&)
 940 {
 941   Standard_NotImplemented::Raise();
 942 }
 943 //=======================================================================
 944 //function : Extrema_ExtElC
 945 //purpose  :
 946 //=======================================================================
 947 Extrema_ExtElC::Extrema_ExtElC (const gp_Elips&, const gp_Elips&)
 948 {
 949   Standard_NotImplemented::Raise();
 950 }
 951 //=======================================================================
 952 //function : Extrema_ExtElC
 953 //purpose  :
 954 //=======================================================================
 955 Extrema_ExtElC::Extrema_ExtElC (const gp_Elips&, const gp_Hypr&)
 956 {
 957   Standard_NotImplemented::Raise();
 958 }
 959 //=======================================================================
 960 //function : Extrema_ExtElC
 961 //purpose  :
 962 //=======================================================================
 963 Extrema_ExtElC::Extrema_ExtElC (const gp_Elips&, const gp_Parab&)
 964 {
 965   Standard_NotImplemented::Raise();
 966 }
 967 //=======================================================================
 968 //function : Extrema_ExtElC
 969 //purpose  :
 970 //=======================================================================
 971 Extrema_ExtElC::Extrema_ExtElC (const gp_Hypr&, const gp_Hypr&)
 972 {
 973   Standard_NotImplemented::Raise();
 974 }
 975 //=======================================================================
 976 //function : Extrema_ExtElC
 977 //purpose  :
 978 //=======================================================================
 979 Extrema_ExtElC::Extrema_ExtElC (const gp_Hypr&, const gp_Parab&)
 980 {
 981   Standard_NotImplemented::Raise();
 982 }
 983 //=======================================================================
 984 //function : Extrema_ExtElC
 985 //purpose  :
 986 //=======================================================================
 987 Extrema_ExtElC::Extrema_ExtElC (const gp_Parab&, const gp_Parab&)
 988 {
 989   Standard_NotImplemented::Raise();
 990 }
 991 //=======================================================================
 992 //function : IsDone
 993 //purpose  :
 994 //=======================================================================
 995 Standard_Boolean Extrema_ExtElC::IsDone () const {
 996   return myDone;
 997 }
 998 //=======================================================================
 999 //function : IsParallel
1000 //purpose  :
1001 //=======================================================================
1002 Standard_Boolean Extrema_ExtElC::IsParallel () const
1003 {
1004   if (!IsDone()) {
1005     StdFail_NotDone::Raise();
1006   }
1007   return myIsPar;
1008 }
1009 //=======================================================================
1010 //function : NbExt
1011 //purpose  :
1012 //=======================================================================
1013 Standard_Integer Extrema_ExtElC::NbExt () const
1014 {
1015   if (IsParallel()) {
1016     StdFail_InfiniteSolutions::Raise();
1017   }
1018   return myNbExt;
1019 }
1020 //=======================================================================
1021 //function : SquareDistance
1022 //purpose  :
1023 //=======================================================================
1024 Standard_Real Extrema_ExtElC::SquareDistance (const Standard_Integer N) const
1025 {
1026   if (!myDone) {
1027     StdFail_NotDone::Raise();
1028   }
1029   if (myIsPar) {
1030     if (N < 1 || N > 2) {
1031       Standard_OutOfRange::Raise();
1032     }
1033   }
1034   else {
1035     if (N < 1 || N > NbExt()) {
1036       Standard_OutOfRange::Raise();
1037     }
1038   }
1039   return mySqDist[N-1];
1040 }
1041 //=======================================================================
1042 //function : Points
1043 //purpose  :
1044 //=======================================================================
1045 void Extrema_ExtElC::Points (const Standard_Integer N,
1046                              Extrema_POnCurv& P1,
1047                              Extrema_POnCurv& P2) const
1048 {
1049   if (N < 1 || N > NbExt()) {
1050     Standard_OutOfRange::Raise();
1051   }
1052   P1 = myPoint[N-1][0];
1053   P2 = myPoint[N-1][1];
1054 }
1055
1056
1057 //=======================================================================
1058 //function : RefineDir
1059 //purpose  :
1060 //=======================================================================
1061 void RefineDir(gp_Dir& aDir)
1062 {
1063   Standard_Integer i, j, k, iK;
1064   Standard_Real aCx[3], aEps, aX1, aX2, aOne;
1065   //
1066   iK=3;
1067   aEps=RealEpsilon();
1068   aDir.Coord(aCx[0], aCx[1], aCx[2]);
1069   //
1070   for (i=0; i<iK; ++i) {
1071     aOne=(aCx[i]>0.) ? 1. : -1.;
1072     aX1=aOne-aEps;
1073     aX2=aOne+aEps;
1074     //
1075     if (aCx[i]>aX1 && aCx[i]<aX2) {
1076       j=(i+1)%iK;
1077       k=(i+2)%iK;
1078       aCx[i]=aOne;
1079       aCx[j]=0.;
1080       aCx[k]=0.;
1081       aDir.SetCoord(aCx[0], aCx[1], aCx[2]);
1082       return;
1083     }
1084   }
1085 }