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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020