/* * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com * * Permission to use, copy, modify, distribute and sell this software * and its documentation for any purpose is hereby granted without fee, * provided that the above copyright notice appear in all copies. * Erwin Coumans makes no representations about the suitability * of this software for any purpose. * It is provided "as is" without express or implied warranty. */ #include "BU_AlgebraicPolynomialSolver.h" #include #include int BU_AlgebraicPolynomialSolver::Solve2Quadratic(SimdScalar p, SimdScalar q) { SimdScalar basic_h_local; SimdScalar basic_h_local_delta; basic_h_local = p * 0.5f; basic_h_local_delta = basic_h_local * basic_h_local - q; if (basic_h_local_delta > 0.0f) { basic_h_local_delta = sqrtf(basic_h_local_delta); m_roots[0] = - basic_h_local + basic_h_local_delta; m_roots[1] = - basic_h_local - basic_h_local_delta; return 2; } else if (SimdGreaterEqual(basic_h_local_delta, SIMD_EPSILON)) { m_roots[0] = - basic_h_local; return 1; } else { return 0; } } int BU_AlgebraicPolynomialSolver::Solve2QuadraticFull(SimdScalar a,SimdScalar b, SimdScalar c) { SimdScalar radical = b * b - 4.0f * a * c; if(radical >= 0.f) { SimdScalar sqrtRadical = sqrtf(radical); SimdScalar idenom = 1.0f/(2.0f * a); m_roots[0]=(-b + sqrtRadical) * idenom; m_roots[1]=(-b - sqrtRadical) * idenom; return 2; } return 0; } #define cubic_rt(x) \ ((x) > 0.0f ? powf((SimdScalar)(x), 0.333333333333333333333333f) : \ ((x) < 0.0f ? -powf((SimdScalar)-(x), 0.333333333333333333333333f) : 0.0f)) /* */ /* this function solves the following cubic equation: */ /* */ /* 3 2 */ /* lead * x + a * x + b * x + c = 0. */ /* */ /* it returns the number of different roots found, and stores the roots in */ /* roots[0,2]. it returns -1 for a degenerate equation 0 = 0. */ /* */ int BU_AlgebraicPolynomialSolver::Solve3Cubic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c) { SimdScalar p, q, r; SimdScalar delta, u, phi; SimdScalar dummy; if (lead != 1.0) { /* */ /* transform into normal form: x^3 + a x^2 + b x + c = 0 */ /* */ if (SimdEqual(lead, SIMD_EPSILON)) { /* */ /* we have a x^2 + b x + c = 0 */ /* */ if (SimdEqual(a, SIMD_EPSILON)) { /* */ /* we have b x + c = 0 */ /* */ if (SimdEqual(b, SIMD_EPSILON)) { if (SimdEqual(c, SIMD_EPSILON)) { return -1; } else { return 0; } } else { m_roots[0] = -c / b; return 1; } } else { p = c / a; q = b / a; return Solve2QuadraticFull(a,b,c); } } else { a = a / lead; b = b / lead; c = c / lead; } } /* */ /* we substitute x = y - a / 3 in order to eliminate the quadric term. */ /* we get x^3 + p x + q = 0 */ /* */ a /= 3.0f; u = a * a; p = b / 3.0f - u; q = a * (2.0f * u - b) + c; /* */ /* now use Cardano's formula */ /* */ if (SimdEqual(p, SIMD_EPSILON)) { if (SimdEqual(q, SIMD_EPSILON)) { /* */ /* one triple root */ /* */ m_roots[0] = -a; return 1; } else { /* */ /* one real and two complex roots */ /* */ m_roots[0] = cubic_rt(-q) - a; return 1; } } q /= 2.0f; delta = p * p * p + q * q; if (delta > 0.0f) { /* */ /* one real and two complex roots. note that v = -p / u. */ /* */ u = -q + sqrtf(delta); u = cubic_rt(u); m_roots[0] = u - p / u - a; return 1; } else if (delta < 0.0) { /* */ /* Casus irreducibilis: we have three real roots */ /* */ r = sqrtf(-p); p *= -r; r *= 2.0; phi = acosf(-q / p) / 3.0f; dummy = SIMD_2_PI / 3.0f; m_roots[0] = r * cosf(phi) - a; m_roots[1] = r * cosf(phi + dummy) - a; m_roots[2] = r * cosf(phi - dummy) - a; return 3; } else { /* */ /* one single and one SimdScalar root */ /* */ r = cubic_rt(-q); m_roots[0] = 2.0f * r - a; m_roots[1] = -r - a; return 2; } } /* */ /* this function solves the following quartic equation: */ /* */ /* 4 3 2 */ /* lead * x + a * x + b * x + c * x + d = 0. */ /* */ /* it returns the number of different roots found, and stores the roots in */ /* roots[0,3]. it returns -1 for a degenerate equation 0 = 0. */ /* */ int BU_AlgebraicPolynomialSolver::Solve4Quartic(SimdScalar lead, SimdScalar a, SimdScalar b, SimdScalar c, SimdScalar d) { SimdScalar p, q ,r; SimdScalar u, v, w; int i, num_roots, num_tmp; //SimdScalar tmp[2]; if (lead != 1.0) { /* */ /* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */ /* */ if (SimdEqual(lead, SIMD_EPSILON)) { /* */ /* we have a x^3 + b x^2 + c x + d = 0 */ /* */ if (SimdEqual(a, SIMD_EPSILON)) { /* */ /* we have b x^2 + c x + d = 0 */ /* */ if (SimdEqual(b, SIMD_EPSILON)) { /* */ /* we have c x + d = 0 */ /* */ if (SimdEqual(c, SIMD_EPSILON)) { if (SimdEqual(d, SIMD_EPSILON)) { return -1; } else { return 0; } } else { m_roots[0] = -d / c; return 1; } } else { p = c / b; q = d / b; return Solve2QuadraticFull(b,c,d); } } else { return Solve3Cubic(1.0, b / a, c / a, d / a); } } else { a = a / lead; b = b / lead; c = c / lead; d = d / lead; } } /* */ /* we substitute x = y - a / 4 in order to eliminate the cubic term. */ /* we get: y^4 + p y^2 + q y + r = 0. */ /* */ a /= 4.0f; p = b - 6.0f * a * a; q = a * (8.0f * a * a - 2.0f * b) + c; r = a * (a * (b - 3.f * a * a) - c) + d; if (SimdEqual(q, SIMD_EPSILON)) { /* */ /* biquadratic equation: y^4 + p y^2 + r = 0. */ /* */ num_roots = Solve2Quadratic(p, r); if (num_roots > 0) { if (m_roots[0] > 0.0f) { if (num_roots > 1) { if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) { u = sqrtf(m_roots[1]); m_roots[2] = u - a; m_roots[3] = -u - a; u = sqrtf(m_roots[0]); m_roots[0] = u - a; m_roots[1] = -u - a; return 4; } else { u = sqrtf(m_roots[0]); m_roots[0] = u - a; m_roots[1] = -u - a; return 2; } } else { u = sqrtf(m_roots[0]); m_roots[0] = u - a; m_roots[1] = -u - a; return 2; } } } return 0; } else if (SimdEqual(r, SIMD_EPSILON)) { /* */ /* no absolute term: y (y^3 + p y + q) = 0. */ /* */ num_roots = Solve3Cubic(1.0, 0.0, p, q); for (i = 0; i < num_roots; ++i) m_roots[i] -= a; if (num_roots != -1) { m_roots[num_roots] = -a; ++num_roots; } else { m_roots[0] = -a; num_roots = 1;; } return num_roots; } else { /* */ /* we solve the resolvent cubic equation */ /* */ num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q); if (num_roots == -1) { num_roots = 1; m_roots[0] = 0.0f; } /* */ /* build two quadric equations */ /* */ w = m_roots[0]; u = w * w - r; v = 2.0f * w - p; if (SimdEqual(u, SIMD_EPSILON)) u = 0.0; else if (u > 0.0f) u = sqrtf(u); else return 0; if (SimdEqual(v, SIMD_EPSILON)) v = 0.0; else if (v > 0.0f) v = sqrtf(v); else return 0; if (q < 0.0f) v = -v; w -= u; num_roots=Solve2Quadratic(v, w); for (i = 0; i < num_roots; ++i) { m_roots[i] -= a; } w += 2.0f *u; SimdScalar tmp[2]; tmp[0] = m_roots[0]; tmp[1] = m_roots[1]; num_tmp = Solve2Quadratic(-v, w); for (i = 0; i < num_tmp; ++i) { m_roots[i + num_roots] = tmp[i] - a; m_roots[i]=tmp[i]; } return (num_tmp + num_roots); } }