static int cubicRootsX(double A, double B, double C, double D, double s[3]) { if (approximately_zero(A)) { // we're just a quadratic return quadraticRootsX(B, C, D, s); } if (approximately_zero(D)) { // 0 is one root int num = quadraticRootsX(A, B, C, s); for (int i = 0; i < num; ++i) { if (approximately_zero(s[i])) { return num; } } s[num++] = 0; return num; } if (approximately_zero(A + B + C + D)) { // 1 is one root int num = quadraticRootsX(A, A + B, -D, s); for (int i = 0; i < num; ++i) { if (approximately_equal(s[i], 1)) { return num; } } s[num++] = 1; return num; } double a, b, c; { double invA = 1 / A; a = B * invA; b = C * invA; c = D * invA; } double a2 = a * a; double Q = (a2 - b * 3) / 9; double R = (2 * a2 * a - 9 * a * b + 27 * c) / 54; double Q3 = Q * Q * Q; double R2MinusQ3 = R * R - Q3; double adiv3 = a / 3; double r; double* roots = s; if (approximately_zero_squared(R2MinusQ3)) { if (approximately_zero(R)) {/* one triple solution */ *roots++ = -adiv3; } else { /* one single and one double solution */ double u = cube_root(-R); *roots++ = 2 * u - adiv3; *roots++ = -u - adiv3; } } else if (R2MinusQ3 < 0) // we have 3 real roots { double theta = acos(R / sqrt(Q3)); double neg2RootQ = -2 * sqrt(Q); r = neg2RootQ * cos(theta / 3) - adiv3; *roots++ = r; r = neg2RootQ * cos((theta + 2 * PI) / 3) - adiv3; *roots++ = r; r = neg2RootQ * cos((theta - 2 * PI) / 3) - adiv3; *roots++ = r; } else // we have 1 real root { double A = fabs(R) + sqrt(R2MinusQ3); A = cube_root(A); if (R > 0) { A = -A; } if (A != 0) { A += Q / A; } r = A - adiv3; *roots++ = r; } return (int)(roots - s); }
int quarticRoots(const double A, const double B, const double C, const double D, const double E, double s[4]) { if (approximately_zero(A)) { if (approximately_zero(B)) { return quadraticRootsX(C, D, E, s); } return cubicRootsX(B, C, D, E, s); } int num; int i; if (approximately_zero(E)) { // 0 is one root num = cubicRootsX(A, B, C, D, s); for (i = 0; i < num; ++i) { if (approximately_zero(s[i])) { return num; } } s[num++] = 0; return num; } if (approximately_zero_squared(A + B + C + D + E)) { // 1 is one root num = cubicRootsX(A, A + B, -(D + E), -E, s); // note that -C==A+B+D+E for (i = 0; i < num; ++i) { if (approximately_equal(s[i], 1)) { return num; } } s[num++] = 1; return num; } double u, v; /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */ const double invA = 1 / A; const double a = B * invA; const double b = C * invA; const double c = D * invA; const double d = E * invA; /* substitute x = y - a/4 to eliminate cubic term: x^4 + px^2 + qx + r = 0 */ const double a2 = a * a; const double p = -3 * a2 / 8 + b; const double q = a2 * a / 8 - a * b / 2 + c; const double r = -3 * a2 * a2 / 256 + a2 * b / 16 - a * c / 4 + d; if (approximately_zero(r)) { /* no absolute term: y(y^3 + py + q) = 0 */ num = cubicRootsX(1, 0, p, q, s); s[num++] = 0; } else { /* solve the resolvent cubic ... */ (void) cubicRootsX(1, -p / 2, -r, r * p / 2 - q * q / 8, s); /* ... and take the one real solution ... */ const double z = s[0]; /* ... to build two quadric equations */ u = z * z - r; v = 2 * z - p; if (approximately_zero(u)) { u = 0; } else if (u > 0) { u = sqrt(u); } else { return 0; } if (approximately_zero(v)) { v = 0; } else if (v > 0) { v = sqrt(v); } else { return 0; } num = quadraticRootsX(1, q < 0 ? -v : v, z - u, s); num += quadraticRootsX(1, q < 0 ? v : -v, z + u, s + num); } // eliminate duplicates for (i = 0; i < num - 1; ++i) { for (int j = i + 1; j < num; ) { if (approximately_equal(s[i], s[j])) { if (j < --num) { s[j] = s[num]; } } else { ++j; } } } /* resubstitute */ const double sub = a / 4; for (i = 0; i < num; ++i) { s[i] -= sub; } return num; }
static int findRoots(const QuadImplicitForm& i, const Quadratic& q2, double roots[4], bool useCubic, bool& disregardCount) { double a, b, c; set_abc(&q2[0].x, a, b, c); double d, e, f; set_abc(&q2[0].y, d, e, f); const double t4 = i.x2() * a * a + i.xy() * a * d + i.y2() * d * d; const double t3 = 2 * i.x2() * a * b + i.xy() * (a * e + b * d) + 2 * i.y2() * d * e; const double t2 = i.x2() * (b * b + 2 * a * c) + i.xy() * (c * d + b * e + a * f) + i.y2() * (e * e + 2 * d * f) + i.x() * a + i.y() * d; const double t1 = 2 * i.x2() * b * c + i.xy() * (c * e + b * f) + 2 * i.y2() * e * f + i.x() * b + i.y() * e; const double t0 = i.x2() * c * c + i.xy() * c * f + i.y2() * f * f + i.x() * c + i.y() * f + i.c(); #if QUARTIC_DEBUG // create a string mathematica understands char str[1024]; bzero(str, sizeof(str)); sprintf(str, "Solve[%1.19g x^4 + %1.19g x^3 + %1.19g x^2 + %1.19g x + %1.19g == 0, x]", t4, t3, t2, t1, t0); #endif if (approximately_zero(t4)) { disregardCount = true; if (approximately_zero(t3)) { return quadraticRootsX(t2, t1, t0, roots); } return cubicRootsX(t3, t2, t1, t0, roots); } if (approximately_zero(t0)) { // 0 is one root disregardCount = true; int num = cubicRootsX(t4, t3, t2, t1, roots); for (int i = 0; i < num; ++i) { if (approximately_zero(roots[i])) { return num; } } roots[num++] = 0; return num; } if (useCubic) { assert(approximately_zero(t4 + t3 + t2 + t1 + t0)); // 1 is one root int num = cubicRootsX(t4, t4 + t3, -(t1 + t0), -t0, roots); // note that -C==A+B+D+E for (int i = 0; i < num; ++i) { if (approximately_equal(roots[i], 1)) { return num; } } roots[num++] = 1; return num; } return quarticRoots(t4, t3, t2, t1, t0, roots); }