diff options
Diffstat (limited to 'intern/libmv/libmv/numeric/poly.h')
| -rw-r--r-- | intern/libmv/libmv/numeric/poly.h | 123 |
1 files changed, 123 insertions, 0 deletions
diff --git a/intern/libmv/libmv/numeric/poly.h b/intern/libmv/libmv/numeric/poly.h new file mode 100644 index 00000000000..76ba062d475 --- /dev/null +++ b/intern/libmv/libmv/numeric/poly.h @@ -0,0 +1,123 @@ +// Copyright (c) 2007, 2008 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_NUMERIC_POLY_H_ +#define LIBMV_NUMERIC_POLY_H_ + +#include <cmath> +#include <stdio.h> + +namespace libmv { + +// Solve the cubic polynomial +// +// x^3 + a*x^2 + b*x + c = 0 +// +// The number of roots (from zero to three) is returned. If the number of roots +// is less than three, then higher numbered x's are not changed. For example, +// if there are 2 roots, only x0 and x1 are set. +// +// The GSL cubic solver was used as a reference for this routine. +template<typename Real> +int SolveCubicPolynomial(Real a, Real b, Real c, + Real *x0, Real *x1, Real *x2) { + Real q = a * a - 3 * b; + Real r = 2 * a * a * a - 9 * a * b + 27 * c; + + Real Q = q / 9; + Real R = r / 54; + + Real Q3 = Q * Q * Q; + Real R2 = R * R; + + Real CR2 = 729 * r * r; + Real CQ3 = 2916 * q * q * q; + + if (R == 0 && Q == 0) { + // Tripple root in one place. + *x0 = *x1 = *x2 = -a / 3; + return 3; + + } else if (CR2 == CQ3) { + // This test is actually R2 == Q3, written in a form suitable for exact + // computation with integers. + // + // Due to finite precision some double roots may be missed, and considered + // to be a pair of complex roots z = x +/- epsilon i close to the real + // axis. + Real sqrtQ = sqrt(Q); + if (R > 0) { + *x0 = -2 * sqrtQ - a / 3; + *x1 = sqrtQ - a / 3; + *x2 = sqrtQ - a / 3; + } else { + *x0 = -sqrtQ - a / 3; + *x1 = -sqrtQ - a / 3; + *x2 = 2 * sqrtQ - a / 3; + } + return 3; + + } else if (CR2 < CQ3) { + // This case is equivalent to R2 < Q3. + Real sqrtQ = sqrt(Q); + Real sqrtQ3 = sqrtQ * sqrtQ * sqrtQ; + Real theta = acos(R / sqrtQ3); + Real norm = -2 * sqrtQ; + *x0 = norm * cos(theta / 3) - a / 3; + *x1 = norm * cos((theta + 2.0 * M_PI) / 3) - a / 3; + *x2 = norm * cos((theta - 2.0 * M_PI) / 3) - a / 3; + + // Put the roots in ascending order. + if (*x0 > *x1) { + std::swap(*x0, *x1); + } + if (*x1 > *x2) { + std::swap(*x1, *x2); + if (*x0 > *x1) { + std::swap(*x0, *x1); + } + } + return 3; + } + Real sgnR = (R >= 0 ? 1 : -1); + Real A = -sgnR * pow(fabs(R) + sqrt(R2 - Q3), 1.0/3.0); + Real B = Q / A; + *x0 = A + B - a / 3; + return 1; +} + +// The coefficients are in ascending powers, i.e. coeffs[N]*x^N. +template<typename Real> +int SolveCubicPolynomial(const Real *coeffs, Real *solutions) { + if (coeffs[0] == 0.0) { + // TODO(keir): This is a quadratic not a cubic. Implement a quadratic + // solver! + return 0; + } + Real a = coeffs[2] / coeffs[3]; + Real b = coeffs[1] / coeffs[3]; + Real c = coeffs[0] / coeffs[3]; + return SolveCubicPolynomial(a, b, c, + solutions + 0, + solutions + 1, + solutions + 2); +} +} // namespace libmv +#endif // LIBMV_NUMERIC_POLY_H_ |