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