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