diff --git a/Makefile.am b/Makefile.am index f067e47a..a761b37f 100644 --- a/Makefile.am +++ b/Makefile.am @@ -114,6 +114,8 @@ UTIL_FILES = \ point.cpp \ point.hpp \ point.ipp \ + polynomial.cpp \ + polynomial.hpp \ pool.cpp \ pool.hpp \ pool.ipp \ diff --git a/polynomial.cpp b/polynomial.cpp new file mode 100644 index 00000000..7b1231d0 --- /dev/null +++ b/polynomial.cpp @@ -0,0 +1,131 @@ +/* + * This file is part of libgim. + * + * libgim is free software: you can redistribute it and/or modify it under the + * terms of the GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) any later + * version. + * + * libgim 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 General Public License for more + * details. + * + * You should have received a copy of the GNU General Public License + * along with libgim. If not, see . + * + * Copyright 2015 Danny Robson + */ + + +#include "polynomial.hpp" + +#include "maths.hpp" + +#include +#include + +//----------------------------------------------------------------------------- +namespace util { namespace polynomial { + template <> + std::array + solve (std::array coeff) + { + const float a = coeff[0]; + const float b = coeff[1]; + + if (almost_zero (a)) + return { std::numeric_limits::quiet_NaN () }; + + return { -b / a }; + } +} } + +//----------------------------------------------------------------------------- +namespace util { namespace polynomial { + template <> + std::array + solve (std::array coeff) + { + const float a = coeff[0]; + const float b = coeff[1]; + const float c = coeff[2]; + + if (almost_zero (a)) { + auto s = solve<2> ({b, c}); + return { s[0], std::numeric_limits::quiet_NaN () }; + } + + auto d = std::sqrt (pow2 (b) - 4 * a * c); + return { (-b - d) / (2 * a), + (-b + d) / (2 * a) }; + } +} } + + +//----------------------------------------------------------------------------- +// From graphics gems: http://tog.acm.org/resources/GraphicsGems/gemsiv/vec_mat/ray/solver.c +namespace util { namespace polynomial { + template <> + std::array + solve (std::array coeff) + { + const float _a = coeff[0]; + const float _b = coeff[1]; + const float _c = coeff[2]; + const float _d = coeff[3]; + + if (almost_zero (_a)) { + auto s = solve<3> ({_b, _c, _d}); + return {s[0], s[1], std::numeric_limits::quiet_NaN () }; + } + + std::array s = { + std::numeric_limits::quiet_NaN (), + std::numeric_limits::quiet_NaN (), + std::numeric_limits::quiet_NaN () + }; + + // Normalise to x^3 + ax^2 + bx + c = 0 + const float a = _b / _a; + const float b = _c / _a; + const float c = _d / _a; + + // Substituate x = y - a / 3 to eliminate the quadric. Now: x^3 + px + q = 0 + float p = (-1 / 3.f * a * a + b) / 3.f; + float q = (2 / 27.f * a * a * a - 1 / 3.f * a * b + c) / 2.f; + + // Solve using Cardano's method + float D = q * q + p * p * p; + + if (almost_zero (D)) + { + if (almost_zero (q)) { + s[0] = 0.f; + } else { + float u = std::cbrt (-q); + s[0] = 2 * u; + s[1] = -u; + } + } else if (D < 0) { + float phi = 1 / 3.f * std::acos (-q / std::sqrt (-p * p * p)); + float t = 2 * std::sqrt (-p); + + s[0] = t * std::cos (phi); + s[1] = -t * std::cos (phi + PI_f / 3.f); + s[2] = -t * std::cos (phi - PI_f / 3.f); + } else { + float u = std::cbrt (std::sqrt (D) + std::fabs (q)); + if (q > 0.f) + s[0] = -u + p / u; + else + s[0] = u - p / u; + } + + // Resubstitute a / 3 from above + float sub = a / 3.f; + for (auto &i: s) + i -= sub; + return s; + } +} } diff --git a/polynomial.hpp b/polynomial.hpp new file mode 100644 index 00000000..b0cbad69 --- /dev/null +++ b/polynomial.hpp @@ -0,0 +1,38 @@ +/* + * This file is part of libgim. + * + * libgim is free software: you can redistribute it and/or modify it under the + * terms of the GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) any later + * version. + * + * libgim 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 General Public License for more + * details. + * + * You should have received a copy of the GNU General Public License + * along with libgim. If not, see . + * + * Copyright 2015 Danny Robson + */ + +#ifndef __UTIL_POLYNOMIAL_HPP +#define __UTIL_POLYNOMIAL_HPP + +#include +#include + +namespace util { + namespace polynomial { + template + std::array + solve (std::array); + + template + float + eval (std::array); + } +} + +#endif diff --git a/test/Makefile.am b/test/Makefile.am index 04ed9da6..565dafed 100644 --- a/test/Makefile.am +++ b/test/Makefile.am @@ -18,6 +18,7 @@ TEST_BIN = \ md5 \ options \ point \ + polynomial \ pool \ rand \ range \ diff --git a/test/polynomial.cpp b/test/polynomial.cpp new file mode 100644 index 00000000..780252fd --- /dev/null +++ b/test/polynomial.cpp @@ -0,0 +1,44 @@ +#include "polynomial.hpp" + +#include "float.hpp" +#include "debug.hpp" + +#include +#include +#include + +int +main (int, char**) +{ + auto nan = std::numeric_limits::quiet_NaN (); + + struct { + std::array coeffs; + std::array solutions; + } CUBICS [] = { + { { 0, 0, -8, 7 }, { 7.f / 8, nan, nan } }, + { { 0, 4, -8, 2 }, { 0.292893f, 1.70710f, nan } }, + { { 1, 0, 0, 0 }, { 0, nan, nan } }, + { { 1, 0, 2, 0 }, { 0, nan, nan } }, + { { -2, 2, -2, 2 }, { 1, nan, nan } }, + { { 1, 0, 0, 2 }, { -std::cbrt(2.f), nan, nan } }, + { { 2, -3, 8, -2 }, { 0.2728376017309678f, nan, nan } }, + { { 1, 4, -8, 7 }, { -5.6389f, nan, nan } }, + { { 1, 3, -6, -8 }, { -4, -1, 2, } }, + }; + + for (auto &i: CUBICS) { + auto s = util::polynomial::solve (i.coeffs); + + std::sort (s.begin (), s.end ()); + + for (size_t j = 0; j < 3; ++j) { + std::cerr << i.solutions[j] << "==" << s[j] << '\n'; + + bool equal = ieee_single::almost_equal (i.solutions[j], s[j], 864'026'624); + bool invalid = std::isnan (i.solutions[j]) && std::isnan (s[j]); + + CHECK (equal || invalid); + } + } +}