From 436ae3db54aa93f9b3c0da5114ecfd412b440dd1 Mon Sep 17 00:00:00 2001
From: Danny Robson <danny@nerdcruft.net>
Date: Thu, 22 Jan 2015 14:56:05 +1100
Subject: [PATCH] bezier: add distance to point

---
 bezier.cpp | 151 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 151 insertions(+)

diff --git a/bezier.cpp b/bezier.cpp
index 1669dc2f..a367171f 100644
--- a/bezier.cpp
+++ b/bezier.cpp
@@ -20,6 +20,7 @@
 #include "bezier.hpp"
 
 #include "debug.hpp"
+#include "polynomial.hpp"
 
 #include <algorithm>
 #include <iterator>
@@ -96,6 +97,156 @@ namespace util {
 }
 
 
+//-----------------------------------------------------------------------------
+namespace util {
+    template <>
+    float
+    bezier<1>::distance (util::point2f target) const
+    {
+        auto v = m_points[1] - m_points[0];
+        auto w = target - m_points[0];
+
+        auto c1 = dot (w, v);
+        if (c1 <= 0)
+            return m_points[0].distance (target);
+
+        auto c2 = dot (v, v);
+        if (c2 <= c1)
+            return m_points[1].distance (target);
+
+        auto b = c1 / c2;
+        auto p = m_points[0] + b * v;
+
+        return p.distance (target);
+    }
+}
+
+
+//-----------------------------------------------------------------------------
+namespace util {
+    // TODO: use a more reliable method like [Xiao-Dia Chen 2010]
+    template <>
+    float
+    bezier<2>::distance (util::point2f target) const
+    {
+        // Using procedure from: http://blog.gludion.com/2009/08/distance-to-quadratic-bezier-curve.html
+        auto p0 = m_points[0];
+        auto p1 = m_points[1];
+        auto p2 = m_points[2];
+
+        // Parametric form: P(t) = (1-t)^2*P0 + 2t(1-t)P1 + t^2*P2
+        //
+        // Derivative: dP/dt = -2(1-t)P0 + 2(1-2t)P1 + 2tP2
+        //                   = 2(A+Bt), A=(P1-P0), B=(P2-P1-A)
+        //
+        auto A = p1 - p0;
+        auto B = p2 - p1 - A;
+
+        // Make: dot(target, dP/dt) == 0
+        //       dot (M - P(t), A+Bt) == 0
+        //
+        // Solve: at^3 + bt^2 + ct + d,
+        //        a = B^2,
+        //        b = 3A.B,
+        //        c = 2A^2+M'.B,
+        //        d = M'.A,
+        //        M' = P0-M
+
+        const auto M  = target;
+        const auto M_ = p0 - M;
+
+        //float a = dot (B, B);
+        //float b = 3.f * dot (A, B);
+        //float c = 2.f * dot (A, A) + dot (M_, B);
+        //float d = dot (M_, A);
+
+        const util::vector2f p102 = {
+            2 * p1.x - p0.x - p2.x,
+            2 * p1.y - p0.y - p2.y
+        };
+
+        const float a = dot (B, 2.f * p102);
+        const float b = dot (B, 4.f * (p0 - p1)) + dot (A, 2.f * p102);
+        const float c = dot (B, 2.f * (M - p0)) + dot (A, 4.f * (p0 - p1));
+        const float d = dot (A, 2.f * (M - p0));
+
+        auto solutions = util::polynomial::solve<3> ({a, b, c, d});
+
+        float dist = std::numeric_limits<float>::infinity ();
+
+        for (auto t: solutions) {
+            if (std::isnan (t))
+                continue;
+
+            if (t <= 0)
+                dist = min (dist, p0.distance (target));
+            else if (t > 1)
+                dist = min (p2.distance (target));
+            else {
+                auto p = eval (t);
+                dist = min (dist, p.distance (target));
+            }
+        }
+
+        return dist;
+    }
+}
+
+
+//-----------------------------------------------------------------------------
+float refine_cubic (util::bezier<3> b,
+                    util::point2f target,
+                    float t,
+                    float d,
+                    float p)
+{
+    // TODO: use an iteration of newton before handing back
+    if (p < 0.00001) {
+        return t;
+    }
+
+    float t_l = std::max (0.f, t - p);
+    float t_r = std::min (1.f, t + p);
+
+    util::point2f p_l = b.eval (t_l);
+    util::point2f p_r = b.eval (t_r);
+
+    float d_l = p_l.distance (target);
+    float d_r = p_r.distance (target);
+
+    if (d_l < d) { return refine_cubic (b, target, t_l, d_l, p); }
+    if (d_r < d) { return refine_cubic (b, target, t_r, d_r, p); }
+
+    return refine_cubic (b, target, t, d, p / 2);
+}
+
+
+//-----------------------------------------------------------------------------
+namespace util {
+    template <>
+    float
+    bezier<3>::distance (util::point2f target) const
+    {
+        static constexpr size_t SUBDIV = 32;
+        std::array<util::point2f, SUBDIV> lookup;
+
+        for (size_t i = 0; i < SUBDIV; ++i)
+            lookup[i] = eval (i / float (SUBDIV - 1));
+
+        size_t best = 0;
+        for (size_t i = 1; i < lookup.size (); ++i)
+            if (lookup[i].distance2 (target) < lookup[best].distance2 (target))
+                best = i;
+
+        return refine_cubic (*this,
+                               target,
+                               best / float (SUBDIV - 1),
+                               lookup[best].distance (target),
+                               1.f / SUBDIV);
+    }
+}
+
+
 //-----------------------------------------------------------------------------
 template <size_t S>
 util::point2f&