src/math/math_DirectPolynomialRoots.cxx

   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
  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
  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.
  36
  37 const Standard_Real ZERO = 1.0e-30;
  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 = 0., 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);
 392           Y1 = -2.0 * Sb * Sp3 * cos(M_PI / 6.0 - Sb * Omega / 3.0);
 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