MRPT  2.0.4
poly_roots.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9
10 #include "math-precomp.h" // Precompiled headers
11
12 #include <mrpt/math/poly_roots.h>
13 #include <cmath>
14
15 // Based on:
16 // poly.cpp : solution of cubic and quartic equation
17 // (c) Khashin S.I. http://math.ivanovo.ac.ru/dalgebra/Khashin/index.html
18 // khash2 (at) gmail.com
19 //
20
21 #define TwoPi 6.28318530717958648
22 const double eps = 1e-14;
23
24 //---------------------------------------------------------------------------
25 // x - array of size 3
26 // In case 3 real roots: => x[0], x[1], x[2], return 3
27 // 2 real roots: x[0], x[1], return 2
28 // 1 real root : x[0], x[1] +- i*x[2], return 1
29 int mrpt::math::solve_poly3(double* x, double a, double b, double c) noexcept
30 { // solve cubic equation x^3 + a*x^2 + b*x + c
31  double a2 = a * a;
32  double q = (a2 - 3 * b) / 9;
33  double r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
34  double r2 = r * r;
35  double q3 = q * q * q;
36  double A, B;
37  if (r2 < q3)
38  {
39  double t = r / sqrt(q3);
40  if (t < -1) t = -1;
41  if (t > 1) t = 1;
42  t = acos(t);
43  a /= 3;
44  q = -2 * sqrt(q);
45  x[0] = q * cos(t / 3) - a;
46  x[1] = q * cos((t + TwoPi) / 3) - a;
47  x[2] = q * cos((t - TwoPi) / 3) - a;
48  return (3);
49  }
50  else
51  {
52  A = -pow(std::abs(r) + sqrt(r2 - q3), 1. / 3);
53  if (r < 0) A = -A;
54  B = A == 0 ? 0 : q / A;
55
56  a /= 3;
57  x[0] = (A + B) - a;
58  x[1] = -0.5 * (A + B) - a;
59  x[2] = 0.5 * sqrt(3.) * (A - B);
60  if (std::abs(x[2]) < eps)
61  {
62  x[2] = x[1];
63  return (2);
64  }
65  return (1);
66  }
67 } // SolveP3(double *x,double a,double b,double c) {
68 //---------------------------------------------------------------------------
69 // a>=0!
70 void CSqrt(
71  double x, double y, double& a, double& b) // returns: a+i*s = sqrt(x+i*y)
72 {
73  double r = sqrt(x * x + y * y);
74  if (y == 0)
75  {
76  r = sqrt(r);
77  if (x >= 0)
78  {
79  a = r;
80  b = 0;
81  }
82  else
83  {
84  a = 0;
85  b = r;
86  }
87  }
88  else
89  { // y != 0
90  a = sqrt(0.5 * (x + r));
91  b = 0.5 * y / a;
92  }
93 }
94 //---------------------------------------------------------------------------
96  double* x, double b, double d) // solve equation x^4 + b*x^2 + d = 0
97 {
98  double D = b * b - 4 * d;
99  if (D >= 0)
100  {
101  double sD = sqrt(D);
102  double x1 = (-b + sD) / 2;
103  double x2 = (-b - sD) / 2; // x2 <= x1
104  if (x2 >= 0) // 0 <= x2 <= x1, 4 real roots
105  {
106  double sx1 = sqrt(x1);
107  double sx2 = sqrt(x2);
108  x[0] = -sx1;
109  x[1] = sx1;
110  x[2] = -sx2;
111  x[3] = sx2;
112  return 4;
113  }
114  if (x1 < 0) // x2 <= x1 < 0, two pair of imaginary roots
115  {
116  double sx1 = sqrt(-x1);
117  double sx2 = sqrt(-x2);
118  x[0] = 0;
119  x[1] = sx1;
120  x[2] = 0;
121  x[3] = sx2;
122  return 0;
123  }
124  // now x2 < 0 <= x1 , two real roots and one pair of imginary root
125  double sx1 = sqrt(x1);
126  double sx2 = sqrt(-x2);
127  x[0] = -sx1;
128  x[1] = sx1;
129  x[2] = 0;
130  x[3] = sx2;
131  return 2;
132  }
133  else
134  { // if( D < 0 ), two pair of compex roots
135  double sD2 = 0.5 * sqrt(-D);
136  CSqrt(-0.5 * b, sD2, x[0], x[1]);
137  CSqrt(-0.5 * b, -sD2, x[2], x[3]);
138  return 0;
139  } // if( D>=0 )
140 } // SolveP4Bi(double *x, double b, double d) // solve equation x^4 + b*x^2 d
141 //---------------------------------------------------------------------------
142 static void dblSort3(double& a, double& b, double& c) // make: a <= b <= c
143 {
144  if (a > b) std::swap(a, b); // now a<=b
145  if (c < b)
146  {
147  std::swap(b, c); // now a<=b, b<=c
148  if (a > b) std::swap(a, b); // now a<=b
149  }
150 }
151 //---------------------------------------------------------------------------
153  double* x, double b, double c,
154  double d) // solve equation x^4 + b*x^2 + c*x + d
155 {
156  // if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
157  if (std::abs(c) < 1e-14 * (std::abs(b) + std::abs(d)))
158  return SolveP4Bi(x, b, d); // After that, c!=0
159
160  int res3 = mrpt::math::solve_poly3(
161  x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
162  // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
163  if (res3 > 1) // 3 real roots,
164  {
165  dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
166  // Note: x[0]*x[1]*x[2]= c*c > 0
167  if (x[0] > 0) // all roots are positive
168  {
169  double sz1 = sqrt(x[0]);
170  double sz2 = sqrt(x[1]);
171  double sz3 = sqrt(x[2]);
172  // Note: sz1*sz2*sz3= -c (and not equal to 0)
173  if (c > 0)
174  {
175  x[0] = (-sz1 - sz2 - sz3) / 2;
176  x[1] = (-sz1 + sz2 + sz3) / 2;
177  x[2] = (+sz1 - sz2 + sz3) / 2;
178  x[3] = (+sz1 + sz2 - sz3) / 2;
179  return 4;
180  }
181  // now: c<0
182  x[0] = (-sz1 - sz2 + sz3) / 2;
183  x[1] = (-sz1 + sz2 - sz3) / 2;
184  x[2] = (+sz1 - sz2 - sz3) / 2;
185  x[3] = (+sz1 + sz2 + sz3) / 2;
186  return 4;
187  } // if( x[0] > 0) // all roots are positive
188  // now x[0] <= x[1] < 0, x[2] > 0
189  // two pair of comlex roots
190  double sz1 = sqrt(-x[0]);
191  double sz2 = sqrt(-x[1]);
192  double sz3 = sqrt(x[2]);
193
194  if (c > 0) // sign = -1
195  {
196  x[0] = -sz3 / 2;
197  x[1] = (sz1 - sz2) / 2; // x[0]±i*x[1]
198  x[2] = sz3 / 2;
199  x[3] = (-sz1 - sz2) / 2; // x[2]±i*x[3]
200  return 0;
201  }
202  // now: c<0 , sign = +1
203  x[0] = sz3 / 2;
204  x[1] = (-sz1 + sz2) / 2;
205  x[2] = -sz3 / 2;
206  x[3] = (sz1 + sz2) / 2;
207  return 0;
208  } // if( res3>1 ) // 3 real roots,
209  // now resoventa have 1 real and pair of compex roots
210  // x[0] - real root, and x[0]>0,
211  // x[1]±i*x[2] - complex roots,
212  double sz1 = sqrt(x[0]);
213  double szr, szi;
214  CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
215  if (c > 0) // sign = -1
216  {
217  x[0] = -sz1 / 2 - szr; // 1st real root
218  x[1] = -sz1 / 2 + szr; // 2nd real root
219  x[2] = sz1 / 2;
220  x[3] = szi;
221  return 2;
222  }
223  // now: c<0 , sign = +1
224  x[0] = sz1 / 2 - szr; // 1st real root
225  x[1] = sz1 / 2 + szr; // 2nd real root
226  x[2] = -sz1 / 2;
227  x[3] = szi;
228  return 2;
229 } // SolveP4De(double *x, double b, double c, double d) // solve equation
230 // x^4
231 // + b*x^2 + c*x + d
232 //-----------------------------------------------------------------------------
233 double N4Step(
234  double x, double a, double b, double c,
235  double d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
236 {
237  double fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
238  if (fxs == 0) return 1e99;
239  double fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
240  return x - fx / fxs;
241 }
242 //-----------------------------------------------------------------------------
243 // x - array of size 4
244 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
245 // return 2: 2 real roots x[0], x[1] and complex x[2]±i*x[3],
246 // return 0: two pair of complex roots: x[0]+-i*x[1], x[2]+-i*x[3],
248  double* x, double a, double b, double c, double d) noexcept
249 { // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
250  // move to a=0:
251  double d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
252  double c1 = c + 0.5 * a * (0.25 * a * a - b);
253  double b1 = b - 0.375 * a * a;
254  int res = SolveP4De(x, b1, c1, d1);
255  if (res == 4)
256  {
257  x[0] -= a / 4;
258  x[1] -= a / 4;
259  x[2] -= a / 4;
260  x[3] -= a / 4;
261  }
262  else if (res == 2)
263  {
264  x[0] -= a / 4;
265  x[1] -= a / 4;
266  x[2] -= a / 4;
267  }
268  else
269  {
270  x[0] -= a / 4;
271  x[2] -= a / 4;
272  }
273  // one Newton step for each real root:
274  if (res > 0)
275  {
276  x[0] = N4Step(x[0], a, b, c, d);
277  x[1] = N4Step(x[1], a, b, c, d);
278  }
279  if (res > 2)
280  {
281  x[2] = N4Step(x[2], a, b, c, d);
282  x[3] = N4Step(x[3], a, b, c, d);
283  }
284  return res;
285 }
286 //-----------------------------------------------------------------------------
287 #define F5(t) ((((((t) + a) * (t) + b) * (t) + c) * (t) + d) * (t) + e)
288 //-----------------------------------------------------------------------------
289 static double SolveP5_1(
290  double a, double b, double c, double d,
291  double e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
292 {
293  int cnt;
294  if (std::abs(e) < eps) return 0;
295
296  double brd = std::abs(a); // brd - border of real roots
297  if (std::abs(b) > brd) brd = std::abs(b);
298  if (std::abs(c) > brd) brd = std::abs(c);
299  if (std::abs(d) > brd) brd = std::abs(d);
300  if (std::abs(e) > brd) brd = std::abs(e);
301  brd++; // brd - border of real roots
302
303  double x0, f0; // less, than root
304  double x1, f1; // greater, than root
305  double x2, f2, f2s; // next values, f(x2), f'(x2)
306  double dx = 1e8;
307
308  if (e < 0)
309  {
310  x0 = 0;
311  x1 = brd;
312  f0 = e;
313  f1 = F5(x1);
314  x2 = 0.01 * brd;
315  }
316  else
317  {
318  x0 = -brd;
319  x1 = 0;
320  f0 = F5(x0);
321  f1 = e;
322  x2 = -0.01 * brd;
323  }
324
325  if (std::abs(f0) < eps) return x0;
326  if (std::abs(f1) < eps) return x1;
327
328  // now x0<x1, f(x0)<0, f(x1)>0
329  // Firstly 5 bisections
330  for (cnt = 0; cnt < 5; cnt++)
331  {
332  x2 = (x0 + x1) / 2; // next point
333  f2 = F5(x2); // f(x2)
334  if (std::abs(f2) < eps) return x2;
335  if (f2 > 0)
336  {
337  x1 = x2;
338  f1 = f2;
339  }
340  else
341  {
342  x0 = x2;
343  f0 = f2;
344  }
345  }
346
347  // At each step:
348  // x0<x1, f(x0)<0, f(x1)>0.
349  // x2 - next value
350  // we hope that x0 < x2 < x1, but not necessarily
351  do
352  {
353  cnt++;
354  if (x2 <= x0 || x2 >= x1) x2 = (x0 + x1) / 2; // now x0 < x2 < x1
355  f2 = F5(x2); // f(x2)
356  if (std::abs(f2) < eps) return x2;
357  if (f2 > 0)
358  {
359  x1 = x2;
360  f1 = f2;
361  }
362  else
363  {
364  x0 = x2;
365  f0 = f2;
366  }
367  f2s =
368  (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
369  if (std::abs(f2s) < eps)
370  {
371  x2 = 1e99;
372  continue;
373  }
374  dx = f2 / f2s;
375  x2 -= dx;
376  } while (std::abs(dx) > eps);
377  return x2;
378 } // SolveP5_1(double a,double b,double c,double d,double e) // return real
379 // root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
380 //-----------------------------------------------------------------------------
382  double* x, double a, double b, double c, double d,
383  double
384  e) noexcept // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
385 {
386  double r = x[0] = SolveP5_1(a, b, c, d, e);
387  double a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
388  return 1 + solve_poly4(x + 1, a1, b1, c1, d1);
389 } // SolveP5(double *x,double a,double b,double c,double d,double e) // solve
390 // equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
391 //-----------------------------------------------------------------------------
392
393 // a*x^2 + b*x + c = 0
395  double a, double b, double c, double& r1, double& r2) noexcept
396 {
397  if (std::abs(a) < eps)
398  {
399  // b*x+c=0
400  if (std::abs(b) < eps) return 0;
401  r1 = -c / b;
402  r2 = 1e99;
403  return 1;
404  }
405  else
406  {
407  double Di = b * b - 4 * a * c;
408  if (Di < 0)
409  {
410  r1 = r2 = 1e99;
411  return 0;
412  }
413  Di = sqrt(Di);
414  r1 = (-b + Di) / (2 * a);
415  r2 = (-b - Di) / (2 * a);
416  // We ensure at output that r1 <= r2
417  if (r2 < r1) std::swap(r1, r2);
418  return 2;
419  }
420 }
static double SolveP5_1(double a, double b, double c, double d, double e)
Definition: poly_roots.cpp:289
int solve_poly5(double *x, double a, double b, double c, double d, double e) noexcept
Solves equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0.
Definition: poly_roots.cpp:381
int SolveP4Bi(double *x, double b, double d)
Definition: poly_roots.cpp:95
double N4Step(double x, double a, double b, double c, double d)
Definition: poly_roots.cpp:233
static void dblSort3(double &a, double &b, double &c)
Definition: poly_roots.cpp:142
const double eps
Definition: poly_roots.cpp:22
int solve_poly4(double *x, double a, double b, double c, double d) noexcept
Solves quartic equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method. ...
Definition: poly_roots.cpp:247
#define F5(t)
Definition: poly_roots.cpp:287
void CSqrt(double x, double y, double &a, double &b)
Definition: poly_roots.cpp:70
int SolveP4De(double *x, double b, double c, double d)
Definition: poly_roots.cpp:152
int solve_poly3(double *x, double a, double b, double c) noexcept
Solves cubic equation x^3 + a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:29
int solve_poly2(double a, double b, double c, double &r1, double &r2) noexcept
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:394
#define TwoPi
Definition: poly_roots.cpp:21

 Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020