From 7b09710392d35fb55b52031d447a542d99fc6b4b Mon Sep 17 00:00:00 2001 From: Tavian Barnes Date: Tue, 19 Aug 2014 17:10:03 -0400 Subject: Modularize the libdimension codebase. --- libdimension/math/matrix.c | 388 ++++++++++++++++++++++++++++++++++++ libdimension/math/polynomial.c | 443 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 831 insertions(+) create mode 100644 libdimension/math/matrix.c create mode 100644 libdimension/math/polynomial.c (limited to 'libdimension/math') diff --git a/libdimension/math/matrix.c b/libdimension/math/matrix.c new file mode 100644 index 0000000..25590d8 --- /dev/null +++ b/libdimension/math/matrix.c @@ -0,0 +1,388 @@ +/************************************************************************* + * Copyright (C) 2009-2014 Tavian Barnes * + * * + * This file is part of The Dimension Library. * + * * + * The Dimension Library is free software; you can redistribute it and/ * + * or modify it under the terms of the GNU Lesser General Public License * + * as published by the Free Software Foundation; either version 3 of the * + * License, or (at your option) any later version. * + * * + * The Dimension Library is distributed in the hope that it will be * + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty * + * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this program. If not, see * + * . * + *************************************************************************/ + +/** + * @file + * Matrix function implementations. + */ + +#include "internal.h" +#include "dimension/math.h" +#include + +// Identity matrix +dmnsn_matrix +dmnsn_identity_matrix(void) +{ + return dmnsn_new_matrix(1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0); +} + +// Scaling matrix +dmnsn_matrix +dmnsn_scale_matrix(dmnsn_vector s) +{ + return dmnsn_new_matrix(s.x, 0.0, 0.0, 0.0, + 0.0, s.y, 0.0, 0.0, + 0.0, 0.0, s.z, 0.0); +} + +// Translation matrix +dmnsn_matrix +dmnsn_translation_matrix(dmnsn_vector d) +{ + return dmnsn_new_matrix(1.0, 0.0, 0.0, d.x, + 0.0, 1.0, 0.0, d.y, + 0.0, 0.0, 1.0, d.z); +} + +// Left-handed rotation matrix; theta/|theta| = axis, |theta| = angle +dmnsn_matrix +dmnsn_rotation_matrix(dmnsn_vector theta) +{ + // Two trig calls, 25 multiplications, 13 additions + + double angle = dmnsn_vector_norm(theta); + if (fabs(angle) < dmnsn_epsilon) { + return dmnsn_identity_matrix(); + } + dmnsn_vector axis = dmnsn_vector_div(theta, angle); + + // Shorthand to make dmnsn_new_matrix() call legible + + double s = sin(angle); + double t = 1.0 - cos(angle); + + double x = axis.x; + double y = axis.y; + double z = axis.z; + + return dmnsn_new_matrix( + 1.0 + t*(x*x - 1.0), -z*s + t*x*y, y*s + t*x*z, 0.0, + z*s + t*x*y, 1.0 + t*(y*y - 1.0), -x*s + t*y*z, 0.0, + -y*s + t*x*z, x*s + t*y*z, 1.0 + t*(z*z - 1.0), 0.0 + ); +} + +// Find the angle between two vectors with respect to an axis +static double +dmnsn_axis_angle(dmnsn_vector from, dmnsn_vector to, dmnsn_vector axis) +{ + from = dmnsn_vector_sub(from, dmnsn_vector_proj(from, axis)); + to = dmnsn_vector_sub(to, dmnsn_vector_proj(to, axis)); + + double fromnorm = dmnsn_vector_norm(from); + double tonorm = dmnsn_vector_norm(to); + if (fromnorm < dmnsn_epsilon || tonorm < dmnsn_epsilon) { + return 0.0; + } + + from = dmnsn_vector_div(from, fromnorm); + to = dmnsn_vector_div(to, tonorm); + + double angle = acos(dmnsn_vector_dot(from, to)); + + if (dmnsn_vector_dot(dmnsn_vector_cross(from, to), axis) > 0.0) { + return angle; + } else { + return -angle; + } +} + +// Alignment matrix +dmnsn_matrix +dmnsn_alignment_matrix(dmnsn_vector from, dmnsn_vector to, + dmnsn_vector axis1, dmnsn_vector axis2) +{ + double theta1 = dmnsn_axis_angle(from, to, axis1); + dmnsn_matrix align1 = dmnsn_rotation_matrix(dmnsn_vector_mul(theta1, axis1)); + from = dmnsn_transform_direction(align1, from); + axis2 = dmnsn_transform_direction(align1, axis2); + + double theta2 = dmnsn_axis_angle(from, to, axis2); + dmnsn_matrix align2 = dmnsn_rotation_matrix(dmnsn_vector_mul(theta2, axis2)); + + return dmnsn_matrix_mul(align2, align1); +} + +// Matrix inversion helper functions + +/// A 2x2 matrix for inversion by partitioning. +typedef struct { double n[2][2]; } dmnsn_matrix2; + +/// Construct a 2x2 matrix. +static dmnsn_matrix2 dmnsn_new_matrix2(double a1, double a2, + double b1, double b2); +/// Invert a 2x2 matrix. +static dmnsn_matrix2 dmnsn_matrix2_inverse(dmnsn_matrix2 A); +/// Negate a 2x2 matrix. +static dmnsn_matrix2 dmnsn_matrix2_negate(dmnsn_matrix2 A); +/// Subtract two 2x2 matricies. +static dmnsn_matrix2 dmnsn_matrix2_sub(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs); +/// Add two 2x2 matricies. +static dmnsn_matrix2 dmnsn_matrix2_mul(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs); + +/// Invert a matrix with the slower cofactor algorithm, if partitioning failed. +static dmnsn_matrix dmnsn_matrix_inverse_generic(dmnsn_matrix A); +/// Get the [\p row, \p col] cofactor of A. +static double dmnsn_matrix_cofactor(dmnsn_matrix A, size_t row, size_t col); + +// Invert a matrix, by partitioning +dmnsn_matrix +dmnsn_matrix_inverse(dmnsn_matrix A) +{ + // Use partitioning to invert a matrix: + // + // [ P Q ] -1 + // [ R S ] + // + // = [ PP QQ ] + // [ RR SS ], + // + // with PP = inv(P) - inv(P)*Q*RR, + // QQ = -inv(P)*Q*SS, + // RR = -SS*R*inv(P), and + // SS = inv(S - R*inv(P)*Q). + + // The algorithm uses 2 inversions, 6 multiplications, and 2 subtractions, + // giving 52 multiplications, 34 additions, and 8 divisions. + + dmnsn_matrix2 P, Q, R, S, Pi, RPi, PiQ, RPiQ, PP, QQ, RR, SS; + double Pdet = A.n[0][0]*A.n[1][1] - A.n[0][1]*A.n[1][0]; + + if (dmnsn_unlikely(fabs(Pdet) < dmnsn_epsilon)) { + // If P is close to singular, try a more generic algorithm; this is very + // unlikely, but not impossible, eg. + // [ 1 1 0 0 ] + // [ 1 1 1 0 ] + // [ 0 1 1 0 ] + // [ 0 0 0 1 ] + return dmnsn_matrix_inverse_generic(A); + } + + // Partition the matrix + P = dmnsn_new_matrix2(A.n[0][0], A.n[0][1], + A.n[1][0], A.n[1][1]); + Q = dmnsn_new_matrix2(A.n[0][2], A.n[0][3], + A.n[1][2], A.n[1][3]); + R = dmnsn_new_matrix2(A.n[2][0], A.n[2][1], + 0.0, 0.0); + S = dmnsn_new_matrix2(A.n[2][2], A.n[2][3], + 0.0, 1.0); + + // Do this inversion ourselves, since we already have the determinant + Pi = dmnsn_new_matrix2( P.n[1][1]/Pdet, -P.n[0][1]/Pdet, + -P.n[1][0]/Pdet, P.n[0][0]/Pdet); + + // Calculate R*inv(P), inv(P)*Q, and R*inv(P)*Q + RPi = dmnsn_matrix2_mul(R, Pi); + PiQ = dmnsn_matrix2_mul(Pi, Q); + RPiQ = dmnsn_matrix2_mul(R, PiQ); + + // Calculate the partitioned inverse + SS = dmnsn_matrix2_inverse(dmnsn_matrix2_sub(S, RPiQ)); + RR = dmnsn_matrix2_negate(dmnsn_matrix2_mul(SS, RPi)); + QQ = dmnsn_matrix2_negate(dmnsn_matrix2_mul(PiQ, SS)); + PP = dmnsn_matrix2_sub(Pi, dmnsn_matrix2_mul(PiQ, RR)); + + // Reconstruct the matrix + return dmnsn_new_matrix(PP.n[0][0], PP.n[0][1], QQ.n[0][0], QQ.n[0][1], + PP.n[1][0], PP.n[1][1], QQ.n[1][0], QQ.n[1][1], + RR.n[0][0], RR.n[0][1], SS.n[0][0], SS.n[0][1]); +} + +// For nice shorthand +static dmnsn_matrix2 +dmnsn_new_matrix2(double a1, double a2, double b1, double b2) +{ + dmnsn_matrix2 m = { { { a1, a2 }, + { b1, b2 } } }; + return m; +} + +// Invert a 2x2 matrix +static dmnsn_matrix2 +dmnsn_matrix2_inverse(dmnsn_matrix2 A) +{ + // 4 divisions, 2 multiplications, 1 addition + double det = A.n[0][0]*A.n[1][1] - A.n[0][1]*A.n[1][0]; + return dmnsn_new_matrix2( A.n[1][1]/det, -A.n[0][1]/det, + -A.n[1][0]/det, A.n[0][0]/det); +} + +// Also basically a shorthand +static dmnsn_matrix2 +dmnsn_matrix2_negate(dmnsn_matrix2 A) +{ + return dmnsn_new_matrix2(-A.n[0][0], -A.n[0][1], + -A.n[1][0], -A.n[1][1]); +} + +// 2x2 matrix subtraction +static dmnsn_matrix2 +dmnsn_matrix2_sub(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs) +{ + // 4 additions + return dmnsn_new_matrix2( + lhs.n[0][0] - rhs.n[0][0], lhs.n[0][1] - rhs.n[0][1], + lhs.n[1][0] - rhs.n[1][0], lhs.n[1][1] - rhs.n[1][1] + ); +} + +// 2x2 matrix multiplication +static dmnsn_matrix2 +dmnsn_matrix2_mul(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs) +{ + // 8 multiplications, 4 additions + return dmnsn_new_matrix2( + lhs.n[0][0]*rhs.n[0][0] + lhs.n[0][1]*rhs.n[1][0], + lhs.n[0][0]*rhs.n[0][1] + lhs.n[0][1]*rhs.n[1][1], + lhs.n[1][0]*rhs.n[0][0] + lhs.n[1][1]*rhs.n[1][0], + lhs.n[1][0]*rhs.n[0][1] + lhs.n[1][1]*rhs.n[1][1] + ); +} + +// Invert a matrix, if partitioning failed (|P| == 0) +static dmnsn_matrix +dmnsn_matrix_inverse_generic(dmnsn_matrix A) +{ + // For A = [ A' b ] A^-1 = [ A'^-1 -(A'^-1)*b ] + // [ 0 ... 0 1 ], [ 0 ... 0 1 ]. + // + // Invert A' by calculating its adjucate. + dmnsn_matrix inv; + double det = 0.0, C; + + // Perform a Laplace expansion along the first row to give us the adjugate's + // first column and the determinant + for (size_t j = 0; j < 3; ++j) { + C = dmnsn_matrix_cofactor(A, 0, j); + det += A.n[0][j]*C; + inv.n[j][0] = C; + } + + // Divide the first column by the determinant + for (size_t j = 0; j < 3; ++j) { + inv.n[j][0] /= det; + } + + // Find the rest of A' + for (size_t j = 0; j < 3; ++j) { + for (size_t i = 1; i < 3; ++i) { + inv.n[j][i] = dmnsn_matrix_cofactor(A, i, j)/det; + } + inv.n[j][3] = 0.0; + } + + // Find the translational component of the inverse + for (size_t i = 0; i < 3; ++i) { + for (size_t j = 0; j < 3; ++j) { + inv.n[i][3] -= inv.n[i][j]*A.n[j][3]; + } + } + + return inv; +} + +// Gives the cofactor at row, col; the determinant of the matrix formed from the +// upper-left 3x3 corner of A by ignoring row `row' and column `col', +// times (-1)^(row + col) +static double +dmnsn_matrix_cofactor(dmnsn_matrix A, size_t row, size_t col) +{ + // 2 multiplications, 1 addition + double n[4]; + size_t k = 0; + for (size_t i = 0; i < 3; ++i) { + for (size_t j = 0; j < 3; ++j) { + if (i != row && j != col) { + n[k] = A.n[i][j]; + ++k; + } + } + } + + double C = n[0]*n[3] - n[1]*n[2]; + if ((row + col)%2 == 0) { + return C; + } else { + return -C; + } +} + +// 4x4 matrix multiplication +dmnsn_matrix +dmnsn_matrix_mul(dmnsn_matrix lhs, dmnsn_matrix rhs) +{ + // 36 multiplications, 27 additions + dmnsn_matrix r; + + r.n[0][0] = lhs.n[0][0]*rhs.n[0][0] + lhs.n[0][1]*rhs.n[1][0] + lhs.n[0][2]*rhs.n[2][0]; + r.n[0][1] = lhs.n[0][0]*rhs.n[0][1] + lhs.n[0][1]*rhs.n[1][1] + lhs.n[0][2]*rhs.n[2][1]; + r.n[0][2] = lhs.n[0][0]*rhs.n[0][2] + lhs.n[0][1]*rhs.n[1][2] + lhs.n[0][2]*rhs.n[2][2]; + r.n[0][3] = lhs.n[0][0]*rhs.n[0][3] + lhs.n[0][1]*rhs.n[1][3] + lhs.n[0][2]*rhs.n[2][3] + lhs.n[0][3]; + + r.n[1][0] = lhs.n[1][0]*rhs.n[0][0] + lhs.n[1][1]*rhs.n[1][0] + lhs.n[1][2]*rhs.n[2][0]; + r.n[1][1] = lhs.n[1][0]*rhs.n[0][1] + lhs.n[1][1]*rhs.n[1][1] + lhs.n[1][2]*rhs.n[2][1]; + r.n[1][2] = lhs.n[1][0]*rhs.n[0][2] + lhs.n[1][1]*rhs.n[1][2] + lhs.n[1][2]*rhs.n[2][2]; + r.n[1][3] = lhs.n[1][0]*rhs.n[0][3] + lhs.n[1][1]*rhs.n[1][3] + lhs.n[1][2]*rhs.n[2][3] + lhs.n[1][3]; + + r.n[2][0] = lhs.n[2][0]*rhs.n[0][0] + lhs.n[2][1]*rhs.n[1][0] + lhs.n[2][2]*rhs.n[2][0]; + r.n[2][1] = lhs.n[2][0]*rhs.n[0][1] + lhs.n[2][1]*rhs.n[1][1] + lhs.n[2][2]*rhs.n[2][1]; + r.n[2][2] = lhs.n[2][0]*rhs.n[0][2] + lhs.n[2][1]*rhs.n[1][2] + lhs.n[2][2]*rhs.n[2][2]; + r.n[2][3] = lhs.n[2][0]*rhs.n[0][3] + lhs.n[2][1]*rhs.n[1][3] + lhs.n[2][2]*rhs.n[2][3] + lhs.n[2][3]; + + return r; +} + +// Give an axis-aligned box that contains the given box transformed by `lhs' +dmnsn_aabb +dmnsn_transform_aabb(dmnsn_matrix trans, dmnsn_aabb box) +{ + // Infinite/zero bounding box support + if (isinf(box.min.x)) { + return box; + } + + // Taking the "absolute value" of the matrix saves some min/max calculations + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + trans.n[i][j] = fabs(trans.n[i][j]); + } + } + + dmnsn_vector Mt = dmnsn_matrix_column(trans, 3); + dmnsn_aabb ret = { Mt, Mt }; + + dmnsn_vector Mz = dmnsn_matrix_column(trans, 2); + ret.min = dmnsn_vector_add(ret.min, dmnsn_vector_mul(box.min.z, Mz)); + ret.max = dmnsn_vector_add(ret.max, dmnsn_vector_mul(box.max.z, Mz)); + + dmnsn_vector My = dmnsn_matrix_column(trans, 1); + ret.min = dmnsn_vector_add(ret.min, dmnsn_vector_mul(box.min.y, My)); + ret.max = dmnsn_vector_add(ret.max, dmnsn_vector_mul(box.max.y, My)); + + dmnsn_vector Mx = dmnsn_matrix_column(trans, 0); + ret.min = dmnsn_vector_add(ret.min, dmnsn_vector_mul(box.min.x, Mx)); + ret.max = dmnsn_vector_add(ret.max, dmnsn_vector_mul(box.max.x, Mx)); + + return ret; +} diff --git a/libdimension/math/polynomial.c b/libdimension/math/polynomial.c new file mode 100644 index 0000000..09e9603 --- /dev/null +++ b/libdimension/math/polynomial.c @@ -0,0 +1,443 @@ +/************************************************************************* + * Copyright (C) 2010-2011 Tavian Barnes * + * * + * This file is part of The Dimension Library. * + * * + * The Dimension Library is free software; you can redistribute it and/ * + * or modify it under the terms of the GNU Lesser General Public License * + * as published by the Free Software Foundation; either version 3 of the * + * License, or (at your option) any later version. * + * * + * The Dimension Library is distributed in the hope that it will be * + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty * + * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this program. If not, see * + * . * + *************************************************************************/ + +/** + * @file + * Real root isolation algorithm based on work by Vincent, Uspensky, Collins and + * Akritas, Johnson, Krandick, and Rouillier and Zimmerman. + */ + +#include "internal.h" +#include "internal/polynomial.h" +#include "dimension/math.h" +#include + +/// Get the real degree of a polynomial, ignoring leading zeros. +static inline size_t +dmnsn_real_degree(const double poly[], size_t degree) +{ + for (size_t i = degree + 1; i-- > 0;) { + if (dmnsn_likely(fabs(poly[i]) >= dmnsn_epsilon)) { + return i; + } + } + + return 0; +} + +/// Divide each coefficient by the leading coefficient. +static inline void +dmnsn_polynomial_normalize(double poly[], size_t degree) +{ + for (size_t i = 0; i < degree; ++i) { + poly[i] /= poly[degree]; + } + poly[degree] = 1.0; +} + +/// Eliminate trivial zero roots from \p poly[]. +static inline void +dmnsn_eliminate_zero_roots(double **poly, size_t *degree) +{ + size_t i; + for (i = 0; i <= *degree; ++i) { + if (dmnsn_likely(fabs((*poly)[i]) >= dmnsn_epsilon)) { + break; + } + } + + *poly += i; + *degree -= i; +} + +/// Calculate a finite upper bound on the roots of a normalized polynomial. +static inline double +dmnsn_root_bound(const double poly[], size_t degree) +{ + double bound = fabs(poly[0]); + for (size_t i = 1; i < degree; ++i) { + bound = dmnsn_max(bound, fabs(poly[i])); + } + bound += 1.0; + return bound; +} + +/// Copy a polynomial. +static inline void +dmnsn_polynomial_copy(double dest[], const double src[], size_t degree) +{ + for (size_t i = 0; i <= degree; ++i) { + dest[i] = src[i]; + } +} + +/// Transform a polynomial by P'(x) = P(x + 1). +static inline void +dmnsn_polynomial_translate(double poly[], size_t degree) +{ + for (size_t i = 0; i <= degree; ++i) { + for (size_t j = degree - i; j <= degree - 1; ++j) { + poly[j] += poly[j + 1]; + } + } +} + +/// Transform a polynomial by P'(x) = P(c*x). +static inline void +dmnsn_polynomial_scale(double poly[], size_t degree, double c) +{ + double factor = c; + for (size_t i = 1; i <= degree; ++i) { + poly[i] *= factor; + factor *= c; + } +} + +/// Returns the result of Descartes' rule on x^degree * poly(1/(x + 1)). +static size_t +dmnsn_descartes_bound(const double poly[], size_t degree) +{ + // Copy the polynomial so we can be destructive + double p[degree + 1]; + dmnsn_polynomial_copy(p, poly, degree); + + // Calculate poly(1/(1/x + 1)) which avoids reversal + for (size_t i = 1; i <= degree; ++i) { + for (size_t j = i; j >= 1; --j) { + p[j] += p[j - 1]; + } + } + + // Find the number of sign changes in p[] + size_t changes = 0; + int lastsign = dmnsn_sgn(p[0]); + for (size_t i = 1; changes <= 1 && i <= degree; ++i) { + int sign = dmnsn_sgn(p[i]); + if (sign != 0 && sign != lastsign) { + ++changes; + lastsign = sign; + } + } + + return changes; +} + +/// Depth-first search of possible isolating intervals. +static size_t +dmnsn_root_bounds_recursive(double poly[], size_t degree, double *c, double *k, + double bounds[][2], size_t nbounds) +{ + size_t s = dmnsn_descartes_bound(poly, degree); + if (s >= 2) { + // Get the left child + dmnsn_polynomial_scale(poly, degree, 1.0/2.0); + *c *= 2.0; + *k /= 2.0; + double currc = *c, currk = *k; + + // Test the left child + size_t n = dmnsn_root_bounds_recursive(poly, degree, c, k, bounds, nbounds); + if (nbounds == n) { + return n; + } + bounds += n; + nbounds -= n; + + // Get the right child from the last tested polynomial + dmnsn_polynomial_translate(poly, degree); + dmnsn_polynomial_scale(poly, degree, currk/(*k)); + *c = currc + 1.0; + *k = currk; + + // Test the right child + n += dmnsn_root_bounds_recursive(poly, degree, c, k, bounds, nbounds); + return n; + } else if (s == 1) { + bounds[0][0] = (*c)*(*k); + bounds[0][1] = (*c + 1.0)*(*k); + return 1; + } else { + return 0; + } +} + +/// Find ranges that contain a single root. +static size_t +dmnsn_root_bounds(const double poly[], size_t degree, double bounds[][2], + size_t nbounds) +{ + // Copy the polynomial so we can be destructive + double p[degree + 1]; + dmnsn_polynomial_copy(p, poly, degree); + + // Scale the roots to within (0, 1] + double bound = dmnsn_root_bound(p, degree); + dmnsn_polynomial_scale(p, degree, bound); + + // Bounding intervals are of the form (c*k, (c + 1)*k) + double c = 0.0, k = 1.0; + + // Isolate the roots + size_t n = dmnsn_root_bounds_recursive(p, degree, &c, &k, bounds, nbounds); + + // Scale the roots back to within (0, bound] + for (size_t i = 0; i < n; ++i) { + bounds[i][0] *= bound; + bounds[i][1] *= bound; + } + + return n; +} + +/// Maximum number of iterations in dmnsn_bisect_root() before bailout. +#define DMNSN_BISECT_ITERATIONS 64 + +/// Use the false position method to find a root in a range that contains +/// exactly one root. +static inline double +dmnsn_bisect_root(const double poly[], size_t degree, double min, double max) +{ + double evmin = dmnsn_polynomial_evaluate(poly, degree, min); + double evmax = dmnsn_polynomial_evaluate(poly, degree, max); + + // Handle equal bounds, and equal values at the bounds. + if (dmnsn_unlikely(fabs(evmax - evmin) < dmnsn_epsilon)) { + return (min + max)/2.0; + } + + double evinitial = dmnsn_min(fabs(evmin), fabs(evmax)); + double mid, evmid; + int lastsign = 0; + + for (size_t i = 0; i < DMNSN_BISECT_ITERATIONS; ++i) { + mid = (min*evmax - max*evmin)/(evmax - evmin); + evmid = dmnsn_polynomial_evaluate(poly, degree, mid); + int sign = dmnsn_sgn(evmid); + + if ((fabs(evmid) < fabs(mid)*dmnsn_epsilon + // This condition improves stability when one of the bounds is close to + // a different root than we are trying to find + && fabs(evmid) <= evinitial) + || max - min < fabs(mid)*dmnsn_epsilon) + { + break; + } + + if (mid < min) { + // This can happen due to numerical instability in the root bounding + // algorithm, so behave like the normal secant method + max = min; + evmax = evmin; + min = mid; + evmin = evmid; + } else if (mid > max) { + min = max; + evmin = evmax; + max = mid; + evmax = evmid; + } else if (sign == dmnsn_sgn(evmax)) { + max = mid; + evmax = evmid; + if (sign == lastsign) { + // Don't allow the algorithm to keep the same endpoint for three + // iterations in a row; this ensures superlinear convergence + evmin /= 2.0; + } + } else { + min = mid; + evmin = evmid; + if (sign == lastsign) { + evmax /= 2.0; + } + } + + lastsign = sign; + } + + return mid; +} + +/// Use synthetic division to eliminate the root \p r from \p poly[]. +static inline size_t +dmnsn_eliminate_root(double poly[], size_t degree, double r) +{ + double rem = poly[degree]; + for (size_t i = degree; i-- > 0;) { + double temp = poly[i]; + poly[i] = rem; + rem = temp + r*rem; + } + return degree - 1; +} + +/// Solve a normalized linear polynomial algebraically. +static inline size_t +dmnsn_solve_linear(const double poly[2], double x[1]) +{ + x[0] = -poly[0]; + if (x[0] >= dmnsn_epsilon) + return 1; + else + return 0; +} + +/// Solve a normalized quadratic polynomial algebraically. +static inline size_t +dmnsn_solve_quadratic(const double poly[3], double x[2]) +{ + double disc = poly[1]*poly[1] - 4.0*poly[0]; + if (disc >= 0.0) { + double s = sqrt(disc); + x[0] = (-poly[1] + s)/2.0; + x[1] = (-poly[1] - s)/2.0; + + if (x[1] >= dmnsn_epsilon) + return 2; + else if (x[0] >= dmnsn_epsilon) + return 1; + else + return 0; + } else { + return 0; + } +} + +/// Solve a normalized cubic polynomial algebraically. +static inline size_t +dmnsn_solve_cubic(double poly[4], double x[3]) +{ + // Reduce to a monic trinomial (t^3 + p*t + q, t = x + b/3) + double b2 = poly[2]*poly[2]; + double p = poly[1] - b2/3.0; + double q = poly[0] - poly[2]*(9.0*poly[1] - 2.0*b2)/27.0; + + double disc = 4.0*p*p*p + 27.0*q*q; + double bdiv3 = poly[2]/3.0; + + if (disc < 0.0) { + // Three real roots -- this implies p < 0 + double msqrtp3 = -sqrt(-p/3.0); + double theta = acos(3*q/(2*p*msqrtp3))/3.0; + + // Store the roots in order from largest to smallest + x[2] = 2.0*msqrtp3*cos(theta) - bdiv3; + x[0] = -2.0*msqrtp3*cos(4.0*atan(1.0)/3.0 - theta) - bdiv3; + x[1] = -(x[0] + x[2] + poly[2]); + + if (x[2] >= dmnsn_epsilon) + return 3; + else if (x[1] >= dmnsn_epsilon) + return 2; + } else if (disc > 0.0) { + // One real root + double cbrtdiscq = cbrt(sqrt(disc/108.0) + fabs(q)/2.0); + double abst = cbrtdiscq - p/(3.0*cbrtdiscq); + + if (q >= 0) { + x[0] = -abst - bdiv3; + } else { + x[0] = abst - bdiv3; + } + } else if (fabs(p) < dmnsn_epsilon) { + // Equation is a perfect cube + x[0] = -bdiv3; + } else { + // Two real roots; one duplicate + double t1 = -(3.0*q)/(2.0*p), t2 = -2.0*t1; + x[0] = dmnsn_max(t1, t2) - bdiv3; + x[1] = dmnsn_min(t1, t2) - bdiv3; + if (x[1] >= dmnsn_epsilon) + return 2; + } + + if (x[0] >= dmnsn_epsilon) + return 1; + else + return 0; +} + +// Solve a polynomial +DMNSN_HOT size_t +dmnsn_polynomial_solve(const double poly[], size_t degree, double x[]) +{ + // Copy the polynomial so we can be destructive + double copy[degree + 1], *p = copy; + dmnsn_polynomial_copy(p, poly, degree); + + // Index into x[] + size_t i = 0; + + // Account for leading zero coefficients + degree = dmnsn_real_degree(p, degree); + // Normalize the leading coefficient to 1.0 + dmnsn_polynomial_normalize(p, degree); + // Eliminate simple zero roots + dmnsn_eliminate_zero_roots(&p, °ree); + + static const size_t max_algebraic = 3; + if (degree > max_algebraic) { + // Find isolating intervals for (degree - max_algebraic) roots of p[] + double ranges[degree - max_algebraic][2]; + size_t n = dmnsn_root_bounds(p, degree, ranges, degree - max_algebraic); + + for (size_t j = 0; j < n; ++j) { + // Bisect within the found range + double r = dmnsn_bisect_root(p, degree, ranges[j][0], ranges[j][1]); + + // Use synthetic division to eliminate the root `r' + degree = dmnsn_eliminate_root(p, degree, r); + + // Store the found root + x[i] = r; + ++i; + } + } + + switch (degree) { + case 1: + i += dmnsn_solve_linear(p, x + i); + break; + case 2: + i += dmnsn_solve_quadratic(p, x + i); + break; + case 3: + i += dmnsn_solve_cubic(p, x + i); + break; + } + + return i; +} + +// Print a polynomial +void +dmnsn_polynomial_print(FILE *file, const double poly[], size_t degree) +{ + for (size_t i = degree + 1; i-- > 0;) { + if (i < degree) { + fprintf(file, (poly[i] >= 0.0) ? " + " : " - "); + } + fprintf(file, "%.17g", fabs(poly[i])); + if (i >= 2) { + fprintf(file, "*x^%zu", i); + } else if (i == 1) { + fprintf(file, "*x"); + } + } +} -- cgit v1.2.3