polynomial: initial degree 1-3 solvers

Makefile.am

@@ -114,6 +114,8 @@ UTIL_FILES   =		\
     point.cpp		\
     point.hpp		\
     point.ipp		\
+    polynomial.cpp	\
+    polynomial.hpp	\
     pool.cpp		\
     pool.hpp		\
     pool.ipp		\

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 <http://www.gnu.org/licenses/>.
+ *
+ * Copyright 2015 Danny Robson <danny@nerdcruft.net>
+ */
+
+
+#include "polynomial.hpp"
+
+#include "maths.hpp"
+
+#include <limits>
+#include <cmath>
+
+//-----------------------------------------------------------------------------
+namespace util { namespace polynomial {
+    template <>
+    std::array<float,1>
+    solve (std::array<float,2> coeff)
+    {
+        const float a = coeff[0];
+        const float b = coeff[1];
+
+        if (almost_zero (a))
+            return { std::numeric_limits<float>::quiet_NaN () };
+
+        return { -b / a };
+    }
+} }
+
+//-----------------------------------------------------------------------------
+namespace util { namespace polynomial {
+    template <>
+    std::array<float,2>
+    solve (std::array<float,3> 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<float>::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<float,3>
+    solve (std::array<float,4> 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<float>::quiet_NaN () };
+        }
+
+        std::array<float,3> s = {
+            std::numeric_limits<float>::quiet_NaN (),
+            std::numeric_limits<float>::quiet_NaN (),
+            std::numeric_limits<float>::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;
+    }
+} }

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 <http://www.gnu.org/licenses/>.
+ *
+ * Copyright 2015 Danny Robson <danny@nerdcruft.net>
+ */
+
+#ifndef __UTIL_POLYNOMIAL_HPP
+#define __UTIL_POLYNOMIAL_HPP
+
+#include <array>
+#include <cstdlib>
+
+namespace util {
+    namespace polynomial {
+        template <size_t S>
+        std::array<float,S-1>
+        solve (std::array<float,S>);
+
+        template <size_t S>
+        float
+        eval (std::array<float,S>);
+    }
+}
+
+#endif

test/Makefile.am

@@ -18,6 +18,7 @@ TEST_BIN = 		\
 	md5		\
 	options		\
 	point		\
+	polynomial	\
 	pool		\
 	rand		\
 	range 		\

test/polynomial.cpp

@@ -0,0 +1,44 @@
+#include "polynomial.hpp"
+
+#include "float.hpp"
+#include "debug.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+int
+main (int, char**)
+{
+    auto nan = std::numeric_limits<float>::quiet_NaN ();
+
+    struct {
+        std::array<float,4> coeffs;
+        std::array<float,3> 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);
+        }
+    }
+}