Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 "base-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) MRPT_NO_THROWS { // solve cubic equation x^3 + a*x^2 + b*x + c
30  double a2 = a*a;
31  double q = (a2 - 3*b)/9;
32  double r = (a*(2*a2-9*b) + 27*c)/54;
33  double r2 = r*r;
34  double q3 = q*q*q;
35  double A,B;
36  if(r2<q3) {
37  double t=r/sqrt(q3);
38  if( t<-1) t=-1;
39  if( t> 1) t= 1;
40  t=acos(t);
41  a/=3; q=-2*sqrt(q);
42  x[0]=q*cos(t/3)-a;
43  x[1]=q*cos((t+TwoPi)/3)-a;
44  x[2]=q*cos((t-TwoPi)/3)-a;
45  return(3);
46  } else {
47  A =-pow(std::abs(r)+sqrt(r2-q3),1./3);
48  if( r<0 ) A=-A;
49  B = A==0? 0 : q/A;
50 
51  a/=3;
52  x[0] =(A+B)-a;
53  x[1] =-0.5*(A+B)-a;
54  x[2] = 0.5*sqrt(3.)*(A-B);
55  if(std::abs(x[2])<eps) { x[2]=x[1]; return(2); }
56  return(1);
57  }
58 }// SolveP3(double *x,double a,double b,double c) {
59 //---------------------------------------------------------------------------
60 // a>=0!
61 void CSqrt( double x, double y, double &a, double &b) // returns: a+i*s = sqrt(x+i*y)
62 {
63  double r = sqrt(x*x+y*y);
64  if( y==0 ) {
65  r = sqrt(r);
66  if(x>=0) { a=r; b=0; } else { a=0; b=r; }
67  } else { // y != 0
68  a = sqrt(0.5*(x+r));
69  b = 0.5*y/a;
70  }
71 }
72 //---------------------------------------------------------------------------
73 int SolveP4Bi(double *x, double b, double d) // solve equation x^4 + b*x^2 + d = 0
74 {
75  double D = b*b-4*d;
76  if( D>=0 )
77  {
78  double sD = sqrt(D);
79  double x1 = (-b+sD)/2;
80  double x2 = (-b-sD)/2; // x2 <= x1
81  if( x2>=0 ) // 0 <= x2 <= x1, 4 real roots
82  {
83  double sx1 = sqrt(x1);
84  double sx2 = sqrt(x2);
85  x[0] = -sx1;
86  x[1] = sx1;
87  x[2] = -sx2;
88  x[3] = sx2;
89  return 4;
90  }
91  if( x1 < 0 ) // x2 <= x1 < 0, two pair of imaginary roots
92  {
93  double sx1 = sqrt(-x1);
94  double sx2 = sqrt(-x2);
95  x[0] = 0;
96  x[1] = sx1;
97  x[2] = 0;
98  x[3] = sx2;
99  return 0;
100  }
101  // now x2 < 0 <= x1 , two real roots and one pair of imginary root
102  double sx1 = sqrt( x1);
103  double sx2 = sqrt(-x2);
104  x[0] = -sx1;
105  x[1] = sx1;
106  x[2] = 0;
107  x[3] = sx2;
108  return 2;
109  } else { // if( D < 0 ), two pair of compex roots
110  double sD2 = 0.5*sqrt(-D);
111  CSqrt(-0.5*b, sD2, x[0],x[1]);
112  CSqrt(-0.5*b,-sD2, x[2],x[3]);
113  return 0;
114  } // if( D>=0 )
115 } // SolveP4Bi(double *x, double b, double d) // solve equation x^4 + b*x^2 d
116 //---------------------------------------------------------------------------
117 #define SWAP(a,b) { t=b; b=a; a=t; }
118 static void dblSort3( double &a, double &b, double &c) // make: a <= b <= c
119 {
120  double t;
121  if( a>b ) SWAP(a,b); // now a<=b
122  if( c<b ) {
123  SWAP(b,c); // now a<=b, b<=c
124  if( a>b ) SWAP(a,b);// now a<=b
125  }
126 }
127 //---------------------------------------------------------------------------
128 int SolveP4De(double *x, double b, double c, double d) // solve equation x^4 + b*x^2 + c*x + d
129 {
130  //if( c==0 ) return SolveP4Bi(x,b,d); // After that, c!=0
131  if( std::abs(c)<1e-14*(std::abs(b)+std::abs(d)) ) return SolveP4Bi(x,b,d); // After that, c!=0
132 
133  int res3 = mrpt::math::solve_poly3( x, 2*b, b*b-4*d, -c*c); // solve resolvent
134  // by Viet theorem: x1*x2*x3=-c*c not equals to 0, so x1!=0, x2!=0, x3!=0
135  if( res3>1 ) // 3 real roots,
136  {
137  dblSort3(x[0], x[1], x[2]); // sort roots to x[0] <= x[1] <= x[2]
138  // Note: x[0]*x[1]*x[2]= c*c > 0
139  if( x[0] > 0) // all roots are positive
140  {
141  double sz1 = sqrt(x[0]);
142  double sz2 = sqrt(x[1]);
143  double sz3 = sqrt(x[2]);
144  // Note: sz1*sz2*sz3= -c (and not equal to 0)
145  if( c>0 )
146  {
147  x[0] = (-sz1 -sz2 -sz3)/2;
148  x[1] = (-sz1 +sz2 +sz3)/2;
149  x[2] = (+sz1 -sz2 +sz3)/2;
150  x[3] = (+sz1 +sz2 -sz3)/2;
151  return 4;
152  }
153  // now: c<0
154  x[0] = (-sz1 -sz2 +sz3)/2;
155  x[1] = (-sz1 +sz2 -sz3)/2;
156  x[2] = (+sz1 -sz2 -sz3)/2;
157  x[3] = (+sz1 +sz2 +sz3)/2;
158  return 4;
159  } // if( x[0] > 0) // all roots are positive
160  // now x[0] <= x[1] < 0, x[2] > 0
161  // two pair of comlex roots
162  double sz1 = sqrt(-x[0]);
163  double sz2 = sqrt(-x[1]);
164  double sz3 = sqrt( x[2]);
165 
166  if( c>0 ) // sign = -1
167  {
168  x[0] = -sz3/2;
169  x[1] = ( sz1 -sz2)/2; // x[0]±i*x[1]
170  x[2] = sz3/2;
171  x[3] = (-sz1 -sz2)/2; // x[2]±i*x[3]
172  return 0;
173  }
174  // now: c<0 , sign = +1
175  x[0] = sz3/2;
176  x[1] = (-sz1 +sz2)/2;
177  x[2] = -sz3/2;
178  x[3] = ( sz1 +sz2)/2;
179  return 0;
180  } // if( res3>1 ) // 3 real roots,
181  // now resoventa have 1 real and pair of compex roots
182  // x[0] - real root, and x[0]>0,
183  // x[1]±i*x[2] - complex roots,
184  double sz1 = sqrt(x[0]);
185  double szr, szi;
186  CSqrt(x[1], x[2], szr, szi); // (szr+i*szi)^2 = x[1]+i*x[2]
187  if( c>0 ) // sign = -1
188  {
189  x[0] = -sz1/2-szr; // 1st real root
190  x[1] = -sz1/2+szr; // 2nd real root
191  x[2] = sz1/2;
192  x[3] = szi;
193  return 2;
194  }
195  // now: c<0 , sign = +1
196  x[0] = sz1/2-szr; // 1st real root
197  x[1] = sz1/2+szr; // 2nd real root
198  x[2] = -sz1/2;
199  x[3] = szi;
200  return 2;
201 } // SolveP4De(double *x, double b, double c, double d) // solve equation x^4 + b*x^2 + c*x + d
202 //-----------------------------------------------------------------------------
203 double 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
204 {
205  double fxs= ((4*x+3*a)*x+2*b)*x+c; // f'(x)
206  if( fxs==0 ) return 1e99;
207  double fx = (((x+a)*x+b)*x+c)*x+d; // f(x)
208  return x - fx/fxs;
209 }
210 //-----------------------------------------------------------------------------
211 // x - array of size 4
212 // return 4: 4 real roots x[0], x[1], x[2], x[3], possible multiple roots
213 // return 2: 2 real roots x[0], x[1] and complex x[2]±i*x[3],
214 // return 0: two pair of complex roots: x[0]+-i*x[1], x[2]+-i*x[3],
215 int mrpt::math::solve_poly4(double *x,double a,double b,double c,double d) MRPT_NO_THROWS { // solve equation x^4 + a*x^3 + b*x^2 + c*x + d by Dekart-Euler method
216  // move to a=0:
217  double d1 = d + 0.25*a*( 0.25*b*a - 3./64*a*a*a - c);
218  double c1 = c + 0.5*a*(0.25*a*a - b);
219  double b1 = b - 0.375*a*a;
220  int res = SolveP4De( x, b1, c1, d1);
221  if( res==4) { x[0]-= a/4; x[1]-= a/4; x[2]-= a/4; x[3]-= a/4; }
222  else if (res==2) { x[0]-= a/4; x[1]-= a/4; x[2]-= a/4; }
223  else { x[0]-= a/4; x[2]-= a/4; }
224  // one Newton step for each real root:
225  if( res>0 )
226  {
227  x[0] = N4Step(x[0], a,b,c,d);
228  x[1] = N4Step(x[1], a,b,c,d);
229  }
230  if( res>2 )
231  {
232  x[2] = N4Step(x[2], a,b,c,d);
233  x[3] = N4Step(x[3], a,b,c,d);
234  }
235  return res;
236 }
237 //-----------------------------------------------------------------------------
238 #define F5(t) (((((t+a)*t+b)*t+c)*t+d)*t+e)
239 //-----------------------------------------------------------------------------
240 static double 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
241 {
242  int cnt;
243  if( std::abs(e)<eps ) return 0;
244 
245  double brd = std::abs(a); // brd - border of real roots
246  if( std::abs(b)>brd ) brd = std::abs(b);
247  if( std::abs(c)>brd ) brd = std::abs(c);
248  if( std::abs(d)>brd ) brd = std::abs(d);
249  if( std::abs(e)>brd ) brd = std::abs(e);
250  brd++; // brd - border of real roots
251 
252  double x0, f0; // less, than root
253  double x1, f1; // greater, than root
254  double x2, f2, f2s; // next values, f(x2), f'(x2)
255  double dx=1e8;
256 
257  if( e<0 ) { x0 = 0; x1 = brd; f0=e; f1=F5(x1); x2 = 0.01*brd; }
258  else { x0 =-brd; x1 = 0; f0=F5(x0); f1=e; x2 =-0.01*brd; }
259 
260  if( std::abs(f0)<eps ) return x0;
261  if( std::abs(f1)<eps ) return x1;
262 
263  // now x0<x1, f(x0)<0, f(x1)>0
264  // Firstly 5 bisections
265  for( cnt=0; cnt<5; cnt++)
266  {
267  x2 = (x0 + x1)/2; // next point
268  f2 = F5(x2); // f(x2)
269  if( std::abs(f2)<eps ) return x2;
270  if( f2>0 ) { x1=x2; f1=f2; }
271  else { x0=x2; f0=f2; }
272  }
273 
274  // At each step:
275  // x0<x1, f(x0)<0, f(x1)>0.
276  // x2 - next value
277  // we hope that x0 < x2 < x1, but not necessarily
278  do {
279  cnt++;
280  if( x2<=x0 || x2>= x1 ) x2 = (x0 + x1)/2; // now x0 < x2 < x1
281  f2 = F5(x2); // f(x2)
282  if( std::abs(f2)<eps ) return x2;
283  if( f2>0 ) { x1=x2; f1=f2; }
284  else { x0=x2; f0=f2; }
285  f2s= (((5*x2+4*a)*x2+3*b)*x2+2*c)*x2+d; // f'(x2)
286  if( std::abs(f2s)<eps ) { x2=1e99; continue; }
287  dx = f2/f2s;
288  x2 -= dx;
289  } while(std::abs(dx)>eps);
290  return x2;
291 } // 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
292 //-----------------------------------------------------------------------------
293 int mrpt::math::solve_poly5(double *x,double a,double b,double c,double d,double e) MRPT_NO_THROWS // solve equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0
294 {
295  double r = x[0] = SolveP5_1(a,b,c,d,e);
296  double a1 = a+r, b1=b+r*a1, c1=c+r*b1, d1=d+r*c1;
297  return 1+solve_poly4(x+1, a1,b1,c1,d1);
298 } // 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
299 //-----------------------------------------------------------------------------
300 
301 // a*x^2 + b*x + c = 0
302 int mrpt::math::solve_poly2(double a, double b, double c, double &r1, double &r2) MRPT_NO_THROWS
303 {
304  if (std::abs(a)<eps)
305  {
306  // b*x+c=0
307  if (std::abs(b)<eps)
308  return 0;
309  r1 = -c/b;
310  r2 =1e99;
311  return 1;
312  }
313  else
314  {
315  double Di = b*b - 4*a*c;
316  if( Di<0 ) { r1=r2=1e99; return 0; }
317  Di = sqrt(Di);
318  r1 = (-b + Di)/(2*a);
319  r2 = (-b - Di)/(2*a);
320  // We ensure at output that r1 <= r2
321  double t; // temp:
322  if (r2<r1) SWAP(r1,r2);
323  return 2;
324  }
325 }
SolveP5_1
static double SolveP5_1(double a, double b, double c, double d, double e)
Definition: poly_roots.cpp:240
mrpt::math::solve_poly3
int BASE_IMPEXP solve_poly3(double *x, double a, double b, double c) MRPT_NO_THROWS
Solves cubic equation x^3 + a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:29
t
GLdouble GLdouble t
Definition: glext.h:3610
poly_roots.h
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
mrpt::math::solve_poly4
int BASE_IMPEXP solve_poly4(double *x, double a, double b, double c, double d) MRPT_NO_THROWS
Solves quartic equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method. ...
Definition: poly_roots.cpp:215
MRPT_NO_THROWS
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
Definition: maps/CColouredPointsMap.h:72
SolveP4Bi
int SolveP4Bi(double *x, double b, double d)
Definition: poly_roots.cpp:73
N4Step
double N4Step(double x, double a, double b, double c, double d)
Definition: poly_roots.cpp:203
dblSort3
static void dblSort3(double &a, double &b, double &c)
Definition: poly_roots.cpp:118
eps
const double eps
Definition: poly_roots.cpp:22
c
const GLubyte * c
Definition: glext.h:5590
F5
#define F5(t)
Definition: poly_roots.cpp:238
b
GLubyte GLubyte b
Definition: glext.h:5575
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
CSqrt
void CSqrt(double x, double y, double &a, double &b)
Definition: poly_roots.cpp:61
SolveP4De
int SolveP4De(double *x, double b, double c, double d)
Definition: poly_roots.cpp:128
SWAP
#define SWAP(a, b)
Definition: poly_roots.cpp:117
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::math::solve_poly2
int BASE_IMPEXP solve_poly2(double a, double b, double c, double &r1, double &r2) MRPT_NO_THROWS
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:302
res
GLuint res
Definition: glext.h:6298
x
GLenum GLint x
Definition: glext.h:3516
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
mrpt::math::solve_poly5
int BASE_IMPEXP solve_poly5(double *x, double a, double b, double c, double d, double e) MRPT_NO_THROWS
Solves equation x^5 + a*x^4 + b*x^3 + c*x^2 + d*x + e = 0.
Definition: poly_roots.cpp:293
TwoPi
#define TwoPi
Definition: poly_roots.cpp:21


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019