src/Extrema/Extrema_ExtElC.cxx

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