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