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