1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
#pragma once
#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;
}
|