b311480e |
1 | // Copyright (c) 1997-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. |
b311480e |
14 | |
0797d9d3 |
15 | //#ifndef OCCT_DEBUG |
7fd59977 |
16 | #define No_Standard_RangeError |
17 | #define No_Standard_OutOfRange |
18 | #define No_Standard_DimensionError |
7fd59977 |
19 | |
42cf5bc1 |
20 | //#endif |
7fd59977 |
21 | |
42cf5bc1 |
22 | #include <math_DirectPolynomialRoots.hxx> |
7fd59977 |
23 | #include <Standard_RangeError.hxx> |
42cf5bc1 |
24 | #include <StdFail_InfiniteSolutions.hxx> |
7fd59977 |
25 | |
b311480e |
26 | // Reference pour solution equation 3ieme degre et 2ieme degre : |
27 | // ALGORITHMES NUMERIQUES ANALYSE ET MISE EN OEUVRE, tome 2 |
28 | // (equations et systemes non lineaires) |
29 | // J. VIGNES editions TECHNIP. |
b311480e |
30 | const Standard_Real ZERO = 1.0e-30; |
7fd59977 |
31 | const Standard_Real EPSILON = RealEpsilon(); |
32 | const Standard_Real RADIX = 2; |
33 | const Standard_Real Un_Sur_Log_RADIX = 1.0/log(2.0); |
34 | |
35 | static Standard_Real Value(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X) { |
36 | |
37 | Standard_Real Result = Poly[0]; |
38 | for(Standard_Integer Index = 1; Index < N; Index++) { |
39 | Result = Result * X + Poly[Index]; |
40 | } |
41 | return Result; |
42 | } |
43 | |
44 | |
45 | static void Values(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X, |
46 | Standard_Real& Val, Standard_Real& Der) { |
47 | |
48 | Val = Poly[0] * X + Poly[1]; |
49 | Der = Poly[0]; |
50 | for(Standard_Integer Index = 2; Index < N; Index++) { |
51 | Der = Der * X + Val; |
52 | Val = Val * X + Poly[Index]; |
53 | } |
54 | } |
55 | |
56 | static Standard_Real Improve(const Standard_Integer N, Standard_Real *Poly, const Standard_Real IniSol) { |
57 | |
302f96fb |
58 | Standard_Real Val = 0., Der, Delta; |
7fd59977 |
59 | Standard_Real Sol = IniSol; |
60 | Standard_Real IniVal = Value(N, Poly, IniSol); |
61 | Standard_Integer Index; |
62 | |
63 | // cout << "Improve\n"; |
64 | for(Index = 1; Index < 10; Index++) { |
65 | Values(N, Poly, Sol, Val, Der); |
66 | if(Abs(Der) <= ZERO) break; |
67 | Delta = - Val / Der; |
68 | if(Abs(Delta) <= EPSILON * Abs(Sol)) break; |
69 | Sol = Sol + Delta; |
70 | // cout << " Iter = " << Index << " Delta = " << Delta |
71 | // << " Val = " << Val << " Der = " << Der << "\n"; |
72 | } |
73 | if(Abs(Val) <= Abs(IniVal)) { |
74 | return Sol; |
75 | } |
76 | else { |
77 | return IniSol; |
78 | } |
79 | } |
80 | |
81 | Standard_Real Improve(const Standard_Real A, const Standard_Real B, const Standard_Real C, |
82 | const Standard_Real D, const Standard_Real E, const Standard_Real IniSol) { |
83 | |
84 | Standard_Real Poly[5]; |
85 | Poly[0] = A; |
86 | Poly[1] = B; |
87 | Poly[2] = C; |
88 | Poly[3] = D; |
89 | Poly[4] = E; |
90 | return Improve(5, Poly, IniSol); |
91 | } |
92 | |
93 | Standard_Real Improve(const Standard_Real A, const Standard_Real B, |
94 | const Standard_Real C, const Standard_Real D, const Standard_Real IniSol) { |
95 | |
96 | Standard_Real Poly[4]; |
97 | Poly[0] = A; |
98 | Poly[1] = B; |
99 | Poly[2] = C; |
100 | Poly[3] = D; |
101 | return Improve(4, Poly, IniSol); |
102 | } |
103 | |
104 | Standard_Real Improve(const Standard_Real A, const Standard_Real B, |
105 | const Standard_Real C, const Standard_Real IniSol) { |
106 | |
107 | Standard_Real Poly[3]; |
108 | Poly[0] = A; |
109 | Poly[1] = B; |
110 | Poly[2] = C; |
111 | return Improve(3, Poly, IniSol); |
112 | } |
113 | |
114 | Standard_Integer BaseExponent(const Standard_Real X) { |
115 | |
116 | if(X > 1.0) { |
117 | return (Standard_Integer)(log(X) * Un_Sur_Log_RADIX); |
118 | } |
119 | else if(X < -1.0) { |
120 | return (Standard_Integer)(-log(-X) * Un_Sur_Log_RADIX); |
121 | } |
122 | else { |
123 | return 0; |
124 | } |
125 | } |
126 | |
127 | |
128 | math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A, |
129 | const Standard_Real B, |
130 | const Standard_Real C, |
131 | const Standard_Real D, |
132 | const Standard_Real E) { |
133 | InfiniteStatus = Standard_False; |
134 | Done = Standard_True; |
135 | Solve(A, B, C, D, E); |
136 | } |
137 | |
138 | math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A, |
139 | const Standard_Real B, |
140 | const Standard_Real C, |
141 | const Standard_Real D) { |
142 | Done = Standard_True; |
143 | InfiniteStatus = Standard_False; |
144 | Solve(A, B, C, D); |
145 | } |
146 | |
147 | math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A, |
148 | const Standard_Real B, |
149 | const Standard_Real C) { |
150 | Done = Standard_True; |
151 | InfiniteStatus = Standard_False; |
152 | Solve(A, B, C); |
153 | } |
154 | |
155 | math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A, |
156 | const Standard_Real B) { |
157 | Done = Standard_True; |
158 | InfiniteStatus = Standard_False; |
159 | Solve(A, B); |
160 | } |
161 | |
162 | |
163 | void math_DirectPolynomialRoots::Solve(const Standard_Real a, |
164 | const Standard_Real b, |
165 | const Standard_Real c, |
166 | const Standard_Real d, |
167 | const Standard_Real e) { |
168 | if(Abs(a) <= ZERO) { |
169 | Solve(b, c, d, e); |
170 | return; |
171 | } |
172 | |
173 | //// modified by jgv, 22.01.09 //// |
174 | Standard_Real aZero = ZERO; |
175 | Standard_Real Abs_b = Abs(b), Abs_c = Abs(c), Abs_d = Abs(d), Abs_e = Abs(e); |
176 | |
177 | if (Abs_b > aZero) |
178 | aZero = Abs_b; |
179 | if (Abs_c > aZero) |
180 | aZero = Abs_c; |
181 | if (Abs_d > aZero) |
182 | aZero = Abs_d; |
183 | if (Abs_e > aZero) |
184 | aZero = Abs_e; |
185 | if (aZero > ZERO) |
186 | aZero = Epsilon(100.*aZero); |
187 | |
188 | if(Abs(a) <= aZero) { |
189 | Standard_Real aZero1000 = 1000.*aZero; |
190 | Standard_Boolean with_a = Standard_False; |
191 | if (Abs_b > ZERO && Abs_b <= aZero1000) |
192 | with_a = Standard_True; |
193 | if (Abs_c > ZERO && Abs_c <= aZero1000) |
194 | with_a = Standard_True; |
195 | if (Abs_d > ZERO && Abs_d <= aZero1000) |
196 | with_a = Standard_True; |
197 | if (Abs_e > ZERO && Abs_e <= aZero1000) |
198 | with_a = Standard_True; |
199 | |
200 | if (!with_a) |
201 | { |
202 | Solve(b, c, d, e); |
203 | return; |
204 | } |
205 | } |
206 | /////////////////////////////////// |
207 | |
208 | Standard_Real A, B, C, D, R3, S3, T3, Q3, Y0, P0, Q0, P, Q, P1, Q1; |
209 | Standard_Real Discr, Sdiscr; |
210 | Standard_Integer Index; |
211 | Standard_Integer Exp; |
212 | Standard_Real PowRadix1,PowRadix2; |
213 | |
214 | A = b / a; |
215 | B = c / a; |
216 | C = d / a; |
217 | D = e / a; |
218 | Exp = BaseExponent(D) / 4; |
219 | //-- |
220 | //-- A = A / pow(RADIX, Exp); |
221 | //-- B = B / pow(RADIX, 2 * Exp); |
222 | //-- C = C / pow(RADIX, 3 * Exp); |
223 | //-- D = D / pow(RADIX, 4 * Exp); |
224 | PowRadix1 = pow(RADIX,Exp); |
225 | A/= PowRadix1; PowRadix2 = PowRadix1 * PowRadix1; |
226 | B/= PowRadix2; |
227 | C/= PowRadix2 * PowRadix1; |
228 | D/= PowRadix2 * PowRadix2; |
229 | //-- |
230 | R3 = -B; |
231 | S3 = A * C - 4.0 * D; |
232 | T3 = D * (4.0 * B - A * A) - C * C; |
233 | Q3 = 1.0; |
234 | math_DirectPolynomialRoots Sol3(Q3, R3, S3, T3); |
235 | //-- ################################################################################ |
236 | if(Sol3.IsDone() == Standard_False) { Done = Standard_False; return; } |
237 | //-- ################################################################################ |
238 | |
239 | |
240 | Y0 = Sol3.Value(1); |
241 | for(Index = 2; Index <= Sol3.NbSolutions(); Index++) { |
242 | if(Sol3.Value(Index) > Y0) Y0 = Sol3.Value(Index); |
243 | } |
244 | Discr = A * Y0 * 0.5 - C; |
245 | if(Discr >= 0.0) { |
246 | Sdiscr = 1.0; |
247 | } |
248 | else { |
249 | Sdiscr = -1.0; |
250 | } |
251 | P0 = A * A * 0.25 - B + Y0; |
252 | if(P0 < 0.0) P0 = 0.0; |
253 | P0 = sqrt(P0); |
254 | Q0 = Y0 * Y0 * 0.25 - D; |
255 | if(Q0 < 0.0) Q0 = 0.0; |
256 | Q0 = sqrt(Q0); |
257 | |
258 | Standard_Real Ademi = A * 0.5; |
259 | Standard_Real Ydemi = Y0 * 0.5; |
260 | Standard_Real SdiscrQ0 = Sdiscr * Q0; |
261 | |
262 | P = Ademi + P0; |
263 | Q = Ydemi + SdiscrQ0; |
264 | P1 = Ademi - P0; |
265 | Q1 = Ydemi - SdiscrQ0; |
266 | // Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) Begin |
267 | Standard_Real eps; |
268 | |
269 | eps = Epsilon(100.*Max(Ademi, P0)); |
270 | if (Abs(P) <= eps) |
271 | P = 0.; |
272 | if (Abs(P1) <= eps) |
273 | P1 = 0.; |
274 | |
275 | eps = Epsilon(100.*Max(Ydemi, SdiscrQ0)); |
276 | if (Abs(Q) <= eps) |
277 | Q = 0.; |
278 | if (Abs(Q1) <= eps) |
279 | Q1 = 0.; |
280 | // Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) End |
281 | Ademi = 1.0; |
282 | |
283 | math_DirectPolynomialRoots ASol2(Ademi, P, Q); |
284 | //-- ################################################################################ |
285 | if(ASol2.IsDone() == Standard_False) { Done = Standard_False; return; } |
286 | //-- ################################################################################ |
287 | math_DirectPolynomialRoots BSol2(Ademi, P1, Q1); |
288 | //-- ################################################################################ |
289 | if(BSol2.IsDone() == Standard_False) { Done = Standard_False; return; } |
290 | //-- ################################################################################ |
291 | |
292 | NbSol = ASol2.NbSolutions() + BSol2.NbSolutions(); |
293 | for(Index = 0; Index < ASol2.NbSolutions(); Index++) { |
294 | TheRoots[Index] = ASol2.TheRoots[Index]; |
295 | } |
296 | for(Index = 0; Index < BSol2.NbSolutions(); Index++) { |
297 | TheRoots[ASol2.NbSolutions() + Index] = BSol2.TheRoots[Index]; |
298 | } |
299 | for(Index = 0; Index < NbSol; Index++) { |
300 | TheRoots[Index] = TheRoots[Index] * PowRadix1; |
301 | TheRoots[Index] = Improve(a, b, c, d, e, TheRoots[Index]); |
302 | } |
303 | } |
304 | |
305 | void math_DirectPolynomialRoots::Solve(const Standard_Real A, |
306 | const Standard_Real B, |
307 | const Standard_Real C, |
308 | const Standard_Real D) { |
309 | |
310 | if(Abs(A) <= ZERO) { |
311 | Solve(B, C, D); |
312 | return; |
313 | } |
314 | |
315 | Standard_Real Beta, Gamma, Del, P1, P2, P, Ep, Q1, Q2, Q3, Q, Eq, A1, A2, Discr; |
316 | Standard_Real Sigma, Psi, D1, D2, Sb, Omega, Sp3, Y1, Dbg, Sdbg, Den1, Den2; |
317 | Standard_Real U, H, Sq; |
318 | Standard_Integer Exp; |
319 | |
320 | Beta = B / A; |
321 | Gamma = C / A; |
322 | Del = D / A; |
323 | |
324 | Exp = BaseExponent(Del) / 3; |
325 | |
326 | Standard_Real PowRadix1 = pow(RADIX,Exp); |
327 | Standard_Real PowRadix2 = PowRadix1*PowRadix1; |
328 | Beta/= PowRadix1; |
329 | Gamma/= PowRadix2; |
330 | Del/= PowRadix2 * PowRadix1; |
331 | //-- Beta = Beta / pow(RADIX, Exp); |
332 | //-- Gamma = Gamma / pow(RADIX, 2 * Exp); |
333 | //-- Del = Del / pow(RADIX, 3 * Exp); |
334 | |
335 | P1 = Gamma; |
336 | P2 = - (Beta * Beta) / 3.0; |
337 | P = P1 + P2; |
338 | Ep = 5.0 * EPSILON * (Abs(P1) + Abs(P2)); |
339 | if(Abs(P) <= Ep) P = 0.0; |
340 | Q1 = Del; |
341 | Q2 = - Beta * Gamma / 3.0; |
342 | Q3 = 2.0 * (Beta * Beta * Beta) / 27.0; |
343 | Q = Q1 + Q2 + Q3; |
344 | Eq = 10.0 * EPSILON * (Abs(Q1) + Abs(Q2) + Abs(Q3)); |
345 | if(Abs(Q) <= Eq) Q = 0.0; |
346 | //-- ############################################################ |
347 | Standard_Real AbsP = P; if(P<0.0) AbsP = -P; |
348 | if(AbsP>1e+80) { Done = Standard_False; return; } |
349 | //-- ############################################################ |
350 | A1 = (P * P * P) / 27.0; |
351 | A2 = (Q * Q) / 4.0; |
352 | Discr = A1 + A2; |
353 | if(P < 0.0) { |
354 | Sigma = - Q2 - Q3; |
355 | Psi = Gamma * Gamma * (4.0 * Gamma - Beta * Beta) / 27.0; |
356 | if(Sigma >= 0.0) { |
357 | D1 = Sigma + 2.0 * sqrt(-A1); |
358 | } |
359 | else { |
360 | D1 = Sigma - 2.0 * sqrt(-A1); |
361 | } |
362 | D2 = Psi / D1; |
363 | Discr = 0.0; |
364 | if(Abs(Del - D1) >= 18.0 * EPSILON * (Abs(Del) + Abs(D1)) && |
365 | Abs(Del - D2) >= 24.0 * EPSILON * (Abs(Del) + Abs(D2))) { |
366 | Discr = (Del - D1) * (Del - D2) / 4.0; |
367 | } |
368 | } |
369 | if(Beta >= 0.0) { |
370 | Sb = 1.0; |
371 | } |
372 | else { |
373 | Sb = -1.0; |
374 | } |
375 | if(Discr < 0.0) { |
376 | NbSol = 3; |
377 | if(Beta == 0.0 && Q == 0.0) { |
378 | TheRoots[0] = sqrt(-P); |
379 | TheRoots[1] = -TheRoots[0]; |
380 | TheRoots[2] = 0.0; |
381 | } |
382 | else { |
383 | Omega = atan(0.5 * Q / sqrt(- Discr)); |
384 | Sp3 = sqrt(-P / 3.0); |
c6541a0c |
385 | Y1 = -2.0 * Sb * Sp3 * cos(M_PI / 6.0 - Sb * Omega / 3.0); |
7fd59977 |
386 | TheRoots[0] = - Beta / 3.0 + Y1; |
387 | if(Beta * Q <= 0.0) { |
388 | TheRoots[1] = - Beta / 3.0 + 2.0 * Sp3 * sin(Omega / 3.0); |
389 | } |
390 | else { |
391 | Dbg = Del - Beta * Gamma; |
392 | if(Dbg >= 0.0) { |
393 | Sdbg = 1.0; |
394 | } |
395 | else { |
396 | Sdbg = -1.0; |
397 | } |
398 | Den1 = 8.0 * Beta * Beta / 9.0 - 4.0 * Beta * Y1 / 3.0 |
399 | - 2.0 * Q / Y1; |
400 | Den2 = 2.0 * Y1 * Y1 - Q / Y1; |
401 | TheRoots[1] = Dbg / Den1 + Sdbg * sqrt(-27.0 * Discr) / Den2; |
402 | } |
403 | TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]); |
404 | } |
405 | } |
406 | else if(Discr > 0.0) { |
407 | NbSol = 1; |
408 | U = sqrt(Discr) + Abs(Q / 2.0); |
409 | if(U >= 0.0) { |
410 | U = pow(U, 1.0 / 3.0); |
411 | } |
412 | else { |
413 | U = - pow(Abs(U), 1.0 / 3.0); |
414 | } |
415 | if(P >= 0.0) { |
416 | H = U * U + P / 3.0 + (P / U) * (P / U) / 9.0; |
417 | } |
418 | else { |
419 | H = U * Abs(Q) / (U * U - P / 3.0); |
420 | } |
421 | if(Beta * Q >= 0.0) { |
422 | if(Abs(H) <= RealSmall() && Abs(Q) <= RealSmall()) { |
423 | TheRoots[0] = - Beta / 3.0 - U + P / (3.0 * U); |
424 | } |
425 | else { |
426 | TheRoots[0] = - Beta / 3.0 - Q / H; |
427 | } |
428 | } |
429 | else { |
430 | TheRoots[0] = - Del / (Beta * Beta / 9.0 + H - Beta * Q / (3.0 * H)); |
431 | } |
432 | } |
433 | else { |
434 | NbSol = 3; |
435 | if(Q >= 0.0) { |
436 | Sq = 1.0; |
437 | } |
438 | else { |
439 | Sq = -1.0; |
440 | } |
441 | Sp3 = sqrt(-P / 3.0); |
442 | if(Beta * Q <= 0.0) { |
443 | TheRoots[0] = -Beta / 3.0 + Sq * Sp3; |
444 | TheRoots[1] = TheRoots[0]; |
445 | if(Beta * Q == 0.0) { |
446 | TheRoots[2] = -Beta / 3.0 - 2.0 * Sq * Sp3; |
447 | } |
448 | else { |
449 | TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]); |
450 | } |
451 | } |
452 | else { |
453 | TheRoots[0] = -Gamma / (Beta + 3.0 * Sq * Sp3); |
454 | TheRoots[1] = TheRoots[0]; |
455 | TheRoots[2] = -Beta / 3.0 - 2.0 * Sq * Sp3; |
456 | } |
457 | } |
458 | for(Standard_Integer Index = 0; Index < NbSol; Index++) { |
459 | TheRoots[Index] = TheRoots[Index] * pow(RADIX, Exp); |
460 | TheRoots[Index] = Improve(A, B, C, D, TheRoots[Index]); |
461 | } |
462 | } |
463 | |
464 | void math_DirectPolynomialRoots::Solve(const Standard_Real A, |
465 | const Standard_Real B, |
466 | const Standard_Real C) { |
467 | |
468 | if(Abs(A) <= ZERO) { |
469 | Solve(B, C); |
470 | return; |
471 | } |
472 | |
473 | Standard_Real EpsD = 3.0 * EPSILON * (B * B + Abs(4.0 * A * C)); |
474 | Standard_Real Discrim = B * B - 4.0 * A * C; |
475 | |
476 | if(Abs(Discrim) <= EpsD) Discrim = 0.0; |
477 | if(Discrim < 0.0) { |
478 | NbSol = 0; |
479 | } |
480 | else if(Discrim == 0.0) { |
481 | NbSol = 2; |
482 | TheRoots[0] = -0.5 * B / A; |
483 | TheRoots[0] = Improve(A, B, C, TheRoots[0]); |
484 | TheRoots[1] = TheRoots[0]; |
485 | } |
486 | else { |
487 | NbSol = 2; |
488 | if(B > 0.0) { |
489 | TheRoots[0] = - (B + sqrt(Discrim)) / (2.0 * A); |
490 | } |
491 | else { |
492 | TheRoots[0] = - (B - sqrt(Discrim)) / (2.0 * A); |
493 | } |
494 | TheRoots[0] = Improve(A, B, C, TheRoots[0]); |
495 | TheRoots[1] = C / (A * TheRoots[0]); |
496 | TheRoots[1] = Improve(A, B, C, TheRoots[1]); |
497 | } |
498 | } |
499 | |
500 | void math_DirectPolynomialRoots::Solve(const Standard_Real A, |
501 | const Standard_Real B) { |
502 | |
503 | if(Abs(A) <= ZERO) { |
504 | if (Abs(B) <= ZERO) { |
505 | InfiniteStatus = Standard_True; |
506 | return; |
507 | } |
508 | NbSol = 0; |
509 | return; |
510 | } |
511 | NbSol = 1; |
512 | TheRoots[0] = -B / A; |
513 | } |
514 | |
515 | void math_DirectPolynomialRoots::Dump(Standard_OStream& o) const { |
516 | o << "math_DirectPolynomialRoots "; |
517 | if (!Done) { |
518 | o <<" Not Done \n"; |
519 | } |
520 | else if(InfiniteStatus) { |
521 | o << " Status = Infinity Roots \n"; |
522 | } |
523 | else if (!InfiniteStatus) { |
524 | o << " Status = Not Infinity Roots \n"; |
525 | o << " Number of solutions = " << NbSol <<"\n"; |
526 | for (Standard_Integer i = 1; i <= NbSol; i++) { |
527 | o << " Solution number " << i << " = " << TheRoots[i-1] <<"\n"; |
528 | } |
529 | } |
530 | } |
531 | |
532 | |
533 | |