summaryrefslogtreecommitdiffstats
path: root/libdimension/math
diff options
context:
space:
mode:
Diffstat (limited to 'libdimension/math')
-rw-r--r--libdimension/math/matrix.c388
-rw-r--r--libdimension/math/polynomial.c443
2 files changed, 831 insertions, 0 deletions
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 <tavianator@tavianator.com> *
+ * *
+ * 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 *
+ * <http://www.gnu.org/licenses/>. *
+ *************************************************************************/
+
+/**
+ * @file
+ * Matrix function implementations.
+ */
+
+#include "internal.h"
+#include "dimension/math.h"
+#include <math.h>
+
+// 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 <tavianator@tavianator.com> *
+ * *
+ * 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 *
+ * <http://www.gnu.org/licenses/>. *
+ *************************************************************************/
+
+/**
+ * @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 <math.h>
+
+/// 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, &degree);
+
+ 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");
+ }
+ }
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-10 12:26:42 +0000