src/math/math_DirectPolynomialRoots.cxx

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