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