Main MRPT website > C++ reference for MRPT 1.9.9
poly_roots.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
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 #define SWAP(a, b) \
143  { \
144  t = b; \
145  b = a; \
146  a = t; \
147  }
148 static void dblSort3(double& a, double& b, double& c) // make: a <= b <= c
149 {
150  double t;
151  if (a > b) SWAP(a, b); // now a<=b
152  if (c < b)
153  {
154  SWAP(b, c); // now a<=b, b<=c
155  if (a > b) SWAP(a, b); // now a<=b
156  }
157 }
158 //---------------------------------------------------------------------------
160  double* x, double b, double c,
161  double d) // solve equation x^4 + b*x^2 + c*x + d
162 {
163  // if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
164  if (std::abs(c) < 1e-14 * (std::abs(b) + std::abs(d)))
165  return SolveP4Bi(x, b, d); // After that, c!=0
166 
167  int res3 = mrpt::math::solve_poly3(
168  x, 2 * b, b * b - 4 * d, -c * c); // solve resolvent
169  // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
170  if (res3 > 1) // 3 real roots,
171  {
172  dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
173  // Note: x[0]*x[1]*x[2]= c*c > 0
174  if (x[0] > 0) // all roots are positive
175  {
176  double sz1 = sqrt(x[0]);
177  double sz2 = sqrt(x[1]);
178  double sz3 = sqrt(x[2]);
179  // Note: sz1*sz2*sz3= -c (and not equal to 0)
180  if (c > 0)
181  {
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  }
188  // now: c<0
189  x[0] = (-sz1 - sz2 + sz3) / 2;
190  x[1] = (-sz1 + sz2 - sz3) / 2;
191  x[2] = (+sz1 - sz2 - sz3) / 2;
192  x[3] = (+sz1 + sz2 + sz3) / 2;
193  return 4;
194  } // if( x[0] > 0) // all roots are positive
195  // now x[0] <= x[1] < 0, x[2] > 0
196  // two pair of comlex roots
197  double sz1 = sqrt(-x[0]);
198  double sz2 = sqrt(-x[1]);
199  double sz3 = sqrt(x[2]);
200 
201  if (c > 0) // sign = -1
202  {
203  x[0] = -sz3 / 2;
204  x[1] = (sz1 - sz2) / 2; // x[0]±i*x[1]
205  x[2] = sz3 / 2;
206  x[3] = (-sz1 - sz2) / 2; // x[2]±i*x[3]
207  return 0;
208  }
209  // now: c<0 , sign = +1
210  x[0] = sz3 / 2;
211  x[1] = (-sz1 + sz2) / 2;
212  x[2] = -sz3 / 2;
213  x[3] = (sz1 + sz2) / 2;
214  return 0;
215  } // if( res3>1 ) // 3 real roots,
216  // now resoventa have 1 real and pair of compex roots
217  // x[0] - real root, and x[0]>0,
218  // x[1]±i*x[2] - complex roots,
219  double sz1 = sqrt(x[0]);
220  double szr, szi;
221  CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
222  if (c > 0) // sign = -1
223  {
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  }
230  // now: c<0 , sign = +1
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 } // SolveP4De(double *x, double b, double c, double d) // solve equation
237 // x^4
238 // + b*x^2 + c*x + d
239 //-----------------------------------------------------------------------------
240 double N4Step(
241  double x, double a, double b, double c,
242  double d) // one Newton step for x^4 + a*x^3 + b*x^2 + c*x + d
243 {
244  double fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c; // f'(x)
245  if (fxs == 0) return 1e99;
246  double fx = (((x + a) * x + b) * x + c) * x + d; // f(x)
247  return x - fx / fxs;
248 }
249 //-----------------------------------------------------------------------------
250 // x - array of size 4
251 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
252 // return 2: 2 real roots x[0], x[1] and complex x[2]±i*x[3],
253 // return 0: two pair of complex roots: x[0]+-i*x[1], x[2]+-i*x[3],
255  double* x, double a, double b, double c, double d) noexcept
256 { // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
257  // move to a=0:
258  double d1 = d + 0.25 * a * (0.25 * b * a - 3. / 64 * a * a * a - c);
259  double c1 = c + 0.5 * a * (0.25 * a * a - b);
260  double b1 = b - 0.375 * a * a;
261  int res = SolveP4De(x, b1, c1, d1);
262  if (res == 4)
263  {
264  x[0] -= a / 4;
265  x[1] -= a / 4;
266  x[2] -= a / 4;
267  x[3] -= a / 4;
268  }
269  else if (res == 2)
270  {
271  x[0] -= a / 4;
272  x[1] -= a / 4;
273  x[2] -= a / 4;
274  }
275  else
276  {
277  x[0] -= a / 4;
278  x[2] -= a / 4;
279  }
280  // one Newton step for each real root:
281  if (res > 0)
282  {
283  x[0] = N4Step(x[0], a, b, c, d);
284  x[1] = N4Step(x[1], a, b, c, d);
285  }
286  if (res > 2)
287  {
288  x[2] = N4Step(x[2], a, b, c, d);
289  x[3] = N4Step(x[3], a, b, c, d);
290  }
291  return res;
292 }
293 //-----------------------------------------------------------------------------
294 #define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)
295 //-----------------------------------------------------------------------------
296 static double SolveP5_1(
297  double a, double b, double c, double d,
298  double e) // return real root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
299 {
300  int cnt;
301  if (std::abs(e) < eps) return 0;
302 
303  double brd = std::abs(a); // brd - border of real roots
304  if (std::abs(b) > brd) brd = std::abs(b);
305  if (std::abs(c) > brd) brd = std::abs(c);
306  if (std::abs(d) > brd) brd = std::abs(d);
307  if (std::abs(e) > brd) brd = std::abs(e);
308  brd++; // brd - border of real roots
309 
310  double x0, f0; // less, than root
311  double x1, f1; // greater, than root
312  double x2, f2, f2s; // next values, f(x2), f'(x2)
313  double dx = 1e8;
314 
315  if (e < 0)
316  {
317  x0 = 0;
318  x1 = brd;
319  f0 = e;
320  f1 = F5(x1);
321  x2 = 0.01 * brd;
322  }
323  else
324  {
325  x0 = -brd;
326  x1 = 0;
327  f0 = F5(x0);
328  f1 = e;
329  x2 = -0.01 * brd;
330  }
331 
332  if (std::abs(f0) < eps) return x0;
333  if (std::abs(f1) < eps) return x1;
334 
335  // now x0<x1, f(x0)<0, f(x1)>0
336  // Firstly 5 bisections
337  for (cnt = 0; cnt < 5; cnt++)
338  {
339  x2 = (x0 + x1) / 2; // next point
340  f2 = F5(x2); // f(x2)
341  if (std::abs(f2) < eps) return x2;
342  if (f2 > 0)
343  {
344  x1 = x2;
345  f1 = f2;
346  }
347  else
348  {
349  x0 = x2;
350  f0 = f2;
351  }
352  }
353 
354  // At each step:
355  // x0<x1, f(x0)<0, f(x1)>0.
356  // x2 - next value
357  // we hope that x0 < x2 < x1, but not necessarily
358  do
359  {
360  cnt++;
361  if (x2 <= x0 || x2 >= x1) x2 = (x0 + x1) / 2; // now x0 < x2 < x1
362  f2 = F5(x2); // f(x2)
363  if (std::abs(f2) < eps) return x2;
364  if (f2 > 0)
365  {
366  x1 = x2;
367  f1 = f2;
368  }
369  else
370  {
371  x0 = x2;
372  f0 = f2;
373  }
374  f2s =
375  (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d; // f'(x2)
376  if (std::abs(f2s) < eps)
377  {
378  x2 = 1e99;
379  continue;
380  }
381  dx = f2 / f2s;
382  x2 -= dx;
383  } while (std::abs(dx) > eps);
384  return x2;
385 } // SolveP5_1(double a,double b,double c,double d,double e) // return real
386 // root of x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
387 //-----------------------------------------------------------------------------
389  double* x, double a, double b, double c, double d,
390  double
391  e) noexcept // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
392 {
393  double r = x[0] = SolveP5_1(a, b, c, d, e);
394  double a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
395  return 1 + solve_poly4(x + 1, a1, b1, c1, d1);
396 } // SolveP5(double *x,double a,double b,double c,double d,double e) // solve
397 // equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
398 //-----------------------------------------------------------------------------
399 
400 // a*x^2 + b*x + c = 0
402  double a, double b, double c, double& r1, double& r2) noexcept
403 {
404  if (std::abs(a) < eps)
405  {
406  // b*x+c=0
407  if (std::abs(b) < eps) return 0;
408  r1 = -c / b;
409  r2 = 1e99;
410  return 1;
411  }
412  else
413  {
414  double Di = b * b - 4 * a * c;
415  if (Di < 0)
416  {
417  r1 = r2 = 1e99;
418  return 0;
419  }
420  Di = sqrt(Di);
421  r1 = (-b + Di) / (2 * a);
422  r2 = (-b - Di) / (2 * a);
423  // We ensure at output that r1 <= r2
424  double t; // temp:
425  if (r2 < r1) SWAP(r1, r2);
426  return 2;
427  }
428 }
mrpt::obs::gnss::a1
double a1
Definition: gnss_messages_novatel.h:454
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
t
GLdouble GLdouble t
Definition: glext.h:3689
c
const GLubyte * c
Definition: glext.h:6313
math-precomp.h
poly_roots.h
SolveP4Bi
int SolveP4Bi(double *x, double b, double d)
Definition: poly_roots.cpp:95
mrpt::math::solve_poly3
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
eps
const double eps
Definition: poly_roots.cpp:22
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
N4Step
double N4Step(double x, double a, double b, double c, double d)
Definition: poly_roots.cpp:240
dblSort3
static void dblSort3(double &a, double &b, double &c)
Definition: poly_roots.cpp:148
mrpt::obs::gnss::a2
double a2
Definition: gnss_messages_novatel.h:454
F5
#define F5(t)
Definition: poly_roots.cpp:294
mrpt::math::solve_poly5
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:388
res
GLuint res
Definition: glext.h:7268
b
GLubyte GLubyte b
Definition: glext.h:6279
mrpt::math::solve_poly2
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:401
CSqrt
void CSqrt(double x, double y, double &a, double &b)
Definition: poly_roots.cpp:70
SolveP4De
int SolveP4De(double *x, double b, double c, double d)
Definition: poly_roots.cpp:159
SWAP
#define SWAP(a, b)
Definition: poly_roots.cpp:142
mrpt::math::solve_poly4
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:254
mrpt::obs::gnss::b1
double b1
Definition: gnss_messages_novatel.h:454
y
GLenum GLint GLint y
Definition: glext.h:3538
TwoPi
#define TwoPi
Definition: poly_roots.cpp:21
x
GLenum GLint x
Definition: glext.h:3538
SolveP5_1
static double SolveP5_1(double a, double b, double c, double d, double e)
Definition: poly_roots.cpp:296
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST