1 // Copyright (c) 1997-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
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
22 #define No_Standard_RangeError
23 #define No_Standard_OutOfRange
24 #define No_Standard_DimensionError
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>
34 class MyTrigoFunction: public math_FunctionWithDerivative {
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);
49 MyTrigoFunction::MyTrigoFunction(const Standard_Real A, const Standard_Real B, const Standard_Real C,
50 const Standard_Real D, const Standard_Real E) {
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;
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);
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;
81 F = AACN*CN + BBSN*(CN+CN) + CC*CN + DD*SN + EE;
82 D = -AACN*SN + BB*(CN*CN+SN*SN);
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) {
94 Standard_Real A = 0.0, B = 0.0, C = 0.0;
95 Perform(A, B, C, D, E, InfBound, SupBound);
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) {
105 Standard_Real A =0.0, B = 0.0;
106 Perform(A, B, C, D, E, InfBound, SupBound);
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) {
119 Perform(A, B, C, D, E, InfBound, SupBound);
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) {
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;
142 if (InfBound <= RealFirst() && SupBound >= RealLast()) {
147 else if (SupBound >= RealLast()) {
148 MyBorneInf = InfBound;
150 Mod = MyBorneInf/Depi;
152 else if (InfBound <= RealFirst()) {
153 MyBorneInf = SupBound - Depi;
155 Mod = MyBorneInf/Depi;
158 MyBorneInf = InfBound;
159 Delta = SupBound-InfBound;
161 if ((SupBound-InfBound) > Depi) { Delta = Depi;}
164 if ((Abs(A) <= Eps) && (Abs(B) <= Eps)) {
168 InfiniteStatus = Standard_True; // infinite de solutions.
177 // Equation du type d*sin(x) + e = 0
178 // =================================
186 Zer(2) = M_PI - Zer(1);
188 for (i = 1; i <= NZer; i++) {
189 if (Zer(i) <= -Eps) {
190 Zer(i) = Depi - Abs(Zer(i));
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))) {
204 else if (Abs(D) <= Eps) {
206 // Equation du premier degre de la forme c*cos(x) + e = 0
207 // ======================================================
217 for (i = 1; i <= NZer; i++) {
218 if (Zer(i) <= -Eps) {
219 Zer(i) = Depi-Abs(Zer(i));
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))) {
234 // Equation du second degre:
235 // =========================
240 math_DirectPolynomialRoots Resol(AA, BB, CC);
241 if (!Resol.IsDone()) {
242 Done = Standard_False;
245 else if(!Resol.InfiniteRoots()) {
246 NZer = Resol.NbSolutions();
247 for (i = 1; i <= NZer; i++) {
248 Zer(i) = Resol.Value(i);
251 else if (Resol.InfiniteRoots()) {
252 InfiniteStatus = Standard_True;
259 // Equation du 4 ieme degre
260 // ========================
266 Standard_Boolean bko;
267 Standard_Integer nbko=0;
270 math_DirectPolynomialRoots Resol4(ko(1), ko(2), ko(3), ko(4), ko(5));
271 if (!Resol4.IsDone()) {
272 Done = Standard_False;
275 else if (!Resol4.InfiniteRoots()) {
276 NZer = Resol4.NbSolutions();
277 for (i = 1; i <= NZer; i++) {
278 Zer(i) = Resol4.Value(i);
281 else if (Resol4.InfiniteRoots()) {
282 InfiniteStatus = Standard_True;
285 Standard_Boolean triok;
288 for(i=1;i<NZer;i++) {
289 if(Zer(i)>Zer(i+1)) {
290 Standard_Real t=Zer(i);
293 triok=Standard_False;
297 while(triok==Standard_False);
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;
310 // cout<<"Pb ds math_TrigonometricFunctionRoots CC="
311 // <<A<<" CS="<<B<<" C="<<C<<" S="<<D<<" Cte="<<E<<endl;
319 //-- Si il y a un coeff petit, on divise
333 // Verification des solutions par rapport aux bornes:
334 // ==================================================
335 Standard_Real SupmInfs100 = (SupBound-InfBound)*0.01;
337 for (i = 1; i <= NZer; i++) {
338 Teta = atan(Zer(i)); Teta+=Teta;
339 if (Zer(i) <= (-Eps)) {
340 Teta = Depi-Abs(Teta);
342 Teta += IntegerPart(Mod)*Depi;
343 if (Teta-MyBorneInf < 0) Teta += Depi;
345 X = Teta -MyBorneInf;
346 if ((X >= (-Epsilon(Delta))) && (X <= Delta+ Epsilon(Delta))) {
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();
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;
366 Flag4 = Standard_False;
368 for(k = 1; k <= NbSol; k++) {
369 //On met les valeurs par ordre croissant:
371 for (l = k; l <= NbSol; l++) {
377 Flag4 = Standard_True;
387 // Cas particulier de PI:
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++) {
399 Flag4 = Standard_True;
402 if ((solIt == startIndex) && (Abs(Teta-Sol(k)) <= Eps)) {
412 for (k = j; k <= NbSol; k++) {
426 void math_TrigonometricFunctionRoots::Dump(Standard_OStream& o) const
428 o << " math_TrigonometricFunctionRoots: \n";
432 else if (InfiniteStatus) {
433 o << " There is an infinity of roots\n";
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";