/* See LICENSE file for copyright and license details. */ #include "common.h" #ifndef TEST CONST_FUNCTION static double refine_cubic_solution(double t, double a, double b, double c, double d) { size_t i; double x; for (i = 0; x = t*t*t*a + t*t*b + t*c + d, !iszeroish(x) && i < 10; i++) t -= x / (t*t*(a*3) + t*(b*2) + c); return t; } static size_t solve_cubic_with_1_known_root(double *ts, double a, double b, double t1) { /* * (t³ + t²a + tb + c) / (t - t₁) = t² + tp + q * t³ + t²a + tb + c = (t² + tp + q)(t - t₁) * t³ + t²a + tb + c = t²(t - t₁) + tp(t - t₁) + q(t - t₁) * t³ + t²a + tb + c = t³ - t²t₁ + t²p - tpt₁ + tq - qt₁ * t³ + t²a + tb + c = t³ + t²(p - t₁) + t(q - pt₁) - qt₁ * * ⎧a = p - t₁ * ⎨b = q - pt₁ * ⎩c = -qt₁ * * ⎧a + t₁ = p * ⎨b + pt₁ = q * ⎩c = -qt₁ * * ⎰a + t₁ = p * ⎱b + pt₁ = q * * ⎰p = a + t₁ * ⎱q = b + pt₁ */ double p = a + t1; double q = b + p * t1; return solve_quadratic(ts, 1, p, q); } static size_t solve_cubic_with_2_known_roots(double *ts, double a, double b, double c, double t1, double t2) { /* * (t - t₁)(t - t₂)(t - t₃) * (t - t₁)t(t - t₃) - (t - t₁)t₂(t - t₃) * (t - t₁)(t² - t₃t) - (t - t₁)(t₂t - t₂t₃) * (t - t₁)t² - (t - t₁)t₃t - (t - t₁)t₂t + (t - t₁)t₂t₃ * t³ - t₁t² - t₃t² + t₁t₃t - t₂t² + t₁t₂t + t₂t₃t - t₁t₂t₃ * t³ - t²(t₁ + t₂ + t₃) + t(t₁t₂ + t₁t₃ + t₂t₃) - t₁t₂t₃ * * ⎧a = -t₁ - t₂ - t₃ * ⎨b = t₁t₂ + t₁t₃ + t₂t₃ * ⎩c = -t₁t₂t₃ * * ⎧t₃ = -t₁ - t₂ - a * ⎨t₃ = (b - t₁t₂)/(t₁ + t₂) * ⎩t₃ = -c/t₁t₂ */ double t12sum = t1 + t2; double t12prod = t1 * t2; double t3a = -a - t12sum; double t3b = iszeroish(t12sum) ? t3a : (b - t12prod) / t12sum; double t3c = iszeroish(t12prod) ? t3a : -c / t12prod; if (iszeroish(t3a - t3b) && iszeroish(t3a - t3c)) { ts[0] = (t3a + t3b + t3c) / 3.0; return 0.0 <= ts[0] && ts[0] <= 1.0; } t1 = 0.5 * (t1 + t2); t1 = refine_cubic_solution(t1, 1.0, a, b, c); return solve_cubic_with_1_known_root(ts, a, b, t1); } size_t solve_cubic(double *ts, double a, double b, double c, double d) { double t1, t2, t3; unsigned int solutions = 0; size_t n = 0; double u = 1 / (a * -3); double D0 = b*b - 3*(a*c); double D1 = b*b*b - (4.5*b)*(a*c) + (13.5*a)*(a*d); double kk = D1*D1 - D0*D0*D0; double r, i; double Kr; if (kk >= 0) { Kr = D1 < 0 ? cbrt(D1 + sqrt(kk)) : cbrt(D1 - sqrt(kk)); } else { double ki = sqrt(-kk); double h = pow(D1*D1 + ki*ki, 1.0 / 6.0); double v = (0.1 / 0.3) * atan2(ki, D1); Kr = h * cos(v); if (1 /* || fabs(cos(v)) > 1.0 - TOLERANCE */) { double Ki = h * sin(v); double t = 1.0 / (Kr * Kr + Ki * Ki); double m = t * D0 + 1; if (iszeroish(Ki - D0 * Ki * t)) { ts[n] = u * (b + Kr + D0 * Kr * t); ts[n] = t1 = refine_cubic_solution(ts[n], a, b, c, d); solutions |= 1; n += 0.0 <= ts[n] && ts[n] <= 1.0; } if (iszeroish(Ki - sqrt(3.0) * Kr)) { ts[n] = u * (b - (0.5 * Kr + sqrt(0.75) * Ki) * m); ts[n] = t2 = refine_cubic_solution(ts[n], a, b, c, d); solutions |= 2; n += 0.0 <= ts[n] && ts[n] <= 1.0; } if (iszeroish(Ki + sqrt(3.0) * Kr)) { ts[n] = u * (b - (0.5 * Kr - sqrt(0.75) * Ki) * m); ts[n] = t3 = refine_cubic_solution(ts[n], a, b, c, d); solutions |= 4; n += 0.0 <= ts[n] && ts[n] <= 1.0; } /* Sometimes the maths doesn't work well enough */ if (solutions != 7) { #if defined(__GNUC__) && !defined(__clang__) # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #elif defined(__clang__) # pragma clang diagnostic push # pragma clang diagnostic ignored "-Wconditional-uninitialized" #endif switch (solutions) { case 1: n += solve_cubic_with_1_known_root(&ts[n], b / a, c / a, t1); break; case 2: n += solve_cubic_with_1_known_root(&ts[n], b / a, c / a, t2); break; case 4: n += solve_cubic_with_1_known_root(&ts[n], b / a, c / a, t3); break; case 7 - 1: n += solve_cubic_with_2_known_roots(&ts[n], b / a, c / a, d / a, t2, t3); break; case 7 - 2: n += solve_cubic_with_2_known_roots(&ts[n], b / a, c / a, d / a, t1, t3); break; case 7 - 4: n += solve_cubic_with_2_known_roots(&ts[n], b / a, c / a, d / a, t1, t2); break; } #if defined(__GNUC__) && !defined(__clang__) # pragma GCC diagnostic pop #elif defined(__clang__) # pragma clang diagnostic pop #endif } return n; } } if (!iszeroish(Kr)) { r = Kr + D0 / Kr; i = u * sqrt(0.75) * (Kr - D0 / Kr); real_case: ts[n] = u * (b + r); ts[n] = t1 = refine_cubic_solution(ts[n], a, b, c, d); n += 0.0 <= ts[n] && ts[n] <= 1.0; if (iszeroish(i)) { ts[n] = u * (b - 0.5 * r); ts[n] = refine_cubic_solution(ts[n], a, b, c, d); n += 0.0 <= ts[n] && ts[n] <= 1.0; return n; } else { n += solve_cubic_with_1_known_root(&ts[n], b / a, c / a, t1); return n; } } else { double D0divKr = D0 / Kr; r = Kr + D0divKr; i = u * sqrt(0.75) * (Kr - D0divKr); if (isfinite(D0divKr)) /* NOT -ffast-math compatible */ goto real_case; ts[n] = u * b; ts[n] = t1 = refine_cubic_solution(ts[n], a, b, c, d); n += 0.0 <= ts[n] && ts[n] <= 1.0; n += solve_cubic_with_1_known_root(&ts[n], b / a, c / a, t1); return n; } } size_t solve_quadratic(double *ts, double a, double b, double c) { size_t n = 0; double negahalf_p = -0.5 * (b / a); double q = c / a; double D = negahalf_p * negahalf_p - q; if (D < 0) { if (D < -TOLERANCE) return 0; ts[n] = negahalf_p; n += (0.0 <= ts[n] && ts[n] <= 1.0); return n; } else { ts[n] = negahalf_p + sqrt(D); n += (0.0 <= ts[n] && ts[n] <= 1.0); ts[n] = negahalf_p - sqrt(D); n += (0.0 <= ts[n] && ts[n] <= 1.0); return n; } } size_t solve_linear(double *ts, double a, double b) { ts[0] = -b / a; return (0.0 <= ts[0] && ts[0] <= 1.0); } #else #define Re(X) X, 0 #define Im(X) 0, X #define Cx(R, I) R, I static void check_cubic(double r1, double i1, double r2, double i2, double r3, double i3, double k, int strict_real) { size_t nexpected = 0, ngot, i, j; double expected[3], got[3]; double error, maxerror = 0; double ra, rb, rc; double ia, ib, ic; int found[3], correct; if (iszeroish(i1)) expected[nexpected++] = r1; if (iszeroish(i2)) expected[nexpected++] = r2; if (iszeroish(i3)) expected[nexpected++] = r3; /* * k(t₁ - t)(t₂ - t)(t₃ - t) = 0 * k(t₁ - t)t₂(t₃ - t) - k(t₁ - t)t(t₃ - t) = 0 * kt₂(t₁ - t)(t₃ - t) - kt(t₁ - t)(t₃ - t) = 0 * kt₂(t₁(t₃ - t) - t(t₃ - t)) - kt(t₁(t₃ - t) - t(t₃ - t)) = 0 * kt₂(t₁t₃ - t₁t - tt₃ + t²) - kt(t₁t₃ - t₁t - tt₃ + t²) = 0 * kt₂(t₁t₃ - t₁t - tt₃ + t²) + kt(-t₁t₃ + t₁t + tt₃ - t²) = 0 * kt₂t₁t₃ - kt₂t₁t - kt₂tt₃ + kt₂t² - ktt₁t₃ + ktt₁t + kt²t₃ - kt³ = 0 * kt₂t₁t₃ - kt₂t₁t¹ - kt₂t¹t₃ + kt₂t² - kt¹t₁t₃ + kt²t₁ + kt²t₃ - kt³ = 0 * -kt³ + kt₂t² + kt²t₁ + kt²t₃ - kt₂t₁t¹ - kt₂t¹t₃ - kt¹t₁t₃ + kt₂t₁t₃ = 0 * kt³ - kt₂t² - kt²t₁ - kt²t₃ + kt₂t₁t¹ + kt₂t¹t₃ + kt¹t₁t₃ - kt₂t₁t₃ = 0 * t³k - t²k(t₁ + t₂ + t₃) + t¹k(t₂t₁ + t₂t₃ + t₁t₃) - kt₂t₁t₃ = 0 * ⎧a := t₁ + t₂ + t₃ ⎫ * ⎨b := t₁t₂ + t₁t₃ + t₂t₃⎬ * ⎩c := t₁t₂t₃ ⎭ * t³k - t²ka + t¹kb - kc = 0 */ ra = r1 + r2 + r3; ia = i1 + i2 + i3; rb = r1*r2 - i1*i2 + r1*r3 - i1*i3 + r2*r3 - i2*i3; ib = r1*i2 + i1*r2 + r1*i3 + i1*r3 + r2*i3 + i2*r3; rc = r1*r2*r3 - r1*i2*i3 - i1*r2*i3 - i1*i2*r3; ic = r1*r2*i3 + r1*i2*r3 + i1*r2*r3 - i1*i2*i3; if (strict_real) { ASSERT(iszeroish(ia)); ASSERT(iszeroish(ib)); ASSERT(iszeroish(ic)); } else { if (!iszeroish(ia) || !iszeroish(ib) || !iszeroish(ic)) return; } ngot = solve_cubic(got, k, -k*ra, k*rb, -k*rc); for (i = 0; i < sizeof(found) / sizeof(*found); i++) found[i] = (i >= nexpected || expected[i] < 0.0 + TOLERANCE || expected[i] > 1.0 - TOLERANCE); if (!nexpected) { if (!ngot) /* FIXME */ fprintf(stderr, "%lg, %lg, %lg, %lg, %lg, %lg, %lg, %i\n", r1, i1, r2, i2, r3, i3, k, strict_real); ASSERT(!ngot); return; } for (i = 0; i < ngot; i++) { correct = (got[i] < 0.0 + TOLERANCE || got[i] > 1.0 - TOLERANCE); error = fabs(got[i] - expected[0]); for (j = 0; j < nexpected; j++) { error = fmin(error, fabs(got[i] - expected[j])); if (tolerant_eq(got[i], expected[j])) found[j] = correct = 1; } if (!correct) maxerror = fmax(maxerror, error); } if (!found[0] || !found[1] || !found[2] || !tolerant_eq(maxerror, 0)) { fprintf(stderr, "\n%lg, %lg, %lg, %lg, %lg, %lg, %lg\n", r1, i1, r2, i2, r3, i3, k); fprintf(stderr, "\t(%la, %la, %la, %la, %la, %la, %la)\n", r1, i1, r2, i2, r3, i3, k); fprintf(stderr, "\tmaxerror: %lf (%la)\n", maxerror, maxerror); fprintf(stderr, "\tfound: %i, %i, %i\n", found[0], found[1], found[2]); fprintf(stderr, "\tgot: %zu\n", ngot); for (i = 0; i < ngot; i++) fprintf(stderr, "\t\t%lf (%la)\n", got[i], got[i]); fprintf(stderr, "\texpected: %zu\n", nexpected); for (i = 0; i < nexpected; i++) fprintf(stderr, "\t\t%lf (%la)\n", expected[i], expected[i]); } ASSERT(found[0]); ASSERT(found[1]); ASSERT(found[2]); ASSERT(tolerant_eq(maxerror, 0)); } static void check_quadratic(double r1, double i1, double r2, double i2, double k, int strict_real) { size_t nexpected = 0, ngot, i, j; double expected[2], got[2]; double error, maxerror = 0; double ra, rb; double ia, ib; int found[2], correct; if (iszeroish(i1)) expected[nexpected++] = r1; if (iszeroish(i2)) expected[nexpected++] = r2; /* * k(t₁ - t)(t₂ - t) = 0 * k(t₁(t₂ - t) - t(t₂ - t)) = 0 * k(t₁t₂ - t¹t₁ - t¹t₂ + t²) = 0 * kt₁t₂ - t¹kt₁ - t¹kt₂ + kt² = 0 * t²k - t¹kt₁ - t¹kt₂ + kt₁t₂ = 0 * t²k - t¹k(t₁ + kt₂) + kt₁t₂ = 0 * ⎰a := t₁ + t₂⎱ * ⎱b := t₁t₂ ⎰ * t²k - t¹ka + kb = 0 */ ra = r1 + r2; ia = i1 + i2; rb = r1*r2 - i1*i2; ib = r1*i2 + i1*r2; if (strict_real) { ASSERT(iszeroish(ia)); ASSERT(iszeroish(ib)); } else { if (!iszeroish(ia) || !iszeroish(ib)) return; } ngot = solve_quadratic(got, k, -k*ra, k*rb); for (i = 0; i < sizeof(found) / sizeof(*found); i++) found[i] = (i >= nexpected || expected[i] < 0.0 + TOLERANCE || expected[i] > 1.0 - TOLERANCE); if (!nexpected) { ASSERT(!ngot); return; } for (i = 0; i < ngot; i++) { correct = (got[i] < 0.0 + TOLERANCE || got[i] > 1.0 - TOLERANCE); error = fabs(got[i] - expected[0]); for (j = 0; j < nexpected; j++) { error = fmin(error, fabs(got[i] - expected[j])); if (tolerant_eq(got[i], expected[j])) found[j] = correct = 1; } if (!correct) maxerror = fmax(maxerror, error); } if (!found[0] || !found[1] || !tolerant_eq(maxerror, 0)) { fprintf(stderr, "\n%lg, %lg, %lg, %lg, %lg\n", r1, i1, r2, i2, k); fprintf(stderr, "\t(%la, %la, %la, %la, %la)\n", r1, i1, r2, i2, k); fprintf(stderr, "\tmaxerror: %lf (%la)\n", maxerror, maxerror); fprintf(stderr, "\tfound: %i, %i\n", found[0], found[1]); fprintf(stderr, "\tgot: %zu\n", ngot); for (i = 0; i < ngot; i++) fprintf(stderr, "\t\t%lf (%la)\n", got[i], got[i]); fprintf(stderr, "\texpected: %zu\n", nexpected); for (i = 0; i < nexpected; i++) fprintf(stderr, "\t\t%lf (%la)\n", expected[i], expected[i]); } ASSERT(found[0]); ASSERT(found[1]); ASSERT(tolerant_eq(maxerror, 0)); } static void check_linear(double r1, double i1, double k, int strict_real) { size_t nexpected = 0, ngot, i; double expected[1] = {999999}, got[1]; double ra; double ia; if (iszeroish(i1) && 0.0 <= r1 && r1 <= 1.0) expected[nexpected++] = r1; /* * k(t - t₁) = 0 * kt - kt₁ = 0 */ ra = r1; ia = i1; if (strict_real) { ASSERT(iszeroish(ia)); } else { if (!iszeroish(ia)) return; } ngot = solve_linear(got, k, -k*ra); ASSERT(ngot == nexpected); for (i = 0; i < ngot; i++) ASSERT(eq(got[i], expected[i])); } static void check_cubic_randomly(void) { double r1 = rand() / (double)RAND_MAX; double i1 = rand() / (double)RAND_MAX; double r2 = rand() / (double)RAND_MAX; double i2 = rand() / (double)RAND_MAX; double r3 = rand() / (double)RAND_MAX; double i3 = rand() / (double)RAND_MAX; double k = rand() / (double)RAND_MAX; r1 = r1 * 3 - 1; i1 = i1 * 3 - 1; r2 = r2 * 3 - 1; i2 = i2 * 3 - 1; r3 = r3 * 3 - 1; i3 = i3 * 3 - 1; k = k * 3 - 1; k += iszeroish(k); check_cubic(Cx(r1, i1), Cx(r2, i2), Cx(r3, i3), k, 0); check_cubic(Im(i1), Cx(r2, i2), Cx(r3, i3), k, 0); check_cubic(Re(r1), Cx(r2, i2), Cx(r3, i3), k, 0); check_cubic(Cx(r1, i1), Im(i2), Cx(r3, i3), k, 0); check_cubic(Im(i1), Im(i2), Cx(r3, i3), k, 0); check_cubic(Re(r1), Im(i2), Cx(r3, i3), k, 0); check_cubic(Cx(r1, i1), Re(r2), Cx(r3, i3), k, 0); check_cubic(Im(i1), Re(r2), Cx(r3, i3), k, 0); check_cubic(Re(r1), Re(r2), Cx(r3, i3), k, 0); check_cubic(Cx(r1, i1), Cx(r1, i1), Cx(r3, i3), k, 0); check_cubic(Im(i1), Im(i1), Cx(r3, i3), k, 0); check_cubic(Re(r1), Re(r1), Cx(r3, i3), k, 0); check_cubic(Cx(r1, i1), Cx(r3, i3), Cx(r3, i3), k, 0); check_cubic(Im(i1), Cx(r3, i3), Cx(r3, i3), k, 0); check_cubic(Re(r1), Cx(r3, i3), Cx(r3, i3), k, 0); check_cubic(Cx(r1, i1), Cx(r2, i2), Im(i3), k, 0); check_cubic(Im(i1), Cx(r2, i2), Im(i3), k, 0); check_cubic(Re(r1), Cx(r2, i2), Im(i3), k, 0); check_cubic(Cx(r1, i1), Im(i2), Im(i3), k, 0); check_cubic(Im(i1), Im(i2), Im(i3), k, 0); check_cubic(Re(r1), Im(i2), Im(i3), k, 0); check_cubic(Cx(r1, i1), Re(r2), Im(i3), k, 0); check_cubic(Im(i1), Re(r2), Im(i3), k, 0); check_cubic(Re(r1), Re(r2), Im(i3), k, 0); check_cubic(Cx(r1, i1), Cx(r1, i1), Im(i3), k, 0); check_cubic(Im(i1), Im(i1), Im(i3), k, 0); check_cubic(Re(r1), Re(r1), Im(i3), k, 0); check_cubic(Cx(r1, i1), Im(i3), Im(i3), k, 0); check_cubic(Im(i1), Im(i3), Im(i3), k, 0); check_cubic(Re(r1), Im(i3), Im(i3), k, 0); check_cubic(Cx(r1, i1), Cx(r2, i2), Re(r3), k, 0); check_cubic(Im(i1), Cx(r2, i2), Re(r3), k, 0); check_cubic(Re(r1), Cx(r2, i2), Re(r3), k, 0); check_cubic(Cx(r1, i1), Im(i2), Re(r3), k, 0); check_cubic(Im(i1), Im(i2), Re(r3), k, 0); check_cubic(Re(r1), Im(i2), Re(r3), k, 0); check_cubic(Cx(r1, i1), Re(r2), Re(r3), k, 0); check_cubic(Im(i1), Re(r2), Re(r3), k, 0); check_cubic(Re(r1), Re(r2), Re(r3), k, 1); check_cubic(Cx(r1, i1), Cx(r1, i1), Re(r3), k, 0); check_cubic(Im(i1), Im(i1), Re(r3), k, 0); check_cubic(Re(r1), Re(r1), Re(r3), k, 1); check_cubic(Cx(r1, i1), Re(r3), Re(r3), k, 0); check_cubic(Im(i1), Re(r3), Re(r3), k, 0); check_cubic(Re(r1), Re(r3), Re(r3), k, 1); check_cubic(Cx(r1, i1), Cx(r1, i1), Cx(r1, i1), k, 0); check_cubic(Im(i1), Im(i1), Im(i1), k, 0); check_cubic(Re(r1), Re(r1), Re(r1), k, 1); } static void check_quadratic_randomly(void) { double r1 = rand() / (double)RAND_MAX; double i1 = rand() / (double)RAND_MAX; double r2 = rand() / (double)RAND_MAX; double i2 = rand() / (double)RAND_MAX; double k = rand() / (double)RAND_MAX; r1 = r1 * 3 - 1; i1 = i1 * 3 - 1; r2 = r2 * 3 - 1; i2 = i2 * 3 - 1; k = k * 3 - 1; k += iszeroish(k); check_quadratic(Cx(r1, i1), Cx(r2, i2), k, 0); check_quadratic(Im(i1), Cx(r2, i2), k, 0); check_quadratic(Re(r1), Cx(r2, i2), k, 0); check_quadratic(Cx(r1, i1), Im(i2), k, 0); check_quadratic(Im(i1), Im(i2), k, 0); check_quadratic(Re(r1), Im(i2), k, 0); check_quadratic(Cx(r1, i1), Re(r2), k, 0); check_quadratic(Im(i1), Re(r2), k, 0); check_quadratic(Re(r1), Re(r2), k, 1); check_quadratic(Cx(r1, i1), Cx(r1, i1), k, 0); check_quadratic(Im(i1), Im(i1), k, 0); check_quadratic(Re(r1), Re(r1), k, 1); } static void check_linear_randomly(void) { double r1 = rand() / (double)RAND_MAX; double i1 = rand() / (double)RAND_MAX; double k = rand() / (double)RAND_MAX; r1 = r1 * 3 - 1; i1 = i1 * 3 - 1; k = k * 3 - 1; k += iszeroish(k); check_linear(Cx(r1, i1), k, 0); check_linear(Im(i1), k, 0); check_linear(Re(r1), k, 0); } int main(void) { size_t i; void *r = malloc(1); srand((unsigned)(uintptr_t)r); free(r); check_linear(Re(0), 1.0, 1); check_linear(Re(0), 1.5, 1); check_linear(Re(0), 0.5, 1); check_linear(Re(0), -0.5, 1); check_linear(Re(0), -1.0, 1); check_linear(Re(0), -1.5, 1); for (i = 0; i < 100000UL; i++) check_linear_randomly(); check_quadratic(Re(0), Re(0), 1.0, 1); check_quadratic(Re(0), Re(0), 1.5, 1); check_quadratic(Re(0), Re(0), 0.5, 1); check_quadratic(Re(0), Re(0), -0.5, 1); check_quadratic(Re(0), Re(0), -1.0, 1); check_quadratic(Re(0), Re(0), -1.5, 1); for (i = 0; i < 10000000UL; i++) check_quadratic_randomly(); check_cubic(Re(0), Re(0), Re(0), 1.0, 1); check_cubic(Re(0), Re(0), Re(0), 1.5, 1); check_cubic(Re(0), Re(0), Re(0), 0.5, 1); check_cubic(Re(0), Re(0), Re(0), -0.5, 1); check_cubic(Re(0), Re(0), Re(0), -1.0, 1); check_cubic(Re(0), Re(0), Re(0), -1.5, 1); for (i = 0; i < 1000000UL; i++) check_cubic_randomly(); return 0; } #endif