libcruft-util/bezier2.cpp
Danny Robson f6056153e3 rename root namespace from util to cruft
This places, at long last, the core library code into the same namespace
as the extended library code.
2018-08-05 14:42:02 +10:00

180 lines
4.7 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 "polynomial.hpp"
#include "coord/iostream.hpp"
///////////////////////////////////////////////////////////////////////////////
namespace cruft {
template <>
point2f
bezier<2>::eval (float t) const
{
CHECK_LIMIT (t, 0.f, 1.f);
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<cruft::point> ();
}
}
//-----------------------------------------------------------------------------
namespace cruft {
template <>
std::array<cruft::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 cruft {
template <>
cruft::vector2f
bezier<2>::d1 (const float t) const noexcept
{
CHECK_LIMIT (t, 0.f, 1.f);
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 cruft {
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 cruft {
template <>
float
bezier<2>::distance (cruft::point2f q) const noexcept
{
return abs (sdot (q).distance);
}
}