1 #include "precomp.hpp" 2 #include "polynom_solver.h" 3 4 #include <math.h> 5 #include <iostream> 6 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. 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. 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
