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