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