From bd12519d9463c337cebd38e23f56d342d0c2445f Mon Sep 17 00:00:00 2001
From: Danny Robson <danny@nerdcruft.net>
Date: Thu, 22 Jan 2015 00:27:46 +1100
Subject: [PATCH] polynomial: use newtons method after cubic solve

---
 polynomial.cpp      | 38 ++++++++++++++++++++++++++++++++++++++
 polynomial.hpp      |  2 +-
 test/polynomial.cpp |  4 +---
 3 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/polynomial.cpp b/polynomial.cpp
index a556a427..600e97e5 100644
--- a/polynomial.cpp
+++ b/polynomial.cpp
@@ -126,6 +126,44 @@ namespace util { namespace polynomial {
         float sub = a / 3.f;
         for (auto &i: s)
             i -= sub;
+
+        // Run an iteration of Newtons method to make the results slightly
+        // more accurate, they're a little loose straight out of the bat.
+        float da = 3;
+        float db = 2 * a;
+        float dc = b;
+
+        for (auto &i: s) {
+            float deriv = da * i * i + db * i + dc;
+            if (almost_zero (deriv))
+                continue;
+
+            i = i - eval (coeffs, i) / deriv;
+        }
+
         return s;
     }
 } }
+
+
+//-----------------------------------------------------------------------------
+template <size_t S>
+float
+util::polynomial::eval (const std::array<float,S> coeffs, const float x)
+{
+    float x_ = 1.f;
+    float sum = 0.f;
+    for (size_t i = 0; i < S; ++i) {
+        sum += coeffs[S-i-1] * x_;
+        x_ *= x;
+    }
+
+    return sum;
+}
+
+
+//-----------------------------------------------------------------------------
+template float util::polynomial::eval (std::array<float,1>, float);
+template float util::polynomial::eval (std::array<float,2>, float);
+template float util::polynomial::eval (std::array<float,3>, float);
+template float util::polynomial::eval (std::array<float,4>, float);
diff --git a/polynomial.hpp b/polynomial.hpp
index b0cbad69..3144e2f9 100644
--- a/polynomial.hpp
+++ b/polynomial.hpp
@@ -31,7 +31,7 @@ namespace util {
 
         template <size_t S>
         float
-        eval (std::array<float,S>);
+        eval (std::array<float,S>, float x);
     }
 }
 
diff --git a/test/polynomial.cpp b/test/polynomial.cpp
index 780252fd..f9f191d5 100644
--- a/test/polynomial.cpp
+++ b/test/polynomial.cpp
@@ -33,9 +33,7 @@ main (int, char**)
         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 equal = ieee_single::almost_equal (i.solutions[j], s[j]);
             bool invalid = std::isnan (i.solutions[j]) && std::isnan (s[j]);
 
             CHECK (equal || invalid);