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