159 lines
3.8 KiB
C++
159 lines
3.8 KiB
C++
#ifndef STAR_SPLINE_HPP
|
|
#define STAR_SPLINE_HPP
|
|
|
|
#include "StarVector.hpp"
|
|
#include "StarInterpolation.hpp"
|
|
#include "StarLogging.hpp"
|
|
#include "StarLruCache.hpp"
|
|
|
|
namespace Star {
|
|
|
|
// Implementation of DeCasteljau Algorithm for Bezier Curves
|
|
template <typename DataT, size_t Dimension, size_t Order, class PointT = Vector<DataT, Dimension>>
|
|
class Spline : public Array<PointT, Order + 1> {
|
|
public:
|
|
typedef Array<PointT, Order + 1> PointData;
|
|
|
|
template <typename... T>
|
|
Spline(PointT const& e1, T const&... rest)
|
|
: PointData(e1, rest...) {
|
|
m_pointCache.setMaxSize(1000);
|
|
m_lengthCache.setMaxSize(1000);
|
|
}
|
|
|
|
Spline() : PointData(PointData::filled(PointT())) {
|
|
m_pointCache.setMaxSize(1000);
|
|
m_lengthCache.setMaxSize(1000);
|
|
}
|
|
|
|
PointT pointAt(float t) const {
|
|
float u = clamp<float>(t, 0, 1);
|
|
if (u != t) {
|
|
t = u;
|
|
Logger::warn("Passed out of range time to Spline::pointAt");
|
|
}
|
|
|
|
if (auto p = m_pointCache.ptr(t))
|
|
return *p;
|
|
|
|
PointData intermediates(*this);
|
|
PointData temp;
|
|
for (size_t order = Order + 1; order > 1; order--) {
|
|
for (size_t i = 1; i < order; i++) {
|
|
temp[i - 1] = lerp(t, intermediates[i - 1], intermediates[i]);
|
|
}
|
|
intermediates = std::move(temp);
|
|
}
|
|
|
|
m_pointCache.set(t, intermediates[0]);
|
|
return intermediates[0];
|
|
}
|
|
|
|
PointT tangentAt(float t) const {
|
|
float u = clamp<float>(t, 0, 1);
|
|
if (u != t) {
|
|
t = u;
|
|
Logger::warn("Passed out of range time to Spline::tangentAt");
|
|
}
|
|
|
|
// constructs a hodograph and returns pointAt
|
|
Spline<DataT, Dimension, Order - 1> hodograph;
|
|
for (size_t i = 0; i < Order; i++) {
|
|
hodograph[i] = ((*this)[i + 1] - (*this)[i]) * Order;
|
|
}
|
|
return hodograph.pointAt(t);
|
|
}
|
|
|
|
DataT length(float begin = 0, float end = 1, size_t subdivisions = 100) const {
|
|
if (!(begin <= 1 && begin >= 0 && end <= 1 && end >= 0 && begin <= end)) {
|
|
Logger::warn("Passed invalid range to Spline::length");
|
|
return 0;
|
|
}
|
|
|
|
if (!begin) {
|
|
if (auto p = m_lengthCache.ptr(end))
|
|
return *p;
|
|
}
|
|
|
|
DataT res = 0;
|
|
PointT previousPoint = pointAt(begin);
|
|
for (size_t i = 1; i <= subdivisions; i++) {
|
|
PointT currentPoint = pointAt(i / subdivisions * (end - begin));
|
|
res += (currentPoint - previousPoint).magnitude();
|
|
previousPoint = currentPoint;
|
|
}
|
|
|
|
if (!begin)
|
|
m_lengthCache.set(end, res);
|
|
|
|
return res;
|
|
}
|
|
|
|
float arcLenPara(float u, DataT epsilon = .01) const {
|
|
if (u == 0)
|
|
return 0;
|
|
if (u == 1)
|
|
return 1;
|
|
u = clamp<float>(u, 0, 1);
|
|
if (u == 0 || u == 1) {
|
|
Logger::warn("Passed out of range time to Spline::arcLenPara");
|
|
return u;
|
|
}
|
|
DataT targetLength = length() * u;
|
|
float t = .5;
|
|
float lower = 0;
|
|
float upper = 1;
|
|
DataT approxLen = length(0, t);
|
|
while (targetLength - approxLen > epsilon || targetLength - approxLen < -epsilon) {
|
|
if (targetLength > approxLen) {
|
|
lower = t;
|
|
} else {
|
|
upper = t;
|
|
}
|
|
t = (upper - lower) * .5 + lower;
|
|
approxLen = length(0, t);
|
|
}
|
|
return t;
|
|
}
|
|
|
|
PointT& origin() {
|
|
m_pointCache.clear();
|
|
m_lengthCache.clear();
|
|
return (*this)[0];
|
|
}
|
|
|
|
PointT const& origin() const {
|
|
return (*this)[0];
|
|
}
|
|
|
|
PointT& dest() {
|
|
m_pointCache.clear();
|
|
m_lengthCache.clear();
|
|
return (*this)[Order];
|
|
}
|
|
|
|
PointT const& dest() const {
|
|
return (*this)[Order];
|
|
}
|
|
|
|
PointT& operator[](size_t index) {
|
|
m_pointCache.clear();
|
|
m_lengthCache.clear();
|
|
return PointData::operator[](index);
|
|
}
|
|
|
|
PointT const& operator[](size_t index) const {
|
|
return PointData::operator[](index);
|
|
}
|
|
|
|
protected:
|
|
mutable LruCache<float, PointT> m_pointCache;
|
|
mutable LruCache<float, DataT> m_lengthCache;
|
|
};
|
|
|
|
typedef Spline<float, 2, 3, Vec2F> CSplineF;
|
|
|
|
}
|
|
|
|
#endif
|