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