diff --git a/Makefile.am b/Makefile.am index adc14edc..a5a76950 100644 --- a/Makefile.am +++ b/Makefile.am @@ -37,6 +37,9 @@ UTIL_FILES = \ ascii.hpp \ backtrace.hpp \ bezier.cpp \ + bezier1.cpp \ + bezier2.cpp \ + bezier3.cpp \ bezier.hpp \ bitwise.cpp \ bitwise.hpp \ diff --git a/bezier.cpp b/bezier.cpp index 215a94b4..0966a416 100644 --- a/bezier.cpp +++ b/bezier.cpp @@ -25,150 +25,11 @@ #include -//----------------------------------------------------------------------------- -template -util::bezier::bezier (const util::point2f (&_points)[S+1]) +/////////////////////////////////////////////////////////////////////////////// +template +util::bezier::bezier (const util::point2f (&_points)[N+1]) { - std::copy (_points, _points + S + 1, m_points); -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - point2f - bezier<1>::eval (float t) const - { - CHECK_GE (t, 0); - CHECK_LE (t, 1); - - auto v0 = (1 - t) * m_points[0]; - auto v1 = t * m_points[1]; - - return { - v0.x + v1.x, - v0.y + v1.y - }; - } -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - point2f - bezier<2>::eval (float t) const - { - CHECK_GE (t, 0); - CHECK_LE (t, 1); - - auto v0 = pow2 (1 - t) * m_points[0]; - auto v1 = 2 * (1 - t) * t * m_points[1]; - auto v2 = pow2 (t) * m_points[2]; - - return { - v0.x + v1.x + v2.x, - v0.y + v1.y + v2.y - }; - } -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - point2f - bezier<3>::eval (float t) const - { - CHECK_GE (t, 0); - CHECK_LE (t, 1); - - auto v0 = pow (1 - t, 3) * m_points[0]; - auto v1 = 3 * pow2 (1 - t) * t * m_points[1]; - auto v2 = 3 * pow2 (1 - t) * t * m_points[2]; - auto v3 = pow (t, 3) * m_points[3]; - - return { - v0.x + v1.x + v2.x + v3.x, - v0.y + v1.y + v2.y + v3.y - }; - } -} - - -//----------------------------------------------------------------------------- -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 util::distance (target, m_points[0]); - - auto c2 = dot (v, v); - if (c2 <= c1) - return util::distance (target, m_points[1]); - - auto b = c1 / c2; - auto p = m_points[0] + b * v; - - return util::distance (target, p); - } -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - std::array - bezier<3>::coeffs (void) const - { - const auto &v = m_coeffs; - - return { - -1.f * v[0] +3.f * v[1] -3.f * v[2] +1.f * v[3], - 3.f * v[0] -6.f * v[1] +3.f * v[2], - -3.f * v[0] +3.f * v[1], - 1.f * v[0] - }; - } -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - std::array - bezier<2>::coeffs (void) const - { - auto &v = m_coeffs; - - return { - +1.f * v[2] -2.f * v[1] + 1.f * v[0], - -2.f * v[2] +2.f * v[1], - +1.f * v[2] - }; - } -} - - -//----------------------------------------------------------------------------- -namespace util { - template <> - std::array - bezier<1>::coeffs (void) const - { - auto &v = m_coeffs; - - return { - -1.f * v[1] + 1.f * v[0], - +1.f * v[1], - }; - } + std::copy (_points, _points + N + 1, m_points); } @@ -176,9 +37,9 @@ namespace util { // XXX: If the line is co-linear we'll have no solutions. But we return 1 // anyway as this function is used to find any point that intersects as part // of other more comprehensive tests. -template +template size_t -util::bezier::intersections (point2f p0, point2f p1) const +util::bezier::intersections (point2f p0, point2f p1) const { float A = p1.y - p0.y; // A = y2 - y1 float B = p0.x - p1.x; // B = x1 - x2 @@ -186,21 +47,21 @@ util::bezier::intersections (point2f p0, point2f p1) const p0.y * (p1.x - p0.x); // Build the intersection polynomial - const std::array bcoeff = coeffs (); + const std::array bcoeff = coeffs (); - std::array pcoeff; + std::array pcoeff; for (size_t i = 0; i < pcoeff.size (); ++i) pcoeff[i] = A * bcoeff[i].x + B * bcoeff[i].y; pcoeff.back () += C; - const auto r = polynomial::roots (pcoeff); + const auto r = polynomial::roots (pcoeff); // The curve and line are colinear if (std::all_of (r.begin (), r.end (), [] (auto i) { return std::isnan (i); })) return 1; size_t count = 0; - for (size_t i = 0; i < S; ++i) { + for (size_t i = 0; i < N; ++i) { // Ensure the solutions are on the curve const auto t = r[i]; if (std::isnan (t)) @@ -226,130 +87,9 @@ util::bezier::intersections (point2f p0, point2f p1) const //----------------------------------------------------------------------------- -namespace util { - 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); - - // We have our cubic, so pass off to the solver - auto solutions = util::polynomial::roots<3> ({a, b, c, d}); - - // Find the smallest distance and return - float dist = std::numeric_limits::infinity (); - - for (auto t: solutions) { - if (std::isnan (t)) - continue; - - if (t <= 0) - dist = min (dist, util::distance (target, p0)); - else if (t > 1) - dist = min (util::distance (target, p2)); - else { - auto p = eval (t); - dist = min (dist, util::distance (target, p)); - } - } - - 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 = util::distance (target, p_l); - float d_r = util::distance (target, p_r); - - 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 { - // TODO: use a more reliable method like [Xiao-Dia Chen 2010] - template <> - float - bezier<3>::distance (util::point2f target) const - { - static constexpr size_t SUBDIV = 32; - std::array 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) { - auto d_i = util::distance2 (target, lookup[i]); - auto d_b = util::distance2 (target, lookup[best]); - - if (d_i < d_b) - best = i; - } - - return refine_cubic (*this, - target, - best / float (SUBDIV - 1), - util::distance (target, lookup[best]), - 1.f / SUBDIV); - } -} - - -//----------------------------------------------------------------------------- -template +template util::region2f -util::bezier::region (void) const +util::bezier::region (void) const { float x0 = m_points[0].x; float y0 = m_points[0].y; @@ -357,7 +97,7 @@ util::bezier::region (void) const float x1 = x0; float y1 = y0; - for (size_t i = 1; i < S+1; ++i) { + for (size_t i = 1; i < N+1; ++i) { x0 = min (x0, m_points[i].x); y0 = min (y0, m_points[i].y); @@ -373,67 +113,67 @@ util::bezier::region (void) const //----------------------------------------------------------------------------- -template +template util::point2f& -util::bezier::operator[] (size_t idx) +util::bezier::operator[] (size_t idx) { - CHECK_LE (idx, S); + CHECK_LE (idx, N); return m_points[idx]; } //----------------------------------------------------------------------------- -template +template const util::point2f& -util::bezier::operator[] (size_t idx) const +util::bezier::operator[] (size_t idx) const { - CHECK_LE (idx, S); + CHECK_LE (idx, N); return m_points[idx]; } /////////////////////////////////////////////////////////////////////////////// -template +template const util::point2f* -util::bezier::begin (void) const +util::bezier::begin (void) const { return std::cbegin (m_points); } //----------------------------------------------------------------------------- -template +template const util::point2f* -util::bezier::end (void) const +util::bezier::end (void) const { return std::cend (m_points); } //----------------------------------------------------------------------------- -template +template const util::point2f* -util::bezier::cbegin (void) const +util::bezier::cbegin (void) const { return std::cbegin (m_points); } //----------------------------------------------------------------------------- -template +template const util::point2f* -util::bezier::cend (void) const +util::bezier::cend (void) const { return std::cend (m_points); } /////////////////////////////////////////////////////////////////////////////// -template +template std::ostream& -util::operator<< (std::ostream &os, const bezier &b) +util::operator<< (std::ostream &os, const bezier &b) { using value_type = decltype(*b.cbegin()); @@ -449,9 +189,9 @@ util::operator<< (std::ostream &os, const bezier &b) /////////////////////////////////////////////////////////////////////////////// -#define INSTANTIATE(S) \ -template class util::bezier; \ -template std::ostream& util::operator<< (std::ostream&, const bezier&); +#define INSTANTIATE(N) \ +template class util::bezier; \ +template std::ostream& util::operator<< (std::ostream&, const bezier&); INSTANTIATE(1) INSTANTIATE(2) diff --git a/bezier.hpp b/bezier.hpp index 2253b04e..53429e09 100644 --- a/bezier.hpp +++ b/bezier.hpp @@ -23,22 +23,41 @@ #include namespace util { - template + struct sdot_t { + float distance; + float dot; + }; + + template class bezier { public: using value_type = point2f::value_type; - bezier (const util::point2f (&)[S+1]); + bezier (const util::point2f (&)[N+1]); point2f eval (float t) const; // Calculate the expanded polynomial coeffecients in terms of t - std::array + std::array coeffs (void) const; size_t intersections (point2f from, point2f to) const; - float distance (point2f) const; + util::vector2f tangent (float t) const; + // 1st derivative w.r.t. t + util::vector2f d1 (float t) const noexcept; + // 2nd derivative w.r.t. t + util::vector2f d2 (float t) const noexcept; + + float closest (point2f) const noexcept; + + sdot_t sdot (point2f) const noexcept; + + float sdistance2 (point2f) const noexcept; + float sdistance (point2f) const noexcept; + + float distance2 (point2f) const noexcept; + float distance (point2f) const noexcept; region2f region (void) const; @@ -54,13 +73,13 @@ namespace util { // HACK: allow easy access to component-wise arithmetic using // vector2f rather than point2f in the implementation. union { - point2f m_points[S+1]; - vector2f m_coeffs[S+1]; + point2f m_points[N+1]; + vector2f m_coeffs[N+1]; }; }; - template - std::ostream& operator<< (std::ostream&, const bezier&); + template + std::ostream& operator<< (std::ostream&, const bezier&); } #endif diff --git a/bezier1.cpp b/bezier1.cpp new file mode 100644 index 00000000..089999b8 --- /dev/null +++ b/bezier1.cpp @@ -0,0 +1,131 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015-2016 Danny Robson + */ + +#include "./bezier.hpp" + +#include + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + point2f + bezier<1>::eval (float t) const + { + CHECK_GE (t, 0); + CHECK_LE (t, 1); + + auto v0 = (1 - t) * m_points[0]; + auto v1 = t * m_points[1]; + + return { + v0.x + v1.x, + v0.y + v1.y + }; + } +} + + +/////////////////////////////////////////////////////////////////////////////// +constexpr +util::vector2f +orthonormal (util::vector2f v) +{ + const auto len = norm (v); + CHECK_NEZ (len); + return util::vector2f { -v.y / len, v.x / len }; +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + float + bezier<1>::closest (util::point2f q) const noexcept + { + const auto ab = m_points[1] - m_points[0]; + const auto aq = q - m_points[0]; + + return dot (aq, ab) / dot (ab, ab); + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + float + bezier<1>::distance (util::point2f q) const noexcept + { + const auto ab = m_points[1] - m_points[0]; + const auto t = limit (closest (q), 0, 1); + const auto p = m_points[0] + t * ab; + + return util::distance (q, p); + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + sdot_t + bezier<1>::sdot (const point2f q) const noexcept + { + // find the closest parameter 't' to the point 'q' for the parametric line + const auto qa = m_points[0] - q; + const auto ab = m_points[1] - m_points[0]; + const auto t = closest (q); + + // find the vector to, and distance to, the nearest endpoint 'e' + const auto qe = m_points[t > 0.5] - q; + const auto d_e = norm (qe); + + // if we're on the segment return the distance to the segment + if (t >= 0 && t <= 1) { + const auto ortho = util::vector2f { -ab.y, ab.x } / norm (ab); + const auto d = dot (ortho, qa); + + // not _entirely_ sure why we need this condition + if (abs (d) <= d_e) { + return { d, 0 }; + } + } + + // return the distance and angle to the endpoint + return { + sign (cross (ab, qa)) * d_e, + abs ( + dot (normalised (ab), normalised (qe)) + ) + }; + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + std::array + bezier<1>::coeffs (void) const + { + auto &v = m_coeffs; + + return { + -1.f * v[1] + 1.f * v[0], + +1.f * v[1], + }; + } +} diff --git a/bezier2.cpp b/bezier2.cpp new file mode 100644 index 00000000..cb252207 --- /dev/null +++ b/bezier2.cpp @@ -0,0 +1,187 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015-2016 Danny Robson + */ + +#include "./bezier.hpp" + +#include "./polynomial.hpp" +#include "./coord/iostream.hpp" + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + point2f + bezier<2>::eval (float t) const + { + CHECK_LIMIT (t, 0, 1); + + const auto &P0 = m_coeffs[0]; + const auto &P1 = m_coeffs[1]; + const auto &P2 = m_coeffs[2]; + + return ( + (1 - t) * (1 - t) * P0 + + 2 * (1 - t) * t * P1 + + t * t * P2 + ).as (); + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + std::array + bezier<2>::coeffs (void) const + { + auto &v = m_coeffs; + + return { + +1.f * v[2] -2.f * v[1] + 1.f * v[0], + -2.f * v[2] +2.f * v[1], + +1.f * v[2] + }; + } +} + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + util::vector2f + bezier<2>::d1 (const float t) const noexcept + { + CHECK_LIMIT (t, 0, 1); + + const auto &P0 = m_coeffs[0]; + const auto &P1 = m_coeffs[1]; + const auto &P2 = m_coeffs[2]; + + return 2 * (1 - t) * (P1 - P0) + + 2 * t * (P2 - P1); + } +} + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + sdot_t + bezier<2>::sdot (point2f q) const noexcept + { + // setup inter-point vectors + const auto ab = m_points[1] - m_points[0]; + const auto bc = m_points[2] - m_points[1]; + const auto qa = m_points[0] - q; + const auto qb = m_points[1] - q; + const auto qc = m_points[2] - q; + + // setup variables we want to minimise + float d = std::numeric_limits::infinity (); + float t = std::numeric_limits::quiet_NaN (); + + // distance from A + const auto d_a = sign (cross (ab, qa)) * norm2 (qa); + if (abs (d_a) < abs (d)) { + d = d_a; + t = -dot (ab, qa) / norm2 (ab); + } + + // distance from B + const auto d_b = sign (cross (bc, qc)) * norm2 (qc); + if (abs (d_b) < abs (d)) { + d = d_b; + t = -dot (bc, qb) / norm2 (bc); + } + + // Using procedure from: http://blog.gludion.com/2009/08/distance-to-quadratic-bezier-curve.html + // + // 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) + // + const auto &p0 = m_points[0]; + const auto &p1 = m_points[1]; + const auto &p2 = m_points[2]; + + const auto A = p1 - p0; + const auto B = p2 - p1 - A; + + // Make: dot (q, 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 = q; + const auto M_ = p0 - M; + + const std::array + poly = { + dot (B, B), + 3 * dot (A, B), + 2 * dot (A, A) + dot (M_, B), + dot (M_, A), + }; + + // test at polynomial minima + for (const auto r: polynomial::roots<3> (poly)) { + // bail if we have fewer roots than expected + if (std::isnan (r)) + break; + + // ignore if this root is off the curve + if (r < 0 || r > 1) + continue; + + const auto qe = eval (r) - q; + + const auto d_e = sign (cross (ab, qe)) * norm2 (qe); + if (abs (d_e) <= abs (d)) { + d = d_e; + t = r; + } + } + + // calculate the angles from the point to the endpoints if needed + d = sign (d) * std::sqrt (abs (d)); + + if (t >= 0 && t <= 1) + return { d, 0 }; + if (t < 0) { + return { d, abs (dot (normalised (ab), normalised (qa))) }; + } else + return { d, abs (dot (normalised (bc), normalised (qc))) }; + } +} + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + float + bezier<2>::distance (util::point2f q) const noexcept + { + return abs (sdot (q).distance); + } +} + + diff --git a/bezier3.cpp b/bezier3.cpp new file mode 100644 index 00000000..13555c2c --- /dev/null +++ b/bezier3.cpp @@ -0,0 +1,239 @@ +/* + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Copyright 2015-2016 Danny Robson + */ + +#include "./bezier.hpp" + +#include "coord/iostream.hpp" + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + point2f + bezier<3>::eval (float t) const + { + CHECK_GE (t, 0); + CHECK_LE (t, 1); + + auto v0 = pow (1 - t, 3) * m_points[0]; + auto v1 = 3 * pow2 (1 - t) * t * m_points[1]; + auto v2 = 3 * pow2 (1 - t) * t * m_points[2]; + auto v3 = pow (t, 3) * m_points[3]; + + return { + v0.x + v1.x + v2.x + v3.x, + v0.y + v1.y + v2.y + v3.y + }; + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + std::array + bezier<3>::coeffs (void) const + { + const auto &v = m_coeffs; + + return { + -1.f * v[0] +3.f * v[1] -3.f * v[2] +1.f * v[3], + 3.f * v[0] -6.f * v[1] +3.f * v[2], + -3.f * v[0] +3.f * v[1], + 1.f * v[0] + }; + } +} + + +//----------------------------------------------------------------------------- +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 = util::distance (target, p_l); + float d_r = util::distance (target, p_r); + + 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 { + // TODO: use a more reliable method like [Xiao-Dia Chen 2010] + template <> + float + bezier<3>::distance (util::point2f target) const noexcept + { + static constexpr size_t SUBDIV = 32; + std::array 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) { + auto d_i = util::distance2 (target, lookup[i]); + auto d_b = util::distance2 (target, lookup[best]); + + if (d_i < d_b) + best = i; + } + + return refine_cubic (*this, + target, + best / float (SUBDIV - 1), + util::distance (target, lookup[best]), + 1.f / SUBDIV); + } +} + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + util::vector2f + bezier<3>::tangent (const float t) const + { + CHECK_LIMIT (t, 0, 1); + + return mix ( + mix (m_coeffs[1] - m_coeffs[0], m_coeffs[2] - m_coeffs[1], t), + mix (m_coeffs[2] - m_coeffs[1], m_coeffs[3] - m_coeffs[2], t), + t + ); + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + util::vector2f + bezier<3>::d1 (const float t) const noexcept + { + const auto &P0 = m_points[0]; + const auto &P1 = m_points[1]; + const auto &P2 = m_points[2]; + const auto &P3 = m_points[3]; + + return 3 * (1 - t) * (1 - t) * (P1 - P0) + + 6 * (1 - t) * t * (P2 - P1) + + 3 * t * t * (P3 - P2); + } +} + + +//----------------------------------------------------------------------------- +namespace util { + template <> + util::vector2f + bezier<3>::d2 (const float t) const noexcept + { + const auto &P0 = m_points[0]; + const auto &P1 = m_points[1]; + const auto &P2 = m_points[2]; + const auto &P3 = m_points[3]; + + return 6 * (1 - t) * (P2 - P1 + P0 - P1) + + 6 * t * (P3 - P2 + P1 - P2); + } +} + + +/////////////////////////////////////////////////////////////////////////////// +namespace util { + template <> + sdot_t + bezier<3>::sdot (point2f src) const noexcept + { + const auto ab = m_points[1] - m_points[0]; + const auto cd = m_points[3] - m_points[2]; + const auto qa = m_points[0] - src; + const auto qd = m_points[3] - src; + + // setup variables for minimisation + float d = std::numeric_limits::infinity (); + float t = std::numeric_limits::quiet_NaN (); + + // distance from A + const auto d_a = util::sign (cross (ab, qa)) * norm (qa); + if (abs (d_a) < abs (d)) { + d = d_a; + t = -dot (ab, qa) / norm2 (ab); + } + + // distance from D + const auto d_d = util::sign (cross (cd, qd)) * norm (qd); + if (abs (d_d) < abs (d)) { + d = d_d; + t = -dot (cd, qd) / norm2 (cd); + } + + // Iterative minimum distance search + static constexpr int SUBDIVISIONS = 4; + static constexpr int REFINEMENTS = 4; + + for (int i = 0; i <= SUBDIVISIONS; ++i) { + auto r = float (i) / SUBDIVISIONS; + + for (int step = 0; ; ++step) { + const auto qp = eval (r) - src; + + const auto d_p = sign (cross (tangent (r), qp)) * norm (qp); + if (abs (d_p) < abs (d)) { + d = d_p; + t = r; + } + + if (step == REFINEMENTS) + break; + + // converge a little using newton's method + const auto d1_ = d1 (r); + const auto d2_ = d2 (r); + r -= dot (qp, d1_) / (dot (d1_, d1_) + dot (qp, d2_)); + + // bail if it looks like we're going to escape the curve + if (r < 0 || r > 1) + break; + } + } + + if (t >= 0 && t <= 1) + return { d, 0 }; + if (t < 0) + return { d, abs (dot (normalised (ab), normalised (qa))) }; + else + return { d, abs (dot (normalised (cd), normalised (qd))) }; + } +}