src/math/math_TrigonometricFunctionRoots.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 // lpa, le 03/09/91
  16
  17
  18 // Implementation de la classe resolvant les equations en cosinus-sinus.
  19 // Equation de la forme a*cos(x)*cos(x)+2*b*cos(x)*sin(x)+c*cos(x)+d*sin(x)+e
  20
  21 //#ifndef OCCT_DEBUG
  22 #define No_Standard_RangeError
  23 #define No_Standard_OutOfRange
  24 #define No_Standard_DimensionError
  25 //#endif
  26
  27 #include <math_TrigonometricFunctionRoots.hxx>
  28 #include <math_DirectPolynomialRoots.hxx>
  29 #include <Standard_OutOfRange.hxx>
  30 #include <math_FunctionWithDerivative.hxx>
  31 #include <math_NewtonFunctionRoot.hxx>
  32 #include <Precision.hxx>
  33
  34
  35 class MyTrigoFunction: public math_FunctionWithDerivative
  36 {
  37   Standard_Real AA;
  38   Standard_Real BB;
  39   Standard_Real CC;
  40   Standard_Real DD;
  41   Standard_Real EE;
  42
  43  public:
  44   MyTrigoFunction(const Standard_Real A,
  45                   const Standard_Real B,
  46                   const Standard_Real C,
  47                   const Standard_Real D,
  48                   const Standard_Real E)
  49   : AA(A), BB(B), CC(C), DD(D), EE(E)
  50   {
  51   }
  52
  53   Standard_Boolean Value(const Standard_Real X, Standard_Real& F);
  54   Standard_Boolean Derivative(const Standard_Real X, Standard_Real& D);
  55   Standard_Boolean Values(const Standard_Real X, Standard_Real& F, Standard_Real& D);
  56 };
  57
  58  Standard_Boolean MyTrigoFunction::Value(const Standard_Real X, Standard_Real& F) {
  59    Standard_Real CN= cos(X), SN = sin(X);
  60    //-- F= AA*CN*CN+2*BB*CN*SN+CC*CN+DD*SN+EE;
  61    F=CN*(AA*CN + (BB+BB)*SN + CC) + DD*SN + EE;
  62    return Standard_True;
  63  }
  64
  65  Standard_Boolean MyTrigoFunction::Derivative(const Standard_Real X, Standard_Real& D) {
  66    Standard_Real CN= Cos(X), SN = Sin(X);
  67    //-- D = -2*AA*CN*SN+2*BB*(CN*CN-SN*SN)-CC*SN+DD*CN;
  68    D = -AA*CN*SN + BB*(CN*CN-SN*SN);
  69    D+=D;
  70    D-=CC*SN+DD*CN;
  71    return Standard_True;
  72  }
  73
  74  Standard_Boolean MyTrigoFunction::Values(const Standard_Real X, Standard_Real& F, Standard_Real& D) {
  75    Standard_Real CN= Cos(X), SN = Sin(X);
  76    //-- F= AA*CN*CN+2*BB*CN*SN+CC*CN+DD*SN+EE;
  77    //-- D = -2*AA*CN*SN+2*BB*(CN*CN-SN*SN)-CC*SN+DD*CN;
  78    Standard_Real AACN = AA*CN;
  79    Standard_Real BBSN = BB*SN;
  80
  81    F = AACN*CN + BBSN*(CN+CN) + CC*CN + DD*SN + EE;
  82    D = -AACN*SN + BB*(CN*CN+SN*SN);
  83    D+=D;
  84    D+=-CC*SN+DD*CN;
  85    return Standard_True;
  86  }
  87
  88
  89 math_TrigonometricFunctionRoots::math_TrigonometricFunctionRoots
  90                                 (const Standard_Real theD,
  91                                  const Standard_Real theE,
  92                                  const Standard_Real theInfBound,
  93                                  const Standard_Real theSupBound)
  94 : NbSol         (-1),
  95   Sol           (1, 4),
  96   InfiniteStatus(Standard_False),
  97   Done          (Standard_False)
  98 {
  99   const Standard_Real A(0.0), B(0.0), C(0.0);
 100   Perform(A, B, C, theD, theE, theInfBound, theSupBound);
 101 }
 102
 103
 104 math_TrigonometricFunctionRoots::math_TrigonometricFunctionRoots
 105                                 (const Standard_Real theC,
 106                                  const Standard_Real theD,
 107                                  const Standard_Real theE,
 108                                  const Standard_Real theInfBound,
 109                                  const Standard_Real theSupBound)
 110 : NbSol         (-1),
 111   Sol           (1, 4),
 112   InfiniteStatus(Standard_False),
 113   Done          (Standard_False)
 114 {
 115   const Standard_Real A(0.0), B(0.0);
 116   Perform(A, B, theC, theD, theE, theInfBound, theSupBound);
 117 }
 118
 119
 120
 121 math_TrigonometricFunctionRoots::math_TrigonometricFunctionRoots
 122                                 (const Standard_Real theA,
 123                                  const Standard_Real theB,
 124                                  const Standard_Real theC,
 125                                  const Standard_Real theD,
 126                                  const Standard_Real theE,
 127                                  const Standard_Real theInfBound,
 128                                  const Standard_Real theSupBound)
 129 : NbSol         (-1),
 130   Sol           (1, 4),
 131   InfiniteStatus(Standard_False),
 132   Done          (Standard_False)
 133 {
 134   Perform(theA, theB, theC, theD, theE, theInfBound, theSupBound);
 135 }
 136
 137 void math_TrigonometricFunctionRoots::Perform(const Standard_Real A,
 138                                               const Standard_Real B,
 139                                               const Standard_Real C,
 140                                               const Standard_Real D,
 141                                               const Standard_Real E,
 142                                               const Standard_Real InfBound,
 143                                               const Standard_Real SupBound) {
 144
 145   Standard_Integer i, j=0, k, l, NZer=0, Nit = 10;
 146   Standard_Real Depi, Delta, Mod, AA, BB, CC, MyBorneInf;
 147   Standard_Real Teta, X;
 148   Standard_Real Eps, Tol1 = 1.e-15;
 149   TColStd_Array1OfReal ko(1,5), Zer(1,4);
 150   Standard_Boolean Flag4;
 151   InfiniteStatus = Standard_False;
 152   Done = Standard_True;
 153
 154   Eps = 1.e-12;
 155
 156   Depi = M_PI+M_PI;
 157   if (InfBound <= RealFirst() && SupBound >= RealLast()) {
 158     MyBorneInf = 0.0;
 159     Delta = Depi;
 160     Mod = 0.0;
 161   }
 162   else if (SupBound >= RealLast()) {
 163     MyBorneInf = InfBound;
 164     Delta = Depi;
 165     Mod = MyBorneInf/Depi;
 166   }
 167   else if (InfBound <= RealFirst()) {
 168     MyBorneInf = SupBound - Depi;
 169     Delta = Depi;
 170     Mod = MyBorneInf/Depi;
 171   }
 172   else {
 173     MyBorneInf = InfBound;
 174     Delta = SupBound-InfBound;
 175     Mod = InfBound/Depi;
 176     if ((SupBound-InfBound) > Depi) { Delta = Depi;}
 177   }
 178
 179   if ((Abs(A) <= Eps) && (Abs(B) <= Eps)) {
 180     if (Abs(C) <= Eps) {
 181       if (Abs(D) <= Eps) {
 182         if (Abs(E) <= Eps) {
 183           InfiniteStatus = Standard_True;   // infinite de solutions.
 184           return;
 185         }
 186         else {
 187           NbSol = 0;
 188           return;
 189         }
 190       }
 191       else {
 192         // Equation du type d*sin(x) + e = 0
 193         // =================================
 194         NbSol = 0;
 195         AA = -E/D;
 196         if (Abs(AA) > 1.) {
 197           return;
 198         }
 199
 200         Zer(1) = ASin(AA);
 201         Zer(2) = M_PI - Zer(1);
 202         NZer = 2;
 203         for (i = 1; i <= NZer; i++) {
 204           if (Zer(i) <= -Eps) {
 205             Zer(i) = Depi - Abs(Zer(i));
 206           }
 207           // On rend les solutions entre InfBound et SupBound:
 208           // =================================================
 209           Zer(i) += IntegerPart(Mod)*Depi;
 210           X = Zer(i)-MyBorneInf;
 211           if ((X > (-Epsilon(Delta))) && (X < Delta+ Epsilon(Delta))) {
 212             NbSol++;
 213             Sol(NbSol) = Zer(i);
 214           }
 215         }
 216       }
 217       return;
 218     }
 219     else if (Abs(D) <= Eps)  {
 220
 221       // Equation du premier degre de la forme c*cos(x) + e = 0
 222       // ======================================================
 223       NbSol = 0;
 224       AA = -E/C;
 225       if (Abs(AA) >1.) {
 226         return;
 227       }
 228       Zer(1) = ACos(AA);
 229       Zer(2) = -Zer(1);
 230       NZer = 2;
 231
 232       for (i = 1; i <= NZer; i++) {
 233         if (Zer(i) <= -Eps) {
 234           Zer(i) = Depi - Abs(Zer(i));
 235         }
 236         // On rend les solutions entre InfBound et SupBound:
 237         // =================================================
 238         Zer(i) += IntegerPart(Mod)*2.*M_PI;
 239         X = Zer(i)-MyBorneInf;
 240         if ((X >= (-Epsilon(Delta))) && (X <= Delta+ Epsilon(Delta))) {
 241           NbSol++;
 242           Sol(NbSol) = Zer(i);
 243         }
 244       }
 245       return;
 246     }
 247     else {
 248
 249       // Equation du second degre:
 250       // =========================
 251       AA = E - C;
 252       BB = 2.0*D;
 253       CC = E + C;
 254
 255       math_DirectPolynomialRoots Resol(AA, BB, CC);
 256       if (!Resol.IsDone()) {
 257         Done = Standard_False;
 258         return;
 259       }
 260       else if(!Resol.InfiniteRoots()) {
 261         NZer = Resol.NbSolutions();
 262         for (i = 1; i <= NZer; i++) {
 263           Zer(i) = Resol.Value(i);
 264         }
 265       }
 266       else if (Resol.InfiniteRoots()) {
 267         InfiniteStatus = Standard_True;
 268         return;
 269       }
 270     }
 271   }
 272   else {
 273     // Two additional analytical cases.
 274     if ((Abs(A) <= Eps) &&
 275         (Abs(E) <= Eps))
 276     {
 277       if (Abs(C) <= Eps)
 278       {
 279         // 2 * B * sin * cos + D * sin = 0
 280         NZer = 2;
 281         Zer(1) = 0.0;
 282         Zer(2) = M_PI;
 283
 284         AA = -D/(B * 2);
 285         if (Abs(AA) <= 1.0 + Precision::PConfusion())
 286         {
 287           NZer = 4;
 288           if (AA >= 1.0)
 289           {
 290             Zer(3)= 0.0;
 291             Zer(4)= 0.0;
 292           }
 293           else if (AA <= -1.0)
 294           {
 295             Zer(3)= M_PI;
 296             Zer(4)= M_PI;
 297           }
 298           else
 299           {
 300             Zer(3)= ACos(AA);
 301             Zer(4) = Depi - Zer(3);
 302           }
 303         }
 304
 305         NbSol = 0;
 306         for (i = 1; i <= NZer; i++)
 307         {
 308           if (Zer(i) <= MyBorneInf - Eps)
 309           {
 310             Zer(i) += Depi;
 311           }
 312           // On rend les solutions entre InfBound et SupBound:
 313           // =================================================
 314           Zer(i) += IntegerPart(Mod)*2.*M_PI;
 315           X = Zer(i)-MyBorneInf;
 316           if ((X >= (-Precision::PConfusion())) &&
 317               (X <= Delta + Precision::PConfusion()))
 318           {
 319             if (Zer(i) < InfBound)
 320               Zer(i) = InfBound;
 321             if (Zer(i) > SupBound)
 322               Zer(i) = SupBound;
 323             NbSol++;
 324             Sol(NbSol) = Zer(i);
 325           }
 326         }
 327         return;
 328       }
 329       if (Abs(D) <= Eps)
 330       {
 331         // 2 * B * sin * cos + C * cos = 0
 332         NZer = 2;
 333         Zer(1) = M_PI / 2.0;
 334         Zer(2) = M_PI * 3.0 / 2.0;
 335
 336         AA = -C/(B * 2);
 337         if (Abs(AA) <= 1.0 + Precision::PConfusion())
 338         {
 339           NZer = 4;
 340           if (AA >= 1.0)
 341           {
 342             Zer(3) = M_PI / 2.0;
 343             Zer(4) = M_PI / 2.0;
 344           }
 345           else if (AA <= -1.0)
 346           {
 347
 348             Zer(3) = M_PI * 3.0 / 2.0;
 349             Zer(4) = M_PI * 3.0 / 2.0;
 350           }
 351           else
 352           {
 353             Zer(3)= ASin(AA);
 354             Zer(4) = M_PI - Zer(3);
 355           }
 356         }
 357
 358         NbSol = 0;
 359         for (i = 1; i <= NZer; i++)
 360         {
 361           if (Zer(i) <= MyBorneInf - Eps)
 362           {
 363             Zer(i) += Depi;
 364           }
 365           // On rend les solutions entre InfBound et SupBound:
 366           // =================================================
 367           Zer(i) += IntegerPart(Mod)*2.*M_PI;
 368           X = Zer(i)-MyBorneInf;
 369           if ((X >= (-Precision::PConfusion())) &&
 370               (X <= Delta + Precision::PConfusion()))
 371           {
 372             if (Zer(i) < InfBound)
 373               Zer(i) = InfBound;
 374             if (Zer(i) > SupBound)
 375               Zer(i) = SupBound;
 376             NbSol++;
 377             Sol(NbSol) = Zer(i);
 378           }
 379         }
 380         return;
 381       }
 382     }
 383
 384 // Equation du 4 ieme degre
 385 // ========================
 386     ko(1) = A-C+E;
 387     ko(2) = 2.0*D-4.0*B;
 388     ko(3) = 2.0*E-2.0*A;
 389     ko(4) = 4.0*B+2.0*D;
 390     ko(5) = A+C+E;
 391     Standard_Boolean bko;
 392     Standard_Integer nbko=0;
 393     do {
 394       bko=Standard_False;
 395       math_DirectPolynomialRoots Resol4(ko(1), ko(2), ko(3), ko(4), ko(5));
 396       if (!Resol4.IsDone()) {
 397         Done = Standard_False;
 398         return;
 399       }
 400       else if (!Resol4.InfiniteRoots()) {
 401         NZer = Resol4.NbSolutions();
 402         for (i = 1; i <= NZer; i++) {
 403           Zer(i) = Resol4.Value(i);
 404         }
 405       }
 406       else if (Resol4.InfiniteRoots()) {
 407         InfiniteStatus = Standard_True;
 408         return;
 409       }
 410       Standard_Boolean triok;
 411       do {
 412         triok=Standard_True;
 413         for(i=1;i<NZer;i++) {
 414           if(Zer(i)>Zer(i+1)) {
 415             Standard_Real t=Zer(i);
 416             Zer(i)=Zer(i+1);
 417             Zer(i+1)=t;
 418             triok=Standard_False;
 419           }
 420         }
 421       }
 422       while(triok==Standard_False);
 423
 424       for(i=1;i<NZer;i++) {
 425         if(Abs(Zer(i+1)-Zer(i))<Eps) {
 426           //-- est ce une racine double ou une erreur numerique ?
 427           Standard_Real qw=Zer(i+1);
 428           Standard_Real va=ko(4)+qw*(2.0*ko(3)+qw*(3.0*ko(2)+qw*(4.0*ko(1))));
 429           //-- cout<<"   Val Double ("<<qw<<")=("<<va<<")"<<endl;
 430           if(Abs(va)>Eps) {
 431             bko=Standard_True;
 432             nbko++;
 433 #ifdef OCCT_DEBUG
 434             //if(nbko==1) {
 435             //  cout<<"Pb ds math_TrigonometricFunctionRoots CC="
 436             //  <<A<<" CS="<<B<<" C="<<C<<" S="<<D<<" Cte="<<E<<endl;
 437             //}
 438 #endif
 439             break;
 440           }
 441         }
 442       }
 443       if(bko) {
 444         //-- Si il y a un coeff petit, on divise
 445         //--
 446
 447         ko(1)*=0.0001;
 448         ko(2)*=0.0001;
 449         ko(3)*=0.0001;
 450         ko(4)*=0.0001;
 451         ko(5)*=0.0001;
 452
 453       }
 454     }
 455     while(bko);
 456   }
 457
 458   // Verification des solutions par rapport aux bornes:
 459   // ==================================================
 460   Standard_Real SupmInfs100 = (SupBound-InfBound)*0.01;
 461   NbSol = 0;
 462   for (i = 1; i <= NZer; i++) {
 463     Teta = atan(Zer(i)); Teta+=Teta;
 464     if (Zer(i) <= (-Eps)) {
 465       Teta = Depi-Abs(Teta);
 466     }
 467     Teta += IntegerPart(Mod)*Depi;
 468     if (Teta-MyBorneInf < 0) Teta += Depi;
 469
 470     X = Teta -MyBorneInf;
 471     if ((X >= (-Epsilon(Delta))) && (X <= Delta+ Epsilon(Delta))) {
 472       X = Teta;
 473
 474       // Appel de Newton:
 475       //OCC541(apo):  Standard_Real TetaNewton=0;
 476       Standard_Real TetaNewton = Teta;
 477       MyTrigoFunction MyF(A, B, C, D, E);
 478       math_NewtonFunctionRoot Resol(MyF, X, Tol1, Eps, Nit);
 479       if (Resol.IsDone()) {
 480         TetaNewton = Resol.Root();
 481       }
 482       //-- lbr le 7 mars 97 (newton converge tres tres loin de la solution initilale)
 483       Standard_Real DeltaNewton = TetaNewton-Teta;
 484       if((DeltaNewton > SupmInfs100) || (DeltaNewton < -SupmInfs100)) {
 485         //-- cout<<"\n Newton X0="<<Teta<<" -> "<<TetaNewton<<endl;
 486       }
 487       else {
 488         Teta=TetaNewton;
 489       }
 490
 491       Flag4 = Standard_False;
 492
 493       for(k = 1; k <= NbSol; k++) {
 494         //On met les valeurs par ordre croissant:
 495         if (Teta < Sol(k)) {
 496           for (l = k; l <= NbSol; l++) {
 497             j = NbSol-l+k;
 498             Sol(j+1) = Sol(j);
 499           }
 500           Sol(k) = Teta;
 501           NbSol++;
 502           Flag4 = Standard_True;
 503           break;
 504         }
 505       }
 506       if (!Flag4) {
 507         NbSol++;
 508         Sol(NbSol) = Teta;
 509       }
 510     }
 511   }
 512   // Cas particulier de  PI:
 513   if(NbSol<4) {
 514     Standard_Integer startIndex = NbSol + 1;
 515     for( Standard_Integer solIt = startIndex; solIt <= 4; solIt++) {
 516       Teta = M_PI + IntegerPart(Mod)*2.0*M_PI;;
 517       X = Teta - MyBorneInf;
 518       if ((X >= (-Epsilon(Delta))) && (X <= Delta + Epsilon(Delta))) {
 519         if (Abs(A-C+E) <= Eps) {
 520           Flag4 = Standard_False;
 521           for (k = 1; k <= NbSol; k++) {
 522             j = k;
 523             if (Teta < Sol(k)) {
 524               Flag4 = Standard_True;
 525               break;
 526             }
 527             if ((solIt == startIndex) && (Abs(Teta-Sol(k)) <= Eps)) {
 528               return;
 529             }
 530           }
 531
 532           if (!Flag4) {
 533             NbSol++;
 534             Sol(NbSol) = Teta;
 535           }
 536           else {
 537             for (k = j; k <= NbSol; k++) {
 538               i = NbSol-k+j;
 539               Sol(i+1) = Sol(i);
 540             }
 541             Sol(j) = Teta;
 542             NbSol++;
 543           }
 544         }
 545       }
 546     }
 547   }
 548 }
 549
 550
 551 void math_TrigonometricFunctionRoots::Dump(Standard_OStream& o) const
 552 {
 553   o << " math_TrigonometricFunctionRoots: \n";
 554   if (!Done) {
 555     o << "Not Done \n";
 556   }
 557   else if (InfiniteStatus) {
 558     o << " There is an infinity of roots\n";
 559   }
 560   else if (!InfiniteStatus) {
 561     o << " Number of solutions = " << NbSol <<"\n";
 562     for (Standard_Integer i = 1; i <= NbSol; i++) {
 563       o << " Value number " << i << "= " << Sol(i) << "\n";
 564     }
 565   }
 566 }