src/Extrema/Extrema_ExtElC.cxx

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