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