234 lines
6.5 KiB
C++
234 lines
6.5 KiB
C++
/*
|
|
* This Source Code Form is subject to the terms of the Mozilla Public
|
|
* License, v. 2.0. If a copy of the MPL was not distributed with this
|
|
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
|
*
|
|
* Copyright 2015-2016 Danny Robson <danny@nerdcruft.net>
|
|
*/
|
|
|
|
#include "bezier.hpp"
|
|
|
|
#include "coord/iostream.hpp"
|
|
|
|
#include <array>
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
namespace cruft {
|
|
template <>
|
|
point2f
|
|
bezier<3>::eval (float t) const
|
|
{
|
|
CHECK_GE (t, 0.f);
|
|
CHECK_LE (t, 1.f);
|
|
|
|
auto v0 = pow (1 - t, 3u) * m_points[0];
|
|
auto v1 = 3 * pow (1 - t, 2u) * t * m_points[1];
|
|
auto v2 = 3 * pow (1 - t, 2u) * t * m_points[2];
|
|
auto v3 = pow (t, 3u) * m_points[3];
|
|
|
|
return {
|
|
v0.x + v1.x + v2.x + v3.x,
|
|
v0.y + v1.y + v2.y + v3.y
|
|
};
|
|
}
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
namespace cruft {
|
|
template <>
|
|
std::array<cruft::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 (cruft::bezier<3> b,
|
|
cruft::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);
|
|
|
|
cruft::point2f p_l = b.eval (t_l);
|
|
cruft::point2f p_r = b.eval (t_r);
|
|
|
|
float d_l = cruft::distance (target, p_l);
|
|
float d_r = cruft::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 cruft {
|
|
// TODO: use a more reliable method like [Xiao-Dia Chen 2010]
|
|
template <>
|
|
float
|
|
bezier<3>::distance (cruft::point2f target) const noexcept
|
|
{
|
|
static constexpr int SUBDIV = 32;
|
|
std::array<cruft::point2f, SUBDIV> lookup;
|
|
|
|
for (int i = 0; i < SUBDIV; ++i)
|
|
lookup[i] = eval (float (i) / (SUBDIV - 1.f));
|
|
|
|
size_t best = 0;
|
|
for (size_t i = 1; i < lookup.size (); ++i) {
|
|
auto d_i = cruft::distance2 (target, lookup[i]);
|
|
auto d_b = cruft::distance2 (target, lookup[best]);
|
|
|
|
if (d_i < d_b)
|
|
best = i;
|
|
}
|
|
|
|
return refine_cubic (*this,
|
|
target,
|
|
best / float (SUBDIV - 1),
|
|
cruft::distance (target, lookup[best]),
|
|
1.f / SUBDIV);
|
|
}
|
|
}
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
namespace cruft {
|
|
template <>
|
|
cruft::vector2f
|
|
bezier<3>::tangent (const float t) const
|
|
{
|
|
CHECK_INCLUSIVE (t, 0.f, 1.f);
|
|
|
|
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 cruft {
|
|
template <>
|
|
cruft::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 cruft {
|
|
template <>
|
|
cruft::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 cruft {
|
|
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 = cruft::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 = cruft::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))) };
|
|
}
|
|
}
|