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