nerdcruft/libcruft-util
1
0
Fork 0
Code Issues Releases Activity

bezier: add signed distance queries

Browse Source
This commit is contained in:
Danny Robson 2016-12-21 20:20:56 +11:00
parent 0da8c189ec
commit 4f795c9e40
6 changed files with 619 additions and 300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile.am
View File

@ -37,6 +37,9 @@ UTIL_FILES = \
ascii.hpp \
backtrace.hpp \
bezier.cpp \
bezier1.cpp \
bezier2.cpp \
bezier3.cpp \
bezier.hpp \
bitwise.cpp \
bitwise.hpp \

324
bezier.cpp
View File

@ -25,150 +25,11 @@
#include <iterator>
//-----------------------------------------------------------------------------
template <size_t S>
util::bezier<S>::bezier (const util::point2f (&_points)[S+1])
///////////////////////////////////////////////////////////////////////////////
template <size_t N>
util::bezier<N>::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<util::vector2f,4>
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<util::vector2f,3>
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<util::vector2f,2>
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 <size_t S>
template <size_t N>
size_t
util::bezier<S>::intersections (point2f p0, point2f p1) const
util::bezier<N>::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<S>::intersections (point2f p0, point2f p1) const
p0.y * (p1.x - p0.x);
// Build the intersection polynomial
const std::array<vector2f,S+1> bcoeff = coeffs ();
const std::array<vector2f,N+1> bcoeff = coeffs ();
std::array<float,S+1> pcoeff;
std::array<float,N+1> 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<S> (pcoeff);
const auto r = polynomial::roots<N> (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<S>::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<float>::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<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) {
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 <size_t S>
template <size_t N>
util::region2f
util::bezier<S>::region (void) const
util::bezier<N>::region (void) const
{
float x0 = m_points[0].x;
float y0 = m_points[0].y;
@ -357,7 +97,7 @@ util::bezier<S>::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<S>::region (void) const
//-----------------------------------------------------------------------------
template <size_t S>
template <size_t N>
util::point2f&
util::bezier<S>::operator[] (size_t idx)
util::bezier<N>::operator[] (size_t idx)
{
CHECK_LE (idx, S);
CHECK_LE (idx, N);
return m_points[idx];
}
//-----------------------------------------------------------------------------
template <size_t S>
template <size_t N>
const util::point2f&
util::bezier<S>::operator[] (size_t idx) const
util::bezier<N>::operator[] (size_t idx) const
{
CHECK_LE (idx, S);
CHECK_LE (idx, N);
return m_points[idx];
}
///////////////////////////////////////////////////////////////////////////////
template <size_t S>
template <size_t N>
const util::point2f*
util::bezier<S>::begin (void) const
util::bezier<N>::begin (void) const
{
return std::cbegin (m_points);
}
//-----------------------------------------------------------------------------
template <size_t S>
template <size_t N>
const util::point2f*
util::bezier<S>::end (void) const
util::bezier<N>::end (void) const
{
return std::cend (m_points);
}
//-----------------------------------------------------------------------------
template <size_t S>
template <size_t N>
const util::point2f*
util::bezier<S>::cbegin (void) const
util::bezier<N>::cbegin (void) const
{
return std::cbegin (m_points);
}
//-----------------------------------------------------------------------------
template <size_t S>
template <size_t N>
const util::point2f*
util::bezier<S>::cend (void) const
util::bezier<N>::cend (void) const
{
return std::cend (m_points);
}
///////////////////////////////////////////////////////////////////////////////
template <size_t S>
template <size_t N>
std::ostream&
util::operator<< (std::ostream &os, const bezier<S> &b)
util::operator<< (std::ostream &os, const bezier<N> &b)
{
using value_type = decltype(*b.cbegin());
@ -449,9 +189,9 @@ util::operator<< (std::ostream &os, const bezier<S> &b)
///////////////////////////////////////////////////////////////////////////////
#define INSTANTIATE(S) \
template class util::bezier<S>; \
template std::ostream& util::operator<< (std::ostream&, const bezier<S>&);
#define INSTANTIATE(N) \
template class util::bezier<N>; \
template std::ostream& util::operator<< (std::ostream&, const bezier<N>&);
INSTANTIATE(1)
INSTANTIATE(2)

35
bezier.hpp
View File

@ -23,22 +23,41 @@
#include <ostream>
namespace util {
template <size_t S>
struct sdot_t {
float distance;
float dot;
};
template <size_t N>
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<vector2f,S+1>
std::array<vector2f,N+1>
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 <size_t S>
std::ostream& operator<< (std::ostream&, const bezier<S>&);
template <size_t N>
std::ostream& operator<< (std::ostream&, const bezier<N>&);
}
#endif

131
bezier1.cpp Normal file
View File

@ -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 <danny@nerdcruft.net>
*/
#include "./bezier.hpp"
#include <iostream>
///////////////////////////////////////////////////////////////////////////////
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<util::vector2f,2>
bezier<1>::coeffs (void) const
{
auto &v = m_coeffs;
return {
-1.f * v[1] + 1.f * v[0],
+1.f * v[1],
};
}
}

187
bezier2.cpp Normal file
View File

@ -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 <danny@nerdcruft.net>
*/
#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<util::point> ();
}
}
//-----------------------------------------------------------------------------
namespace util {
template <>
std::array<util::vector2f,3>
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<float>::infinity ();
float t = std::numeric_limits<float>::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<float,4>
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);
}
}

239
bezier3.cpp Normal file
View File

@ -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 <danny@nerdcruft.net>
*/
#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<util::vector2f,4>
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<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) {
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<float>::infinity ();
float t = std::numeric_limits<float>::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))) };
}
}