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