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