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 <boost/config.hpp>
18
19 namespace std_workaround {
20
21 #if defined(__cpp_lib_nonmember_container_access) || (defined(BOOST_MSVC) && (BOOST_MSVC >= 1900))
22 using std::size;
23 #else
24 template <class C>
25 inline BOOST_CONSTEXPR std::size_t size(const C& c)
26 {
27 return c.size();
28 }
29 template <class T, std::size_t N>
30 inline BOOST_CONSTEXPR std::size_t size(const T(&array)[N]) BOOST_NOEXCEPT
31 {
32 return N;
33 }
34 #endif
35 }
36
37 namespace boost{ namespace math{
38
39 namespace detail
40 {
41 template<class Point>
alpha_distance(Point const & p1,Point const & p2,typename Point::value_type alpha)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
catmull_rom(std::initializer_list<Point> l,bool closed=false,value_type alpha=(value_type)1/(value_type)2)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
max_parameter() const66 value_type max_parameter() const
67 {
68 return m_max_s;
69 }
70
parameter_at_point(size_t i) const71 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
get_points()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 >
catmull_rom(RandomAccessContainer && points,bool closed,typename Point::value_type alpha)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 (std::ptrdiff_t i = num_pnts-1; i >= 0; --i)
115 {
116 m_pnts[i+1] = m_pnts[i];
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 typename Point::value_type s3s = m_s[i+2] - s;
164
165 Point A1_or_A3;
166 typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
167 for(size_t j = 0; j < size(m_pnts[0]); ++j)
168 {
169 A1_or_A3[j] = denom*(s1s*m_pnts[i-1][j] - s0s*m_pnts[i][j]);
170 }
171
172 Point A2_or_B2;
173 for(size_t j = 0; j < size(m_pnts[0]); ++j)
174 {
175 A2_or_B2[j] = denom21*(s2s*m_pnts[i][j] - s1s*m_pnts[i+1][j]);
176 }
177
178 Point B1_or_C;
179 denom = 1/(m_s[i+1] - m_s[i-1]);
180 for(size_t j = 0; j < size(m_pnts[0]); ++j)
181 {
182 B1_or_C[j] = denom*(s2s*A1_or_A3[j] - s0s*A2_or_B2[j]);
183 }
184
185 denom = 1/(m_s[i+2] - m_s[i+1]);
186 for(size_t j = 0; j < size(m_pnts[0]); ++j)
187 {
188 A1_or_A3[j] = denom*(s3s*m_pnts[i+1][j] - s2s*m_pnts[i+2][j]);
189 }
190
191 Point B2;
192 denom = 1/(m_s[i+2] - m_s[i]);
193 for(size_t j = 0; j < size(m_pnts[0]); ++j)
194 {
195 B2[j] = denom*(s3s*A2_or_B2[j] - s1s*A1_or_A3[j]);
196 }
197
198 for(size_t j = 0; j < size(m_pnts[0]); ++j)
199 {
200 B1_or_C[j] = denom21*(s2s*B1_or_C[j] - s1s*B2[j]);
201 }
202
203 return B1_or_C;
204 }
205
206 template<class Point, class RandomAccessContainer >
207 Point catmull_rom<Point, RandomAccessContainer>::prime(const typename Point::value_type s) const
208 {
209 using std_workaround::size;
210 // https://math.stackexchange.com/questions/843595/how-can-i-calculate-the-derivative-of-a-catmull-rom-spline-with-nonuniform-param
211 // http://denkovacs.com/2016/02/catmull-rom-spline-derivatives/
212 if (s < 0 || s > m_max_s)
213 {
214 throw std::domain_error("Parameter outside bounds.\n");
215 }
216 auto it = std::upper_bound(m_s.begin(), m_s.end(), s);
217 //Now *it >= s. We want the index such that m_s[i] <= s < m_s[i+1]:
218 size_t i = std::distance(m_s.begin(), it - 1);
219 Point A1;
220 typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
221 typename Point::value_type k1 = (m_s[i]-s)*denom;
222 typename Point::value_type k2 = (s - m_s[i-1])*denom;
223 for (size_t j = 0; j < size(m_pnts[0]); ++j)
224 {
225 A1[j] = k1*m_pnts[i-1][j] + k2*m_pnts[i][j];
226 }
227
228 Point A1p;
229 for (size_t j = 0; j < size(m_pnts[0]); ++j)
230 {
231 A1p[j] = denom*(m_pnts[i][j] - m_pnts[i-1][j]);
232 }
233
234 Point A2;
235 denom = 1/(m_s[i+1] - m_s[i]);
236 k1 = (m_s[i+1]-s)*denom;
237 k2 = (s - m_s[i])*denom;
238 for (size_t j = 0; j < size(m_pnts[0]); ++j)
239 {
240 A2[j] = k1*m_pnts[i][j] + k2*m_pnts[i+1][j];
241 }
242
243 Point A2p;
244 for (size_t j = 0; j < size(m_pnts[0]); ++j)
245 {
246 A2p[j] = denom*(m_pnts[i+1][j] - m_pnts[i][j]);
247 }
248
249
250 Point B1;
251 for (size_t j = 0; j < size(m_pnts[0]); ++j)
252 {
253 B1[j] = k1*A1[j] + k2*A2[j];
254 }
255
256 Point A3;
257 denom = 1/(m_s[i+2] - m_s[i+1]);
258 k1 = (m_s[i+2]-s)*denom;
259 k2 = (s - m_s[i+1])*denom;
260 for (size_t j = 0; j < size(m_pnts[0]); ++j)
261 {
262 A3[j] = k1*m_pnts[i+1][j] + k2*m_pnts[i+2][j];
263 }
264
265 Point A3p;
266 for (size_t j = 0; j < size(m_pnts[0]); ++j)
267 {
268 A3p[j] = denom*(m_pnts[i+2][j] - m_pnts[i+1][j]);
269 }
270
271 Point B2;
272 denom = 1/(m_s[i+2] - m_s[i]);
273 k1 = (m_s[i+2]-s)*denom;
274 k2 = (s - m_s[i])*denom;
275 for (size_t j = 0; j < size(m_pnts[0]); ++j)
276 {
277 B2[j] = k1*A2[j] + k2*A3[j];
278 }
279
280 Point B1p;
281 denom = 1/(m_s[i+1] - m_s[i-1]);
282 for (size_t j = 0; j < size(m_pnts[0]); ++j)
283 {
284 B1p[j] = denom*(A2[j] - A1[j] + (m_s[i+1]- s)*A1p[j] + (s-m_s[i-1])*A2p[j]);
285 }
286
287 Point B2p;
288 denom = 1/(m_s[i+2] - m_s[i]);
289 for (size_t j = 0; j < size(m_pnts[0]); ++j)
290 {
291 B2p[j] = denom*(A3[j] - A2[j] + (m_s[i+2] - s)*A2p[j] + (s - m_s[i])*A3p[j]);
292 }
293
294 Point Cp;
295 denom = 1/(m_s[i+1] - m_s[i]);
296 for (size_t j = 0; j < size(m_pnts[0]); ++j)
297 {
298 Cp[j] = denom*(B2[j] - B1[j] + (m_s[i+1] - s)*B1p[j] + (s - m_s[i])*B2p[j]);
299 }
300 return Cp;
301 }
302
303
304 }}
305 #endif
306