0022904: Clean up sccsid variables
[occt.git] / src / math / math_DirectPolynomialRoots.cxx
1 //#ifndef DEB
2 #define No_Standard_RangeError
3 #define No_Standard_OutOfRange
4 #define No_Standard_DimensionError
5 //#endif
6
7 #include <math_DirectPolynomialRoots.ixx>
8
9 #include <Standard_RangeError.hxx>
10 #include <StdFail_InfiniteSolutions.hxx> 
11
12
13
14     // Reference pour solution equation 3ieme degre et 2ieme degre :
15     //     ALGORITHMES NUMERIQUES ANALYSE ET MISE EN OEUVRE, tome 2
16     //          (equations et systemes non lineaires)
17     //
18     // J. VIGNES editions TECHNIP.
19
20     const Standard_Real ZERO = 1.0e-30;
21     const Standard_Real EPSILON = RealEpsilon();
22     const Standard_Real RADIX = 2;
23     const Standard_Real Un_Sur_Log_RADIX = 1.0/log(2.0);
24
25     static Standard_Real Value(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X) {
26  
27         Standard_Real Result = Poly[0];
28         for(Standard_Integer Index = 1; Index < N; Index++) {
29           Result = Result * X + Poly[Index];
30         }
31         return Result;
32     }
33
34
35     static void Values(const Standard_Integer N, Standard_Real *Poly, const Standard_Real X,
36                        Standard_Real& Val, Standard_Real& Der) {
37  
38         Val = Poly[0] * X + Poly[1];
39         Der = Poly[0];
40         for(Standard_Integer Index = 2; Index < N; Index++) {
41           Der = Der * X + Val;
42           Val = Val * X + Poly[Index];
43         }
44     }
45
46     static Standard_Real Improve(const Standard_Integer N, Standard_Real *Poly, const Standard_Real IniSol) {
47
48         Standard_Real Val, Der, Delta;
49         Standard_Real Sol = IniSol;
50         Standard_Real IniVal = Value(N, Poly, IniSol);
51         Standard_Integer Index;
52
53 //      cout << "Improve\n";
54         for(Index = 1; Index < 10; Index++) {
55           Values(N, Poly, Sol, Val, Der);
56           if(Abs(Der) <= ZERO) break;
57           Delta = - Val / Der;
58           if(Abs(Delta) <= EPSILON * Abs(Sol)) break;
59           Sol = Sol + Delta;
60 //        cout << " Iter = " << Index << " Delta = " << Delta 
61 //             << " Val  = " << Val   << " Der   = " << Der << "\n";
62         }
63         if(Abs(Val) <= Abs(IniVal)) {
64           return Sol;
65         }
66         else {
67           return IniSol;
68         }
69     }
70
71     Standard_Real Improve(const Standard_Real A, const Standard_Real B, const Standard_Real C,
72                  const Standard_Real D, const Standard_Real E, const Standard_Real IniSol) {
73       
74         Standard_Real Poly[5];
75         Poly[0] = A;
76         Poly[1] = B;
77         Poly[2] = C;
78         Poly[3] = D;
79         Poly[4] = E;
80          return Improve(5, Poly, IniSol);
81     }
82
83     Standard_Real Improve(const Standard_Real A, const Standard_Real B, 
84                  const Standard_Real C, const Standard_Real D, const Standard_Real IniSol) {
85       
86         Standard_Real Poly[4];
87         Poly[0] = A;
88         Poly[1] = B;
89         Poly[2] = C;
90         Poly[3] = D;
91         return Improve(4, Poly, IniSol);
92     }
93
94     Standard_Real Improve(const Standard_Real A, const Standard_Real B, 
95                  const Standard_Real C, const Standard_Real IniSol) {
96       
97         Standard_Real Poly[3];
98         Poly[0] = A;
99         Poly[1] = B;
100         Poly[2] = C;
101         return Improve(3, Poly, IniSol);
102     }
103
104     Standard_Integer BaseExponent(const Standard_Real X) {
105
106         if(X > 1.0) {
107           return (Standard_Integer)(log(X) * Un_Sur_Log_RADIX);
108         }
109         else if(X < -1.0) {
110           return (Standard_Integer)(-log(-X) * Un_Sur_Log_RADIX);
111         }
112         else {
113           return 0;
114         }
115     }
116
117
118     math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A,
119                                const Standard_Real B,
120                                const Standard_Real C,
121                                const Standard_Real D,
122                                const Standard_Real E) {
123       InfiniteStatus = Standard_False;
124       Done = Standard_True;
125       Solve(A, B, C, D, E);
126     }
127
128 math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A,
129                                                        const Standard_Real B,
130                                                        const Standard_Real C,
131                                                        const Standard_Real D) {
132   Done = Standard_True;
133   InfiniteStatus = Standard_False;
134   Solve(A, B, C, D);
135 }
136
137 math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A,
138                                                        const Standard_Real B,
139                                                        const Standard_Real C) {
140   Done = Standard_True;
141   InfiniteStatus = Standard_False;
142   Solve(A, B, C);
143 }
144
145 math_DirectPolynomialRoots::math_DirectPolynomialRoots(const Standard_Real A,
146                                                        const Standard_Real B) {
147   Done = Standard_True;
148   InfiniteStatus = Standard_False;
149   Solve(A, B);
150 }
151
152
153 void math_DirectPolynomialRoots::Solve(const Standard_Real a,
154                                        const Standard_Real b,
155                                        const Standard_Real c,
156                                        const Standard_Real d,
157                                        const Standard_Real e) {
158   if(Abs(a) <= ZERO) {
159     Solve(b, c, d, e);
160     return;
161   }        
162
163   //// modified by jgv, 22.01.09 ////
164   Standard_Real aZero = ZERO;
165   Standard_Real Abs_b = Abs(b), Abs_c = Abs(c), Abs_d = Abs(d), Abs_e = Abs(e);
166
167   if (Abs_b > aZero)
168     aZero = Abs_b;
169   if (Abs_c > aZero)
170     aZero = Abs_c;
171   if (Abs_d > aZero)
172     aZero = Abs_d;
173   if (Abs_e > aZero)
174     aZero = Abs_e;
175   if (aZero > ZERO)
176     aZero = Epsilon(100.*aZero);
177
178   if(Abs(a) <= aZero) {
179     Standard_Real aZero1000 = 1000.*aZero;
180     Standard_Boolean with_a = Standard_False;
181     if (Abs_b > ZERO && Abs_b <= aZero1000)
182       with_a = Standard_True;
183     if (Abs_c > ZERO && Abs_c <= aZero1000)
184       with_a = Standard_True;
185     if (Abs_d > ZERO && Abs_d <= aZero1000)
186       with_a = Standard_True;
187     if (Abs_e > ZERO && Abs_e <= aZero1000)
188       with_a = Standard_True;
189
190     if (!with_a)
191       {
192         Solve(b, c, d, e);
193         return;
194       }
195   }        
196   ///////////////////////////////////
197
198   Standard_Real A, B, C, D, R3, S3, T3, Q3, Y0, P0, Q0, P, Q, P1, Q1;
199   Standard_Real Discr, Sdiscr;
200   Standard_Integer Index;
201   Standard_Integer Exp;
202   Standard_Real PowRadix1,PowRadix2;
203   
204   A = b / a;
205   B = c / a;
206   C = d / a;
207   D = e / a;
208   Exp = BaseExponent(D) / 4;
209   //-- 
210   //-- A = A / pow(RADIX, Exp);
211   //-- B = B / pow(RADIX, 2 * Exp);
212   //-- C = C / pow(RADIX, 3 * Exp);
213   //-- D = D / pow(RADIX, 4 * Exp);
214   PowRadix1 = pow(RADIX,Exp);
215   A/= PowRadix1;  PowRadix2 = PowRadix1 * PowRadix1;
216   B/= PowRadix2;
217   C/= PowRadix2 * PowRadix1;
218   D/= PowRadix2 * PowRadix2;
219   //-- 
220   R3 = -B;
221   S3 = A * C - 4.0 * D;
222   T3 = D * (4.0 * B - A * A) - C * C;
223   Q3 = 1.0;                
224   math_DirectPolynomialRoots Sol3(Q3, R3, S3, T3);
225   //-- ################################################################################
226   if(Sol3.IsDone() == Standard_False) { Done = Standard_False; return; } 
227   //-- ################################################################################
228
229
230   Y0 = Sol3.Value(1);
231   for(Index = 2; Index <= Sol3.NbSolutions(); Index++) {
232     if(Sol3.Value(Index) > Y0) Y0 = Sol3.Value(Index);
233   }
234   Discr = A * Y0 * 0.5 - C;
235   if(Discr >= 0.0) { 
236     Sdiscr = 1.0;
237   }
238   else {
239     Sdiscr = -1.0;
240   }
241   P0 = A * A * 0.25 - B + Y0;
242   if(P0 < 0.0) P0 = 0.0; 
243   P0 = sqrt(P0);
244   Q0 = Y0 * Y0 * 0.25 - D;
245   if(Q0 < 0.0) Q0 = 0.0; 
246   Q0 = sqrt(Q0);
247
248   Standard_Real Ademi    = A  * 0.5;
249   Standard_Real Ydemi    = Y0 * 0.5;
250   Standard_Real SdiscrQ0 = Sdiscr * Q0;
251
252   P = Ademi + P0;
253   Q = Ydemi + SdiscrQ0;
254   P1 = Ademi - P0;
255   Q1 = Ydemi - SdiscrQ0;
256 //  Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) Begin
257   Standard_Real eps;
258
259   eps = Epsilon(100.*Max(Ademi, P0));
260   if (Abs(P) <= eps)
261     P = 0.;
262   if (Abs(P1) <= eps)
263     P1 = 0.;
264
265   eps = Epsilon(100.*Max(Ydemi, SdiscrQ0));
266   if (Abs(Q) <= eps)
267     Q = 0.;
268   if (Abs(Q1) <= eps)
269     Q1 = 0.;
270 //  Modified by skv - Wed Apr 14 16:05:24 2004 IDEM(Airbus) End
271   Ademi = 1.0;
272
273   math_DirectPolynomialRoots ASol2(Ademi, P,  Q);
274   //-- ################################################################################
275   if(ASol2.IsDone() == Standard_False) { Done = Standard_False; return; } 
276   //-- ################################################################################
277   math_DirectPolynomialRoots BSol2(Ademi, P1,  Q1);
278   //-- ################################################################################
279   if(BSol2.IsDone() == Standard_False) { Done = Standard_False; return; } 
280   //-- ################################################################################
281
282   NbSol = ASol2.NbSolutions() + BSol2.NbSolutions();
283   for(Index = 0; Index < ASol2.NbSolutions(); Index++) {
284     TheRoots[Index] = ASol2.TheRoots[Index];
285   }
286   for(Index = 0; Index < BSol2.NbSolutions(); Index++) {
287     TheRoots[ASol2.NbSolutions() + Index] = BSol2.TheRoots[Index];
288   }
289   for(Index = 0; Index < NbSol; Index++) {
290     TheRoots[Index] = TheRoots[Index] * PowRadix1;
291     TheRoots[Index] = Improve(a, b, c, d, e, TheRoots[Index]);
292   }
293 }
294
295     void math_DirectPolynomialRoots::Solve(const Standard_Real A,
296                                            const Standard_Real B,
297                                            const Standard_Real C,
298                                            const Standard_Real D) { 
299
300       if(Abs(A) <= ZERO) {
301         Solve(B, C, D);
302         return;
303       }
304
305       Standard_Real Beta, Gamma, Del, P1, P2, P, Ep, Q1, Q2, Q3, Q, Eq, A1, A2, Discr;
306       Standard_Real Sigma, Psi, D1, D2, Sb, Omega, Sp3, Y1, Dbg, Sdbg, Den1, Den2;
307       Standard_Real U, H, Sq;
308       Standard_Integer Exp;
309
310       Beta  = B / A;
311       Gamma = C / A;
312       Del   = D / A;
313
314       Exp = BaseExponent(Del) / 3;
315
316       Standard_Real PowRadix1 = pow(RADIX,Exp);
317       Standard_Real PowRadix2 = PowRadix1*PowRadix1;
318       Beta/=   PowRadix1;                   
319       Gamma/=  PowRadix2;
320       Del/=    PowRadix2 * PowRadix1;
321       //-- Beta  = Beta  / pow(RADIX, Exp);
322       //-- Gamma = Gamma / pow(RADIX, 2 * Exp);
323       //-- Del   = Del   / pow(RADIX, 3 * Exp);
324
325       P1 = Gamma;
326       P2 = - (Beta * Beta) / 3.0;
327       P = P1 + P2;
328       Ep = 5.0 * EPSILON * (Abs(P1) + Abs(P2));
329       if(Abs(P) <= Ep) P = 0.0;
330       Q1 = Del;
331       Q2 = - Beta * Gamma / 3.0;
332       Q3 = 2.0  * (Beta * Beta * Beta) / 27.0;
333       Q = Q1 + Q2 + Q3;
334       Eq = 10.0 * EPSILON * (Abs(Q1) + Abs(Q2) + Abs(Q3));
335       if(Abs(Q) <= Eq) Q = 0.0;
336       //-- ############################################################
337       Standard_Real AbsP = P; if(P<0.0) AbsP = -P;
338       if(AbsP>1e+80) { Done = Standard_False; return; } 
339       //-- ############################################################      
340       A1 = (P * P * P) / 27.0;  
341       A2 = (Q * Q) / 4.0;
342       Discr = A1 + A2;
343       if(P < 0.0) {
344         Sigma = - Q2 - Q3;
345         Psi = Gamma * Gamma * (4.0 * Gamma - Beta * Beta) / 27.0;
346         if(Sigma >= 0.0) {
347           D1 = Sigma + 2.0 * sqrt(-A1);
348         }
349         else {
350           D1 = Sigma - 2.0 * sqrt(-A1);
351         }
352         D2 = Psi / D1;
353         Discr = 0.0;
354         if(Abs(Del - D1) >= 18.0 * EPSILON * (Abs(Del) + Abs(D1)) &&
355            Abs(Del - D2) >= 24.0 * EPSILON * (Abs(Del) + Abs(D2))) {
356           Discr = (Del - D1) * (Del - D2) / 4.0;
357         }
358       }
359       if(Beta >= 0.0) {
360         Sb = 1.0;
361       }
362       else {
363         Sb = -1.0;
364       }
365       if(Discr < 0.0) {
366         NbSol = 3;
367         if(Beta == 0.0 && Q == 0.0) {
368           TheRoots[0] = sqrt(-P);
369           TheRoots[1] = -TheRoots[0];
370           TheRoots[2] = 0.0;
371         }
372         else {
373           Omega = atan(0.5 * Q / sqrt(- Discr));
374           Sp3 = sqrt(-P / 3.0);
375           Y1 = -2.0 * Sb * Sp3 * cos(M_PI / 6.0 - Sb * Omega / 3.0);          
376           TheRoots[0] = - Beta / 3.0 + Y1;
377           if(Beta * Q <= 0.0) {
378             TheRoots[1] = - Beta / 3.0 + 2.0 * Sp3 * sin(Omega / 3.0);
379           }
380           else {
381             Dbg = Del - Beta * Gamma;
382             if(Dbg >= 0.0) {
383               Sdbg = 1.0;
384             }
385             else {
386               Sdbg = -1.0;
387             }
388             Den1 = 8.0 * Beta * Beta / 9.0 - 4.0 * Beta * Y1 / 3.0 
389                                            - 2.0 * Q / Y1;
390             Den2 = 2.0 * Y1 * Y1 - Q / Y1;
391             TheRoots[1] = Dbg / Den1 + Sdbg * sqrt(-27.0 * Discr) / Den2;
392           }
393           TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]);
394         }
395       }
396       else if(Discr > 0.0) {
397         NbSol = 1;
398         U = sqrt(Discr) + Abs(Q / 2.0);
399         if(U >= 0.0) {
400           U = pow(U, 1.0 / 3.0);   
401         }
402         else {
403           U = - pow(Abs(U), 1.0 / 3.0);
404         }
405         if(P >= 0.0) {
406           H = U * U + P / 3.0 + (P / U) * (P / U) / 9.0;          
407         } 
408         else {
409           H = U * Abs(Q) / (U * U - P / 3.0);
410         }
411         if(Beta * Q >= 0.0) {
412           if(Abs(H) <= RealSmall() && Abs(Q) <= RealSmall()) {
413             TheRoots[0] = - Beta / 3.0 - U + P / (3.0 * U);
414           }
415           else {
416             TheRoots[0] = - Beta / 3.0 - Q / H;
417           }
418         }
419         else {
420           TheRoots[0] = - Del / (Beta * Beta / 9.0 + H - Beta * Q / (3.0 * H));
421         }
422       }
423       else {
424         NbSol = 3;
425         if(Q >= 0.0) {
426           Sq = 1.0;
427         }
428         else {
429           Sq = -1.0;
430         }
431         Sp3 = sqrt(-P / 3.0);
432         if(Beta * Q <= 0.0) {
433           TheRoots[0] = -Beta / 3.0 + Sq * Sp3;
434           TheRoots[1] = TheRoots[0];
435           if(Beta * Q == 0.0) {
436             TheRoots[2] = -Beta / 3.0 - 2.0 * Sq * Sp3;
437           }
438           else {
439             TheRoots[2] = - Del / (TheRoots[0] * TheRoots[1]);
440           }
441         }
442         else {
443           TheRoots[0] = -Gamma / (Beta + 3.0 * Sq * Sp3);
444           TheRoots[1] = TheRoots[0];
445           TheRoots[2] = -Beta / 3.0 - 2.0 * Sq * Sp3;
446         }
447       }
448       for(Standard_Integer Index = 0; Index < NbSol; Index++) {
449         TheRoots[Index] = TheRoots[Index] * pow(RADIX, Exp);
450         TheRoots[Index] = Improve(A, B, C, D, TheRoots[Index]);
451       }
452     }
453
454     void math_DirectPolynomialRoots::Solve(const Standard_Real A,
455                                            const Standard_Real B,
456                                            const Standard_Real C) {
457
458         if(Abs(A) <= ZERO) {
459           Solve(B, C);
460           return;
461         }
462
463         Standard_Real EpsD = 3.0 * EPSILON * (B * B + Abs(4.0 * A * C));
464         Standard_Real Discrim = B * B - 4.0 * A * C;
465
466         if(Abs(Discrim) <= EpsD) Discrim = 0.0;
467         if(Discrim < 0.0) {
468           NbSol = 0;
469         }
470         else if(Discrim == 0.0) {
471           NbSol = 2;
472           TheRoots[0] = -0.5 * B / A;
473           TheRoots[0] = Improve(A, B, C, TheRoots[0]);
474           TheRoots[1] = TheRoots[0];
475         } 
476         else {
477           NbSol = 2;
478           if(B > 0.0) {
479             TheRoots[0] = - (B + sqrt(Discrim)) / (2.0 * A);
480           }
481           else {
482             TheRoots[0] = - (B - sqrt(Discrim)) / (2.0 * A);
483           }
484           TheRoots[0] = Improve(A, B, C, TheRoots[0]);
485           TheRoots[1] = C / (A * TheRoots[0]);
486           TheRoots[1] = Improve(A, B, C, TheRoots[1]);
487         } 
488     }
489
490     void math_DirectPolynomialRoots::Solve(const Standard_Real A,
491                                            const Standard_Real B) { 
492         
493         if(Abs(A) <= ZERO) {
494           if (Abs(B) <= ZERO) {
495             InfiniteStatus = Standard_True;
496             return;
497           }
498           NbSol = 0;
499           return;
500         }
501         NbSol = 1;
502         TheRoots[0] = -B / A;
503     }
504
505 void math_DirectPolynomialRoots::Dump(Standard_OStream& o) const {
506        o << "math_DirectPolynomialRoots ";
507        if (!Done) {
508          o <<" Not Done \n";
509        }
510        else if(InfiniteStatus) {
511          o << " Status = Infinity Roots \n";
512        }
513        else if (!InfiniteStatus) {
514          o << " Status = Not Infinity Roots \n";
515          o << " Number of solutions = " << NbSol <<"\n";
516          for (Standard_Integer i = 1; i <= NbSol; i++) {
517            o << " Solution number " << i << " = " << TheRoots[i-1] <<"\n";
518          }
519        }
520     }
521
522
523