Debian Codegraph @

third-party/boost-math/include/boost/math/interpolators/catmull_rom.hpp

raw
1 // Copyright Nick Thompson, 2017
2 // Use, modification and distribution are subject to the
3 // Boost Software License, Version 1.0.
4 // (See accompanying file LICENSE_1_0.txt
5 // or copy at http://www.boost.org/LICENSE_1_0.txt)
6
7 // This computes the Catmull-Rom spline from a list of points.
8
9 #ifndef BOOST_MATH_INTERPOLATORS_CATMULL_ROM
10 #define BOOST_MATH_INTERPOLATORS_CATMULL_ROM
11
12 #include <cmath>
13 #include <vector>
14 #include <algorithm>
15 #include <iterator>
16 #include <stdexcept>
17 #include <limits>
18
19 namespace std_workaround {
20
21 #if defined(__cpp_lib_nonmember_container_access) || (defined(_MSC_VER) && (_MSC_VER >= 1900))
22 using std::size;
23 #else
24 template <class C>
25 inline constexpr std::size_t size(const C& c)
26 {
27 return c.size();
28 }
29 template <class T, std::size_t N>
30 inline constexpr std::size_t size(const T(&array)[N]) noexcept
31 {
32 return N;
33 }
34 #endif
35 }
36
37 namespace boost{ namespace math{
38
39 namespace detail
40 {
41 template<class Point>
42 typename Point::value_type alpha_distance(Point const & p1, Point const & p2, typename Point::value_type alpha)
43 {
44 using std::pow;
45 using std_workaround::size;
46 typename Point::value_type dsq = 0;
47 for (size_t i = 0; i < size(p1); ++i)
48 {
49 typename Point::value_type dx = p1[i] - p2[i];
50 dsq += dx*dx;
51 }
52 return pow(dsq, alpha/2);
53 }
54 }
55
56 template <class Point, class RandomAccessContainer = std::vector<Point> >
57 class catmull_rom
58 {
59 typedef typename Point::value_type value_type;
60 public:
61
62 catmull_rom(RandomAccessContainer&& points, bool closed = false, value_type alpha = (value_type) 1/ (value_type) 2);
63
64 catmull_rom(std::initializer_list<Point> l, bool closed = false, value_type alpha = (value_type) 1/ (value_type) 2) : catmull_rom<Point, RandomAccessContainer>(RandomAccessContainer(l), closed, alpha) {}
65
66 value_type max_parameter() const
67 {
68 return m_max_s;
69 }
70
71 value_type parameter_at_point(size_t i) const
72 {
73 return m_s[i+1];
74 }
75
76 Point operator()(const value_type s) const;
77
78 Point prime(const value_type s) const;
79
80 RandomAccessContainer&& get_points()
81 {
82 return std::move(m_pnts);
83 }
84
85 private:
86 RandomAccessContainer m_pnts;
87 std::vector<value_type> m_s;
88 value_type m_max_s;
89 };
90
91 template<class Point, class RandomAccessContainer >
92 catmull_rom<Point, RandomAccessContainer>::catmull_rom(RandomAccessContainer&& points, bool closed, typename Point::value_type alpha) : m_pnts(std::move(points))
93 {
94 std::size_t num_pnts = m_pnts.size();
95 //std::cout << "Number of points = " << num_pnts << "\n";
96 if (num_pnts < 4)
97 {
98 throw std::domain_error("The Catmull-Rom curve requires at least 4 points.");
99 }
100 if (alpha < 0 || alpha > 1)
101 {
102 throw std::domain_error("The parametrization alpha must be in the range [0,1].");
103 }
104
105 using std::abs;
106 m_s.resize(num_pnts+3);
107 m_pnts.resize(num_pnts+3);
108 //std::cout << "Number of points now = " << m_pnts.size() << "\n";
109
110 m_pnts[num_pnts+1] = m_pnts[0];
111 m_pnts[num_pnts+2] = m_pnts[1];
112
113 auto tmp = m_pnts[num_pnts-1];
114 for (auto i = num_pnts; i > 0; --i)
115 {
116 m_pnts[i] = m_pnts[i - 1];
117 }
118 m_pnts[0] = tmp;
119
120 m_s[0] = -detail::alpha_distance<Point>(m_pnts[0], m_pnts[1], alpha);
121 if (abs(m_s[0]) < std::numeric_limits<typename Point::value_type>::epsilon())
122 {
123 throw std::domain_error("The first and last point should not be the same.\n");
124 }
125 m_s[1] = 0;
126 for (size_t i = 2; i < m_s.size(); ++i)
127 {
128 typename Point::value_type d = detail::alpha_distance<Point>(m_pnts[i], m_pnts[i-1], alpha);
129 if (abs(d) < std::numeric_limits<typename Point::value_type>::epsilon())
130 {
131 throw std::domain_error("The control points of the Catmull-Rom curve are too close together; this will lead to ill-conditioning.\n");
132 }
133 m_s[i] = m_s[i-1] + d;
134 }
135 if(closed)
136 {
137 m_max_s = m_s[num_pnts+1];
138 }
139 else
140 {
141 m_max_s = m_s[num_pnts];
142 }
143 }
144
145
146 template<class Point, class RandomAccessContainer >
147 Point catmull_rom<Point, RandomAccessContainer>::operator()(const typename Point::value_type s) const
148 {
149 using std_workaround::size;
150 if (s < 0 || s > m_max_s)
151 {
152 throw std::domain_error("Parameter outside bounds.");
153 }
154 auto it = std::upper_bound(m_s.begin(), m_s.end(), s);
155 //Now *it >= s. We want the index such that m_s[i] <= s < m_s[i+1]:
156 size_t i = std::distance(m_s.begin(), it - 1);
157
158 // Only denom21 is used twice:
159 typename Point::value_type denom21 = 1/(m_s[i+1] - m_s[i]);
160 typename Point::value_type s0s = m_s[i-1] - s;
161 typename Point::value_type s1s = m_s[i] - s;
162 typename Point::value_type s2s = m_s[i+1] - s;
163 size_t ip2 = i + 2;
164 // When the curve is closed and we evaluate at the end, the endpoint is in fact the startpoint.
165 if (ip2 == m_s.size()) {
166 ip2 = 0;
167 }
168 typename Point::value_type s3s = m_s[ip2] - s;
169
170 Point A1_or_A3;
171 typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
172 for(size_t j = 0; j < size(m_pnts[0]); ++j)
173 {
174 A1_or_A3[j] = denom*(s1s*m_pnts[i-1][j] - s0s*m_pnts[i][j]);
175 }
176
177 Point A2_or_B2;
178 for(size_t j = 0; j < size(m_pnts[0]); ++j)
179 {
180 A2_or_B2[j] = denom21*(s2s*m_pnts[i][j] - s1s*m_pnts[i+1][j]);
181 }
182
183 Point B1_or_C;
184 denom = 1/(m_s[i+1] - m_s[i-1]);
185 for(size_t j = 0; j < size(m_pnts[0]); ++j)
186 {
187 B1_or_C[j] = denom*(s2s*A1_or_A3[j] - s0s*A2_or_B2[j]);
188 }
189
190 denom = 1/(m_s[ip2] - m_s[i+1]);
191 for(size_t j = 0; j < size(m_pnts[0]); ++j)
192 {
193 A1_or_A3[j] = denom*(s3s*m_pnts[i+1][j] - s2s*m_pnts[ip2][j]);
194 }
195
196 Point B2;
197 denom = 1/(m_s[ip2] - m_s[i]);
198 for(size_t j = 0; j < size(m_pnts[0]); ++j)
199 {
200 B2[j] = denom*(s3s*A2_or_B2[j] - s1s*A1_or_A3[j]);
201 }
202
203 for(size_t j = 0; j < size(m_pnts[0]); ++j)
204 {
205 B1_or_C[j] = denom21*(s2s*B1_or_C[j] - s1s*B2[j]);
206 }
207
208 return B1_or_C;
209 }
210
211 template<class Point, class RandomAccessContainer >
212 Point catmull_rom<Point, RandomAccessContainer>::prime(const typename Point::value_type s) const
213 {
214 using std_workaround::size;
215 // https://math.stackexchange.com/questions/843595/how-can-i-calculate-the-derivative-of-a-catmull-rom-spline-with-nonuniform-param
216 // http://denkovacs.com/2016/02/catmull-rom-spline-derivatives/
217 if (s < 0 || s > m_max_s)
218 {
219 throw std::domain_error("Parameter outside bounds.\n");
220 }
221 auto it = std::upper_bound(m_s.begin(), m_s.end(), s);
222 //Now *it >= s. We want the index such that m_s[i] <= s < m_s[i+1]:
223 size_t i = std::distance(m_s.begin(), it - 1);
224 Point A1;
225 typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
226 typename Point::value_type k1 = (m_s[i]-s)*denom;
227 typename Point::value_type k2 = (s - m_s[i-1])*denom;
228 for (size_t j = 0; j < size(m_pnts[0]); ++j)
229 {
230 A1[j] = k1*m_pnts[i-1][j] + k2*m_pnts[i][j];
231 }
232
233 Point A1p;
234 for (size_t j = 0; j < size(m_pnts[0]); ++j)
235 {
236 A1p[j] = denom*(m_pnts[i][j] - m_pnts[i-1][j]);
237 }
238
239 Point A2;
240 denom = 1/(m_s[i+1] - m_s[i]);
241 k1 = (m_s[i+1]-s)*denom;
242 k2 = (s - m_s[i])*denom;
243 for (size_t j = 0; j < size(m_pnts[0]); ++j)
244 {
245 A2[j] = k1*m_pnts[i][j] + k2*m_pnts[i+1][j];
246 }
247
248 Point A2p;
249 for (size_t j = 0; j < size(m_pnts[0]); ++j)
250 {
251 A2p[j] = denom*(m_pnts[i+1][j] - m_pnts[i][j]);
252 }
253
254
255 Point B1;
256 for (size_t j = 0; j < size(m_pnts[0]); ++j)
257 {
258 B1[j] = k1*A1[j] + k2*A2[j];
259 }
260
261 Point A3;
262 denom = 1/(m_s[i+2] - m_s[i+1]);
263 k1 = (m_s[i+2]-s)*denom;
264 k2 = (s - m_s[i+1])*denom;
265 for (size_t j = 0; j < size(m_pnts[0]); ++j)
266 {
267 A3[j] = k1*m_pnts[i+1][j] + k2*m_pnts[i+2][j];
268 }
269
270 Point A3p;
271 for (size_t j = 0; j < size(m_pnts[0]); ++j)
272 {
273 A3p[j] = denom*(m_pnts[i+2][j] - m_pnts[i+1][j]);
274 }
275
276 Point B2;
277 denom = 1/(m_s[i+2] - m_s[i]);
278 k1 = (m_s[i+2]-s)*denom;
279 k2 = (s - m_s[i])*denom;
280 for (size_t j = 0; j < size(m_pnts[0]); ++j)
281 {
282 B2[j] = k1*A2[j] + k2*A3[j];
283 }
284
285 Point B1p;
286 denom = 1/(m_s[i+1] - m_s[i-1]);
287 for (size_t j = 0; j < size(m_pnts[0]); ++j)
288 {
289 B1p[j] = denom*(A2[j] - A1[j] + (m_s[i+1]- s)*A1p[j] + (s-m_s[i-1])*A2p[j]);
290 }
291
292 Point B2p;
293 denom = 1/(m_s[i+2] - m_s[i]);
294 for (size_t j = 0; j < size(m_pnts[0]); ++j)
295 {
296 B2p[j] = denom*(A3[j] - A2[j] + (m_s[i+2] - s)*A2p[j] + (s - m_s[i])*A3p[j]);
297 }
298
299 Point Cp;
300 denom = 1/(m_s[i+1] - m_s[i]);
301 for (size_t j = 0; j < size(m_pnts[0]); ++j)
302 {
303 Cp[j] = denom*(B2[j] - B1[j] + (m_s[i+1] - s)*B1p[j] + (s - m_s[i])*B2p[j]);
304 }
305 return Cp;
306 }
307
308
309 }}
310 #endif