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
  33
  34 class MyTrigoFunction: public math_FunctionWithDerivative {
  35   Standard_Real AA;
  36   Standard_Real BB;
  37   Standard_Real CC;
  38   Standard_Real DD;
  39   Standard_Real EE;
  40
  41  public:
  42   MyTrigoFunction(const Standard_Real A, const Standard_Real B, const Standard_Real C, const Standard_Real D,
  43              const Standard_Real E);
  44   Standard_Boolean Value(const Standard_Real X, Standard_Real& F);
  45   Standard_Boolean Derivative(const Standard_Real X, Standard_Real& D);
  46   Standard_Boolean Values(const Standard_Real X, Standard_Real& F, Standard_Real& D);
  47 };
  48
  49  MyTrigoFunction::MyTrigoFunction(const Standard_Real A, const Standard_Real B, const Standard_Real C,
  50                         const Standard_Real D, const Standard_Real E) {
  51    AA = A;
  52    BB = B;
  53    CC = C;
  54    DD = D;
  55    EE = E;
  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(const Standard_Real D,
  90                          const Standard_Real E,
  91                          const Standard_Real InfBound,
  92                          const Standard_Real SupBound): Sol(1, 4) {
  93
  94   Standard_Real A = 0.0, B = 0.0, C = 0.0;
  95   Perform(A, B, C, D, E, InfBound, SupBound);
  96 }
  97
  98
  99 math_TrigonometricFunctionRoots::math_TrigonometricFunctionRoots(const Standard_Real C,
 100                          const Standard_Real D,
 101                          const Standard_Real E,
 102                          const Standard_Real InfBound,
 103                          const Standard_Real SupBound): Sol(1, 4) {
 104
 105   Standard_Real A =0.0, B = 0.0;
 106   Perform(A, B, C, D, E, InfBound, SupBound);
 107 }
 108
 109
 110
 111 math_TrigonometricFunctionRoots::math_TrigonometricFunctionRoots(const Standard_Real A,
 112                          const Standard_Real B,
 113                          const Standard_Real C,
 114                          const Standard_Real D,
 115                          const Standard_Real E,
 116                          const Standard_Real InfBound,
 117                          const Standard_Real SupBound): Sol(1, 4) {
 118
 119   Perform(A, B, C, D, E, InfBound, SupBound);
 120 }
 121
 122 void math_TrigonometricFunctionRoots::Perform(const Standard_Real A,
 123                                               const Standard_Real B,
 124                                               const Standard_Real C,
 125                                               const Standard_Real D,
 126                                               const Standard_Real E,
 127                                               const Standard_Real InfBound,
 128                                               const Standard_Real SupBound) {
 129
 130   Standard_Integer i, j=0, k, l, NZer=0, Nit = 10;
 131   Standard_Real Depi, Delta, Mod, AA, BB, CC, MyBorneInf;
 132   Standard_Real Teta, X;
 133   Standard_Real Eps, Tol1 = 1.e-15;
 134   TColStd_Array1OfReal ko(1,5), Zer(1,4);
 135   Standard_Boolean Flag4;
 136   InfiniteStatus = Standard_False;
 137   Done = Standard_True;
 138
 139   Eps = 1.e-12;
 140
 141   Depi = M_PI+M_PI;
 142   if (InfBound <= RealFirst() && SupBound >= RealLast()) {
 143     MyBorneInf = 0.0;
 144     Delta = Depi;
 145     Mod = 0.0;
 146   }
 147   else if (SupBound >= RealLast()) {
 148     MyBorneInf = InfBound;
 149     Delta = Depi;
 150     Mod = MyBorneInf/Depi;
 151   }
 152   else if (InfBound <= RealFirst()) {
 153     MyBorneInf = SupBound - Depi;
 154     Delta = Depi;
 155     Mod = MyBorneInf/Depi;
 156   }
 157   else {
 158     MyBorneInf = InfBound;
 159     Delta = SupBound-InfBound;
 160     Mod = InfBound/Depi;
 161     if ((SupBound-InfBound) > Depi) { Delta = Depi;}
 162   }
 163
 164   if ((Abs(A) <= Eps) && (Abs(B) <= Eps)) {
 165     if (Abs(C) <= Eps) {
 166       if (Abs(D) <= Eps) {
 167         if (Abs(E) <= Eps) {
 168           InfiniteStatus = Standard_True;   // infinite de solutions.
 169           return;
 170         }
 171         else {
 172           NbSol = 0;
 173           return;
 174         }
 175       }
 176       else {
 177 // Equation du type d*sin(x) + e = 0
 178 // =================================
 179         NbSol = 0;
 180         AA = -E/D;
 181         if (Abs(AA) > 1.) {
 182           return;
 183         }
 184
 185         Zer(1) = ASin(AA);
 186         Zer(2) = M_PI - Zer(1);
 187         NZer = 2;
 188         for (i = 1; i <= NZer; i++) {
 189           if (Zer(i) <= -Eps) {
 190             Zer(i) = Depi - Abs(Zer(i));
 191           }
 192           // On rend les solutions entre InfBound et SupBound:
 193           // =================================================
 194           Zer(i) += IntegerPart(Mod)*Depi;
 195           X = Zer(i)-MyBorneInf;
 196           if ((X > (-Epsilon(Delta))) && (X < Delta+ Epsilon(Delta))) {
 197             NbSol++;
 198             Sol(NbSol) = Zer(i);
 199           }
 200         }
 201       }
 202       return;
 203     }
 204     else if (Abs(D) <= Eps)  {
 205
 206 // Equation du premier degre de la forme c*cos(x) + e = 0
 207 // ======================================================
 208       NbSol = 0;
 209       AA = -E/C;
 210       if (Abs(AA) >1.) {
 211         return;
 212       }
 213       Zer(1) = ACos(AA);
 214       Zer(2) = -Zer(1);
 215       NZer = 2;
 216
 217       for (i = 1; i <= NZer; i++) {
 218         if (Zer(i) <= -Eps) {
 219           Zer(i) = Depi-Abs(Zer(i));
 220         }
 221         // On rend les solutions entre InfBound et SupBound:
 222         // =================================================
 223         Zer(i) += IntegerPart(Mod)*2.*M_PI;
 224         X = Zer(i)-MyBorneInf;
 225         if ((X >= (-Epsilon(Delta))) && (X <= Delta+ Epsilon(Delta))) {
 226           NbSol++;
 227           Sol(NbSol) = Zer(i);
 228         }
 229       }
 230       return;
 231     }
 232     else {
 233
 234 // Equation du second degre:
 235 // =========================
 236       AA = E - C;
 237       BB = 2.0*D;
 238       CC = E + C;
 239
 240       math_DirectPolynomialRoots Resol(AA, BB, CC);
 241       if (!Resol.IsDone()) {
 242         Done = Standard_False;
 243         return;
 244       }
 245       else if(!Resol.InfiniteRoots()) {
 246         NZer = Resol.NbSolutions();
 247         for (i = 1; i <= NZer; i++) {
 248           Zer(i) = Resol.Value(i);
 249         }
 250       }
 251       else if (Resol.InfiniteRoots()) {
 252         InfiniteStatus = Standard_True;
 253         return;
 254       }
 255     }
 256   }
 257   else {
 258
 259 // Equation du 4 ieme degre
 260 // ========================
 261     ko(1) = A-C+E;
 262     ko(2) = 2.0*D-4.0*B;
 263     ko(3) = 2.0*E-2.0*A;
 264     ko(4) = 4.0*B+2.0*D;
 265     ko(5) = A+C+E;
 266     Standard_Boolean bko;
 267     Standard_Integer nbko=0;
 268     do {
 269       bko=Standard_False;
 270       math_DirectPolynomialRoots Resol4(ko(1), ko(2), ko(3), ko(4), ko(5));
 271       if (!Resol4.IsDone()) {
 272         Done = Standard_False;
 273         return;
 274       }
 275       else if (!Resol4.InfiniteRoots()) {
 276         NZer = Resol4.NbSolutions();
 277         for (i = 1; i <= NZer; i++) {
 278           Zer(i) = Resol4.Value(i);
 279         }
 280       }
 281       else if (Resol4.InfiniteRoots()) {
 282         InfiniteStatus = Standard_True;
 283         return;
 284       }
 285       Standard_Boolean triok;
 286       do {
 287         triok=Standard_True;
 288         for(i=1;i<NZer;i++) {
 289           if(Zer(i)>Zer(i+1)) {
 290             Standard_Real t=Zer(i);
 291             Zer(i)=Zer(i+1);
 292             Zer(i+1)=t;
 293             triok=Standard_False;
 294           }
 295         }
 296       }
 297       while(triok==Standard_False);
 298
 299       for(i=1;i<NZer;i++) {
 300         if(Abs(Zer(i+1)-Zer(i))<Eps) {
 301           //-- est ce une racine double ou une erreur numerique ?
 302           Standard_Real qw=Zer(i+1);
 303           Standard_Real va=ko(4)+qw*(2.0*ko(3)+qw*(3.0*ko(2)+qw*(4.0*ko(1))));
 304           //-- cout<<"   Val Double ("<<qw<<")=("<<va<<")"<<endl;
 305           if(Abs(va)>Eps) {
 306             bko=Standard_True;
 307             nbko++;
 308 #ifdef OCCT_DEBUG
 309             //if(nbko==1) {
 310             //  cout<<"Pb ds math_TrigonometricFunctionRoots CC="
 311             //  <<A<<" CS="<<B<<" C="<<C<<" S="<<D<<" Cte="<<E<<endl;
 312             //}
 313 #endif
 314             break;
 315           }
 316         }
 317       }
 318       if(bko) {
 319         //-- Si il y a un coeff petit, on divise
 320         //--
 321
 322         ko(1)*=0.0001;
 323         ko(2)*=0.0001;
 324         ko(3)*=0.0001;
 325         ko(4)*=0.0001;
 326         ko(5)*=0.0001;
 327
 328       }
 329     }
 330     while(bko);
 331   }
 332
 333   // Verification des solutions par rapport aux bornes:
 334   // ==================================================
 335   Standard_Real SupmInfs100 = (SupBound-InfBound)*0.01;
 336   NbSol = 0;
 337   for (i = 1; i <= NZer; i++) {
 338     Teta = atan(Zer(i)); Teta+=Teta;
 339     if (Zer(i) <= (-Eps)) {
 340       Teta = Depi-Abs(Teta);
 341     }
 342     Teta += IntegerPart(Mod)*Depi;
 343     if (Teta-MyBorneInf < 0) Teta += Depi;
 344
 345     X = Teta -MyBorneInf;
 346     if ((X >= (-Epsilon(Delta))) && (X <= Delta+ Epsilon(Delta))) {
 347       X = Teta;
 348
 349       // Appel de Newton:
 350       //OCC541(apo):  Standard_Real TetaNewton=0;
 351       Standard_Real TetaNewton = Teta;
 352       MyTrigoFunction MyF(A, B, C, D, E);
 353       math_NewtonFunctionRoot Resol(MyF, X, Tol1, Eps, Nit);
 354       if (Resol.IsDone()) {
 355         TetaNewton = Resol.Root();
 356       }
 357       //-- lbr le 7 mars 97 (newton converge tres tres loin de la solution initilale)
 358       Standard_Real DeltaNewton = TetaNewton-Teta;
 359       if((DeltaNewton > SupmInfs100) || (DeltaNewton < -SupmInfs100)) {
 360         //-- cout<<"\n Newton X0="<<Teta<<" -> "<<TetaNewton<<endl;
 361       }
 362       else {
 363         Teta=TetaNewton;
 364       }
 365
 366       Flag4 = Standard_False;
 367
 368       for(k = 1; k <= NbSol; k++) {
 369         //On met les valeurs par ordre croissant:
 370         if (Teta < Sol(k)) {
 371           for (l = k; l <= NbSol; l++) {
 372             j = NbSol-l+k;
 373             Sol(j+1) = Sol(j);
 374           }
 375           Sol(k) = Teta;
 376           NbSol++;
 377           Flag4 = Standard_True;
 378           break;
 379         }
 380       }
 381       if (!Flag4) {
 382         NbSol++;
 383         Sol(NbSol) = Teta;
 384       }
 385     }
 386   }
 387   // Cas particulier de  PI:
 388   if(NbSol<4) {
 389     Standard_Integer startIndex = NbSol + 1;
 390     for( Standard_Integer solIt = startIndex; solIt <= 4; solIt++) {
 391       Teta = M_PI + IntegerPart(Mod)*2.0*M_PI;;
 392       X = Teta - MyBorneInf;
 393       if ((X >= (-Epsilon(Delta))) && (X <= Delta + Epsilon(Delta))) {
 394         if (Abs(A-C+E) <= Eps) {
 395           Flag4 = Standard_False;
 396           for (k = 1; k <= NbSol; k++) {
 397             j = k;
 398             if (Teta < Sol(k)) {
 399               Flag4 = Standard_True;
 400               break;
 401             }
 402             if ((solIt == startIndex) && (Abs(Teta-Sol(k)) <= Eps)) {
 403               return;
 404             }
 405           }
 406
 407           if (!Flag4) {
 408             NbSol++;
 409             Sol(NbSol) = Teta;
 410           }
 411           else {
 412             for (k = j; k <= NbSol; k++) {
 413               i = NbSol-k+j;
 414               Sol(i+1) = Sol(i);
 415             }
 416             Sol(j) = Teta;
 417             NbSol++;
 418           }
 419         }
 420       }
 421     }
 422   }
 423 }
 424
 425
 426 void math_TrigonometricFunctionRoots::Dump(Standard_OStream& o) const
 427 {
 428   o << " math_TrigonometricFunctionRoots: \n";
 429   if (!Done) {
 430     o << "Not Done \n";
 431   }
 432   else if (InfiniteStatus) {
 433     o << " There is an infinity of roots\n";
 434   }
 435   else if (!InfiniteStatus) {
 436     o << " Number of solutions = " << NbSol <<"\n";
 437     for (Standard_Integer i = 1; i <= NbSol; i++) {
 438       o << " Value number " << i << "= " << Sol(i) << "\n";
 439     }
 440   }
 441 }