Main MRPT website > C++ reference for MRPT 1.5.5
polynom_solver.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 "vision-precomp.h" // Precompiled headers
11 #include "polynom_solver.h"
12 
13 #include <math.h>
14 #include <iostream>
15 
16 #define CV_PI 3.14159265358979323846
17 
18 int solve_deg2(double a, double b, double c, double & x1, double & x2)
19 {
20  double delta = b * b - 4 * a * c;
21 
22  if (delta < 0) return 0;
23 
24  double inv_2a = 0.5 / a;
25 
26  if (delta == 0) {
27  x1 = -b * inv_2a;
28  x2 = x1;
29  return 1;
30  }
31 
32  double sqrt_delta = sqrt(delta);
33  x1 = (-b + sqrt_delta) * inv_2a;
34  x2 = (-b - sqrt_delta) * inv_2a;
35  return 2;
36 }
37 
38 
39 /// Reference : Eric W. Weisstein. "Cubic Equation." From MathWorld--A Wolfram Web Resource.
40 /// http://mathworld.wolfram.com/CubicEquation.html
41 /// \return Number of real roots found.
42 int solve_deg3(double a, double b, double c, double d,
43  double & x0, double & x1, double & x2)
44 {
45  if (a == 0) {
46  // Solve second order sytem
47  if (b == 0) {
48  // Solve first order system
49  if (c == 0)
50  return 0;
51 
52  x0 = -d / c;
53  return 1;
54  }
55 
56  x2 = 0;
57  return solve_deg2(b, c, d, x0, x1);
58  }
59 
60  // Calculate the normalized form x^3 + a2 * x^2 + a1 * x + a0 = 0
61  double inv_a = 1. / a;
62  double b_a = inv_a * b, b_a2 = b_a * b_a;
63  double c_a = inv_a * c;
64  double d_a = inv_a * d;
65 
66  // Solve the cubic equation
67  double Q = (3 * c_a - b_a2) / 9;
68  double R = (9 * b_a * c_a - 27 * d_a - 2 * b_a * b_a2) / 54;
69  double Q3 = Q * Q * Q;
70  double D = Q3 + R * R;
71  double b_a_3 = (1. / 3.) * b_a;
72 
73  if (Q == 0) {
74  if(R == 0) {
75  x0 = x1 = x2 = - b_a_3;
76  return 3;
77  }
78  else {
79  x0 = pow(2 * R, 1 / 3.0) - b_a_3;
80  return 1;
81  }
82  }
83 
84  if (D <= 0) {
85  // Three real roots
86  double theta = acos(R / sqrt(-Q3));
87  double sqrt_Q = sqrt(-Q);
88  x0 = 2 * sqrt_Q * cos(theta / 3.0) - b_a_3;
89  x1 = 2 * sqrt_Q * cos((theta + 2 * CV_PI)/ 3.0) - b_a_3;
90  x2 = 2 * sqrt_Q * cos((theta + 4 * CV_PI)/ 3.0) - b_a_3;
91 
92  return 3;
93  }
94 
95  // D > 0, only one real root
96  double AD = pow(fabs(R) + sqrt(D), 1.0 / 3.0) * (R > 0 ? 1 : (R < 0 ? -1 : 0));
97  double BD = (AD == 0) ? 0 : -Q / AD;
98 
99  // Calculate the only real root
100  x0 = AD + BD - b_a_3;
101 
102  return 1;
103 }
104 
105 /// Reference : Eric W. Weisstein. "Quartic Equation." From MathWorld--A Wolfram Web Resource.
106 /// http://mathworld.wolfram.com/QuarticEquation.html
107 /// \return Number of real roots found.
108 int solve_deg4(double a, double b, double c, double d, double e,
109  double & x0, double & x1, double & x2, double & x3)
110 {
111  if (a == 0) {
112  x3 = 0;
113  return solve_deg3(b, c, d, e, x0, x1, x2);
114  }
115 
116  // Normalize coefficients
117  double inv_a = 1. / a;
118  b *= inv_a; c *= inv_a; d *= inv_a; e *= inv_a;
119  double b2 = b * b, bc = b * c, b3 = b2 * b;
120 
121  // Solve resultant cubic
122  double r0, r1, r2;
123  int n = solve_deg3(1, -c, d * b - 4 * e, 4 * c * e - d * d - b2 * e, r0, r1, r2);
124  if (n == 0) return 0;
125 
126  // Calculate R^2
127  double R2 = 0.25 * b2 - c + r0, R;
128  if (R2 < 0)
129  return 0;
130 
131  R = sqrt(R2);
132  double inv_R = 1. / R;
133 
134  int nb_real_roots = 0;
135 
136  // Calculate D^2 and E^2
137  double D2, E2;
138  if (R < 10E-12) {
139  double temp = r0 * r0 - 4 * e;
140  if (temp < 0)
141  D2 = E2 = -1;
142  else {
143  double sqrt_temp = sqrt(temp);
144  D2 = 0.75 * b2 - 2 * c + 2 * sqrt_temp;
145  E2 = D2 - 4 * sqrt_temp;
146  }
147  }
148  else {
149  double u = 0.75 * b2 - 2 * c - R2,
150  v = 0.25 * inv_R * (4 * bc - 8 * d - b3);
151  D2 = u + v;
152  E2 = u - v;
153  }
154 
155  double b_4 = 0.25 * b, R_2 = 0.5 * R;
156  if (D2 >= 0) {
157  double D = sqrt(D2);
158  nb_real_roots = 2;
159  double D_2 = 0.5 * D;
160  x0 = R_2 + D_2 - b_4;
161  x1 = x0 - D;
162  }
163 
164  // Calculate E^2
165  if (E2 >= 0) {
166  double E = sqrt(E2);
167  double E_2 = 0.5 * E;
168  if (nb_real_roots == 0) {
169  x0 = - R_2 + E_2 - b_4;
170  x1 = x0 - E;
171  nb_real_roots = 2;
172  }
173  else {
174  x2 = - R_2 + E_2 - b_4;
175  x3 = x2 - E;
176  nb_real_roots = 4;
177  }
178  }
179 
180  return nb_real_roots;
181 }
#define CV_PI
GLenum GLsizei n
Definition: glext.h:4618
const GLubyte * c
Definition: glext.h:5590
GLubyte GLubyte b
Definition: glext.h:5575
int solve_deg4(double a, double b, double c, double d, double e, double &x0, double &x1, double &x2, double &x3)
Reference : Eric W.
const GLdouble * v
Definition: glext.h:3603
const float R
int solve_deg2(double a, double b, double c, double &x1, double &x2)
int solve_deg3(double a, double b, double c, double d, double &x0, double &x1, double &x2)
Reference : Eric W.
GLubyte GLubyte GLubyte a
Definition: glext.h:5575



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019