1 // Copyright (c) 1997-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
16 #define No_Standard_RangeError
17 #define No_Standard_OutOfRange
18 #define No_Standard_DimensionError
22 #include <math_DirectPolynomialRoots.hxx>
23 #include <Standard_RangeError.hxx>
24 #include <StdFail_InfiniteSolutions.hxx>
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.
30 const Standard_Real ZERO = 1.0e-30;
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);
35 static Standard_Real Value(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X) {
37 Standard_Real Result = Poly[0];
38 for(Standard_Integer Index = 1; Index < N; Index++) {
39 Result = Result * X + Poly[Index];
45 static void Values(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X,
46 Standard_Real& Val, Standard_Real& Der) {
48 Val = Poly[0] * X + Poly[1];
50 for(Standard_Integer Index = 2; Index < N; Index++) {
52 Val = Val * X + Poly[Index];
56 static Standard_Real Improve(const Standard_Integer N, Standard_Real *Poly, const Standard_Real IniSol) {
58 Standard_Real Val = 0., Der, Delta;
59 Standard_Real Sol = IniSol;
60 Standard_Real IniVal = Value(N, Poly, IniSol);
61 Standard_Integer Index;
63 // std::cout << "Improve\n";
64 for(Index = 1; Index < 10; Index++) {
65 Values(N, Poly, Sol, Val, Der);
66 if(Abs(Der) <= ZERO) break;
68 if(Abs(Delta) <= EPSILON * Abs(Sol)) break;
70 // std::cout << " Iter = " << Index << " Delta = " << Delta
71 // << " Val = " << Val << " Der = " << Der << "\n";
73 if(Abs(Val) <= Abs(IniVal)) {
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) {
84 Standard_Real Poly[5];
90 return Improve(5, Poly, IniSol);
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) {
96 Standard_Real Poly[4];
101 return Improve(4, Poly, IniSol);
104 Standard_Real Improve(const Standard_Real A, const Standard_Real B,
105 const Standard_Real C, const Standard_Real IniSol) {
107 Standard_Real Poly[3];
111 return Improve(3, Poly, IniSol);
114 Standard_Integer BaseExponent(const Standard_Real X) {
117 return (Standard_Integer)(log(X) * Un_Sur_Log_RADIX);
120 return (Standard_Integer)(-log(-X) * Un_Sur_Log_RADIX);
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);
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;
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;
155 math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A,
156 const Standard_Real B) {
157 Done = Standard_True;
158 InfiniteStatus = Standard_False;
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) {
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);
186 aZero = Epsilon(100.*aZero);
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;
206 ///////////////////////////////////
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;
218 Exp = BaseExponent(D) / 4;
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;
227 C/= PowRadix2 * PowRadix1;
228 D/= PowRadix2 * PowRadix2;
231 S3 = A * C - 4.0 * D;
232 T3 = D * (4.0 * B - A * A) - C * C;
234 math_DirectPolynomialRoots Sol3(Q3, R3, S3, T3);
235 //-- ################################################################################
236 if(Sol3.IsDone() == Standard_False) { Done = Standard_False; return; }
237 //-- ################################################################################
241 for(Index = 2; Index <= Sol3.NbSolutions(); Index++) {
242 if(Sol3.Value(Index) > Y0) Y0 = Sol3.Value(Index);
244 Discr = A * Y0 * 0.5 - C;
251 P0 = A * A * 0.25 - B + Y0;
252 if(P0 < 0.0) P0 = 0.0;
254 Q0 = Y0 * Y0 * 0.25 - D;
255 if(Q0 < 0.0) Q0 = 0.0;
258 Standard_Real Ademi = A * 0.5;
259 Standard_Real Ydemi = Y0 * 0.5;
260 Standard_Real SdiscrQ0 = Sdiscr * Q0;
263 Q = Ydemi + SdiscrQ0;
265 Q1 = Ydemi - SdiscrQ0;
266 // Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) Begin
269 eps = Epsilon(100.*Max(Ademi, P0));
275 eps = Epsilon(100.*Max(Ydemi, SdiscrQ0));
280 // Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) End
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 //-- ################################################################################
292 NbSol = ASol2.NbSolutions() + BSol2.NbSolutions();
293 for(Index = 0; Index < ASol2.NbSolutions(); Index++) {
294 TheRoots[Index] = ASol2.TheRoots[Index];
296 for(Index = 0; Index < BSol2.NbSolutions(); Index++) {
297 TheRoots[ASol2.NbSolutions() + Index] = BSol2.TheRoots[Index];
299 for(Index = 0; Index < NbSol; Index++) {
300 TheRoots[Index] = TheRoots[Index] * PowRadix1;
301 TheRoots[Index] = Improve(a, b, c, d, e, TheRoots[Index]);
305 void math_DirectPolynomialRoots::Solve(const Standard_Real A,
306 const Standard_Real B,
307 const Standard_Real C,
308 const Standard_Real D) {
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;
324 Exp = BaseExponent(Del) / 3;
326 Standard_Real PowRadix1 = pow(RADIX,Exp);
327 Standard_Real PowRadix2 = PowRadix1*PowRadix1;
330 Del/= PowRadix2 * PowRadix1;
331 //-- Beta = Beta / pow(RADIX, Exp);
332 //-- Gamma = Gamma / pow(RADIX, 2 * Exp);
333 //-- Del = Del / pow(RADIX, 3 * Exp);
336 P2 = - (Beta * Beta) / 3.0;
338 Ep = 5.0 * EPSILON * (Abs(P1) + Abs(P2));
339 if(Abs(P) <= Ep) P = 0.0;
341 Q2 = - Beta * Gamma / 3.0;
342 Q3 = 2.0 * (Beta * Beta * Beta) / 27.0;
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;
355 Psi = Gamma * Gamma * (4.0 * Gamma - Beta * Beta) / 27.0;
357 D1 = Sigma + 2.0 * sqrt(-A1);
360 D1 = Sigma - 2.0 * sqrt(-A1);
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;
377 if(Beta == 0.0 && Q == 0.0) {
378 TheRoots[0] = sqrt(-P);
379 TheRoots[1] = -TheRoots[0];
383 Omega = atan(0.5 * Q / sqrt(- Discr));
384 Sp3 = sqrt(-P / 3.0);
385 Y1 = -2.0 * Sb * Sp3 * cos(M_PI / 6.0 - Sb * Omega / 3.0);
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);
391 Dbg = Del - Beta * Gamma;
398 Den1 = 8.0 * Beta * Beta / 9.0 - 4.0 * Beta * Y1 / 3.0
400 Den2 = 2.0 * Y1 * Y1 - Q / Y1;
401 TheRoots[1] = Dbg / Den1 + Sdbg * sqrt(-27.0 * Discr) / Den2;
403 TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]);
406 else if(Discr > 0.0) {
408 U = sqrt(Discr) + Abs(Q / 2.0);
410 U = pow(U, 1.0 / 3.0);
413 U = - pow(Abs(U), 1.0 / 3.0);
416 H = U * U + P / 3.0 + (P / U) * (P / U) / 9.0;
419 H = U * Abs(Q) / (U * U - P / 3.0);
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);
426 TheRoots[0] = - Beta / 3.0 - Q / H;
430 TheRoots[0] = - Del / (Beta * Beta / 9.0 + H - Beta * Q / (3.0 * H));
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;
449 TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]);
453 TheRoots[0] = -Gamma / (Beta + 3.0 * Sq * Sp3);
454 TheRoots[1] = TheRoots[0];
455 TheRoots[2] = -Beta / 3.0 - 2.0 * Sq * Sp3;
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]);
464 void math_DirectPolynomialRoots::Solve(const Standard_Real A,
465 const Standard_Real B,
466 const Standard_Real C) {
473 Standard_Real EpsD = 3.0 * EPSILON * (B * B + Abs(4.0 * A * C));
474 Standard_Real Discrim = B * B - 4.0 * A * C;
476 if(Abs(Discrim) <= EpsD) Discrim = 0.0;
480 else if(Discrim == 0.0) {
482 TheRoots[0] = -0.5 * B / A;
483 TheRoots[0] = Improve(A, B, C, TheRoots[0]);
484 TheRoots[1] = TheRoots[0];
489 TheRoots[0] = - (B + sqrt(Discrim)) / (2.0 * A);
492 TheRoots[0] = - (B - sqrt(Discrim)) / (2.0 * A);
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]);
500 void math_DirectPolynomialRoots::Solve(const Standard_Real A,
501 const Standard_Real B) {
504 if (Abs(B) <= ZERO) {
505 InfiniteStatus = Standard_True;
512 TheRoots[0] = -B / A;
515 void math_DirectPolynomialRoots::Dump(Standard_OStream& o) const {
516 o << "math_DirectPolynomialRoots ";
520 else if(InfiniteStatus) {
521 o << " Status = Infinity Roots \n";
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";