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