Danny Robson
fdaa5e1392
LIMIT hid an off-by-one bug when tests used end iterators. We rename the assertion to uncover all uses of the flawed implementation, and split it into an identical assertion, and one intended to protect against iterator ends.
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 (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))) };
|
|
}
|
|
}
|