src/math/math_DirectPolynomialRoots.cxx

   1 // Copyright (c) 1997-1999 Matra Datavision
   2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
   3 //
   4 // This file is part of Open CASCADE Technology software library.
   5 //
   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.
  11 //
  12 // Alternatively, this file may be used under the terms of Open CASCADE
  13 // commercial license or contractual agreement.
  14
  15 //#ifndef OCCT_DEBUG
  16 #define No_Standard_RangeError
  17 #define No_Standard_OutOfRange
  18 #define No_Standard_DimensionError
  19
  20 //#endif
  21
  22 #include <math_DirectPolynomialRoots.hxx>
  23 #include <Standard_RangeError.hxx>
  24 #include <StdFail_InfiniteSolutions.hxx>
  25
  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);
  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
  58         Standard_Real Val = 0., Der, Delta;
  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);
 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);
 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