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