· 6 years ago · Jan 14, 2020, 08:32 PM
1// poly34.cpp : solution of cubic and quartic equation
2// (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
3// khash2 (at) gmail.com
4// Thanks to Alexandr Rakhmanin <rakhmanin (at) gmail.com>
5// public domain
6//
7#include <math.h>
8
9#include "poly34.h" // solution of cubic and quartic equation
10#define TwoPi 6.28318530717958648
11const double eps=1e-14;
12
13//=============================================================================
14// _root3, root3 from http://prografix.narod.ru
15//=============================================================================
16static double _root3 ( double x )
17{
18 double s = 1.;
19 while ( x < 1. )
20 {
21 x *= 8.;
22 s *= 0.5;
23 }
24 while ( x > 8. )
25 {
26 x *= 0.125;
27 s *= 2.;
28 }
29 double r = 1.5;
30 r -= 1./3. * ( r - x / ( r * r ) );
31 r -= 1./3. * ( r - x / ( r * r ) );
32 r -= 1./3. * ( r - x / ( r * r ) );
33 r -= 1./3. * ( r - x / ( r * r ) );
34 r -= 1./3. * ( r - x / ( r * r ) );
35 r -= 1./3. * ( r - x / ( r * r ) );
36 return r * s;
37}
38
39double root3 ( double x )
40{
41 if ( x > 0 ) return _root3 ( x ); else
42 if ( x < 0 ) return-_root3 (-x ); else
43 return 0.;
44}
45
46
47// x - array of size 2
48// return 2: 2 real roots x[0], x[1]
49// return 0: pair of complex roots: x[0]±i*x[1]
50int SolveP2(double *x, double a, double b) { // solve equation x^2 + a*x + b = 0
51 double D = 0.25*a*a - b;
52 if (D >= 0) {
53 D = sqrt(D);
54 x[0] = -0.5*a + D;
55 x[1] = -0.5*a - D;
56 return 2;
57 }
58 x[0] = -0.5*a;
59 x[1] = sqrt(-D);
60 return 0;
61}
62//---------------------------------------------------------------------------
63// x - array of size 3
64// In case 3 real roots: => x[0], x[1], x[2], return 3
65// 2 real roots: x[0], x[1], return 2
66// 1 real root : x[0], x[1] ± i*x[2], return 1
67int SolveP3(double *x,double a,double b,double c) { // solve cubic equation x^3 + a*x^2 + b*x + c = 0
68 double a2 = a*a;
69 double q = (a2 - 3*b)/9;
70 double r = (a*(2*a2-9*b) + 27*c)/54;
71 // equation x^3 + q*x + r = 0
72 double r2 = r*r;
73 double q3 = q*q*q;
74 double A,B;
75 if (r2 <= (q3 + eps)) {//<<-- FIXED!
76 double t=r/sqrt(q3);
77 if( t<-1) t=-1;
78 if( t> 1) t= 1;
79 t=acos(t);
80 a/=3; q=-2*sqrt(q);
81 x[0]=q*cos(t/3)-a;
82 x[1]=q*cos((t+TwoPi)/3)-a;
83 x[2]=q*cos((t-TwoPi)/3)-a;
84 return(3);
85 } else {
86 //A =-pow(fabs(r)+sqrt(r2-q3),1./3);
87 A =-root3(fabs(r)+sqrt(r2-q3));
88 if( r<0 ) A=-A;
89 B = (A==0? 0 : B=q/A);
90
91 a/=3;
92 x[0] =(A+B)-a;
93 x[1] =-0.5*(A+B)-a;
94 x[2] = 0.5*sqrt(3.)*(A-B);
95 if(fabs(x[2])<eps) { x[2]=x[1]; return(2); }
96 return(1);
97 }
98}// SolveP3(double *x,double a,double b,double c) {
99//---------------------------------------------------------------------------
100// a>=0!
101void CSqrt( double x, double y, double &a, double &b) // returns: a+i*s = sqrt(x+i*y)
102{
103 double r = sqrt(x*x+y*y);
104 if( y==0 ) {
105 r = sqrt(r);
106 if(x>=0) { a=r; b=0; } else { a=0; b=r; }
107 } else { // y != 0
108 a = sqrt(0.5*(x+r));
109 b = 0.5*y/a;
110 }
111}
112//---------------------------------------------------------------------------
113int SolveP4Bi(double *x, double b, double d) // solve equation x^4 + b*x^2 + d = 0
114{
115 double D = b*b-4*d;
116 if( D>=0 )
117 {
118 double sD = sqrt(D);
119 double x1 = (-b+sD)/2;
120 double x2 = (-b-sD)/2; // x2 <= x1
121 if( x2>=0 ) // 0 <= x2 <= x1, 4 real roots
122 {
123 double sx1 = sqrt(x1);
124 double sx2 = sqrt(x2);
125 x[0] = -sx1;
126 x[1] = sx1;
127 x[2] = -sx2;
128 x[3] = sx2;
129 return 4;
130 }
131 if( x1 < 0 ) // x2 <= x1 < 0, two pair of imaginary roots
132 {
133 double sx1 = sqrt(-x1);
134 double sx2 = sqrt(-x2);
135 x[0] = 0;
136 x[1] = sx1;
137 x[2] = 0;
138 x[3] = sx2;
139 return 0;
140 }
141 // now x2 < 0 <= x1 , two real roots and one pair of imginary root
142 double sx1 = sqrt( x1);
143 double sx2 = sqrt(-x2);
144 x[0] = -sx1;
145 x[1] = sx1;
146 x[2] = 0;
147 x[3] = sx2;
148 return 2;
149 } else { // if( D < 0 ), two pair of compex roots
150 double sD2 = 0.5*sqrt(-D);
151 CSqrt(-0.5*b, sD2, x[0],x[1]);
152 CSqrt(-0.5*b,-sD2, x[2],x[3]);
153 return 0;
154 } // if( D>=0 )
155} // SolveP4Bi(double *x, double b, double d) // solve equation x^4 + b*x^2 d
156//---------------------------------------------------------------------------
157#define SWAP(a,b) { t=b; b=a; a=t; }
158static void dblSort3( double &a, double &b, double &c) // make: a <= b <= c
159{
160 double t;
161 if( a>b ) SWAP(a,b); // now a<=b
162 if( c<b ) {
163 SWAP(b,c); // now a<=b, b<=c
164 if( a>b ) SWAP(a,b);// now a<=b
165 }
166}
167//---------------------------------------------------------------------------
168int SolveP4De(double *x, double b, double c, double d) // solve equation x^4 + b*x^2 + c*x + d
169{
170 //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
171 if( fabs(c)<1e-14*(fabs(b)+fabs(d)) ) return SolveP4Bi(x,b,d); // After that, c!=0
172
173 int res3 = SolveP3( x, 2*b, b*b-4*d, -c*c); // solve resolvent
174 // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
175 if( res3>1 ) // 3 real roots,
176 {
177 dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
178 // Note: x[0]*x[1]*x[2]= c*c > 0
179 if( x[0] > 0) // all roots are positive
180 {
181 double sz1 = sqrt(x[0]);
182 double sz2 = sqrt(x[1]);
183 double sz3 = sqrt(x[2]);
184 // Note: sz1*sz2*sz3= -c (and not equal to 0)
185 if( c>0 )
186 {
187 x[0] = (-sz1 -sz2 -sz3)/2;
188 x[1] = (-sz1 +sz2 +sz3)/2;
189 x[2] = (+sz1 -sz2 +sz3)/2;
190 x[3] = (+sz1 +sz2 -sz3)/2;
191 return 4;
192 }
193 // now: c<0
194 x[0] = (-sz1 -sz2 +sz3)/2;
195 x[1] = (-sz1 +sz2 -sz3)/2;
196 x[2] = (+sz1 -sz2 -sz3)/2;
197 x[3] = (+sz1 +sz2 +sz3)/2;
198 return 4;
199 } // if( x[0] > 0) // all roots are positive
200 // now x[0] <= x[1] < 0, x[2] > 0
201 // two pair of comlex roots
202 double sz1 = sqrt(-x[0]);
203 double sz2 = sqrt(-x[1]);
204 double sz3 = sqrt( x[2]);
205
206 if( c>0 ) // sign = -1
207 {
208 x[0] = -sz3/2;
209 x[1] = ( sz1 -sz2)/2; // x[0]±i*x[1]
210 x[2] = sz3/2;
211 x[3] = (-sz1 -sz2)/2; // x[2]±i*x[3]
212 return 0;
213 }
214 // now: c<0 , sign = +1
215 x[0] = sz3/2;
216 x[1] = (-sz1 +sz2)/2;
217 x[2] = -sz3/2;
218 x[3] = ( sz1 +sz2)/2;
219 return 0;
220 } // if( res3>1 ) // 3 real roots,
221 // now resoventa have 1 real and pair of compex roots
222 // x[0] - real root, and x[0]>0,
223 // x[1]±i*x[2] - complex roots,
224 // x[0] must be >=0. But one times x[0]=~ 1e-17, so:
225 if (x[0] < 0) x[0] = 0;
226 double sz1 = sqrt(x[0]);
227 double szr, szi;
228 CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
229 if( c>0 ) // sign = -1
230 {
231 x[0] = -sz1/2-szr; // 1st real root
232 x[1] = -sz1/2+szr; // 2nd real root
233 x[2] = sz1/2;
234 x[3] = szi;
235 return 2;
236 }
237 // now: c<0 , sign = +1
238 x[0] = sz1/2-szr; // 1st real root
239 x[1] = sz1/2+szr; // 2nd real root
240 x[2] = -sz1/2;
241 x[3] = szi;
242 return 2;
243} // SolveP4De(double *x, double b, double c, double d) // solve equation x^4 + b*x^2 + c*x + d
244//-----------------------------------------------------------------------------
245double N4Step(double x, double a,double b,double c,double d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
246{
247 double fxs= ((4*x+3*a)*x+2*b)*x+c; // f'(x)
248 if (fxs == 0) return x; //return 1e99; <<-- FIXED!
249 double fx = (((x+a)*x+b)*x+c)*x+d; // f(x)
250 return x - fx/fxs;
251}
252//-----------------------------------------------------------------------------
253// x - array of size 4
254// return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
255// return 2: 2 real roots x[0], x[1] and complex x[2]±i*x[3],
256// return 0: two pair of complex roots: x[0]±i*x[1], x[2]±i*x[3],
257int SolveP4(double *x,double a,double b,double c,double d) { // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
258 // move to a=0:
259 double d1 = d + 0.25*a*( 0.25*b*a - 3./64*a*a*a - c);
260 double c1 = c + 0.5*a*(0.25*a*a - b);
261 double b1 = b - 0.375*a*a;
262 int res = SolveP4De( x, b1, c1, d1);
263 if( res==4) { x[0]-= a/4; x[1]-= a/4; x[2]-= a/4; x[3]-= a/4; }
264 else if (res==2) { x[0]-= a/4; x[1]-= a/4; x[2]-= a/4; }
265 else { x[0]-= a/4; x[2]-= a/4; }
266 // one Newton step for each real root:
267 if( res>0 )
268 {
269 x[0] = N4Step(x[0], a,b,c,d);
270 x[1] = N4Step(x[1], a,b,c,d);
271 }
272 if( res>2 )
273 {
274 x[2] = N4Step(x[2], a,b,c,d);
275 x[3] = N4Step(x[3], a,b,c,d);
276 }
277 return res;
278}
279//-----------------------------------------------------------------------------
280#define F5(t) (((((t+a)*t+b)*t+c)*t+d)*t+e)
281//-----------------------------------------------------------------------------
282double SolveP5_1(double a,double b,double c,double d,double e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
283{
284 int cnt;
285 if( fabs(e)<eps ) return 0;
286
287 double brd = fabs(a); // brd - border of real roots
288 if( fabs(b)>brd ) brd = fabs(b);
289 if( fabs(c)>brd ) brd = fabs(c);
290 if( fabs(d)>brd ) brd = fabs(d);
291 if( fabs(e)>brd ) brd = fabs(e);
292 brd++; // brd - border of real roots
293
294 double x0, f0; // less than root
295 double x1, f1; // greater than root
296 double x2, f2, f2s; // next values, f(x2), f'(x2)
297 double dx;
298
299 if( e<0 ) { x0 = 0; x1 = brd; f0=e; f1=F5(x1); x2 = 0.01*brd; } // positive root
300 else { x0 =-brd; x1 = 0; f0=F5(x0); f1=e; x2 =-0.01*brd; } // negative root
301
302 if( fabs(f0)<eps ) return x0;
303 if( fabs(f1)<eps ) return x1;
304
305 // now x0<x1, f(x0)<0, f(x1)>0
306 // Firstly 10 bisections
307 for( cnt=0; cnt<10; cnt++)
308 {
309 x2 = (x0 + x1) / 2; // next point
310 //x2 = x0 - f0*(x1 - x0) / (f1 - f0); // next point
311 f2 = F5(x2); // f(x2)
312 if( fabs(f2)<eps ) return x2;
313 if( f2>0 ) { x1=x2; f1=f2; }
314 else { x0=x2; f0=f2; }
315 }
316
317 // At each step:
318 // x0<x1, f(x0)<0, f(x1)>0.
319 // x2 - next value
320 // we hope that x0 < x2 < x1, but not necessarily
321 do {
322 if(cnt++>50) break;
323 if( x2<=x0 || x2>= x1 ) x2 = (x0 + x1)/2; // now x0 < x2 < x1
324 f2 = F5(x2); // f(x2)
325 if( fabs(f2)<eps ) return x2;
326 if( f2>0 ) { x1=x2; f1=f2; }
327 else { x0=x2; f0=f2; }
328 f2s= (((5*x2+4*a)*x2+3*b)*x2+2*c)*x2+d; // f'(x2)
329 if( fabs(f2s)<eps ) { x2=1e99; continue; }
330 dx = f2/f2s;
331 x2 -= dx;
332 } while(fabs(dx)>eps);
333 return x2;
334} // SolveP5_1(double a,double b,double c,double d,double e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
335//-----------------------------------------------------------------------------
336int SolveP5(double *x,double a,double b,double c,double d,double e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
337{
338 double r = x[0] = SolveP5_1(a,b,c,d,e);
339 double a1 = a+r, b1=b+r*a1, c1=c+r*b1, d1=d+r*c1;
340 return 1+SolveP4(x+1, a1,b1,c1,d1);
341} // SolveP5(double *x,double a,double b,double c,double d,double e) // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
342//-----------------------------------------------------------------------------