311 lines · plain
1// Copyright Nick Thompson, 20172// Use, modification and distribution are subject to the3// Boost Software License, Version 1.0.4// (See accompanying file LICENSE_1_0.txt5// 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_ROM10#define BOOST_MATH_INTERPOLATORS_CATMULL_ROM11 12#include <cmath>13#include <vector>14#include <algorithm>15#include <iterator>16#include <stdexcept>17#include <limits>18 19namespace std_workaround {20 21#if defined(__cpp_lib_nonmember_container_access) || (defined(_MSC_VER) && (_MSC_VER >= 1900))22 using std::size;23#else24 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]) noexcept31 {32 return N;33 }34#endif35}36 37namespace boost{ namespace math{38 39 namespace detail40 {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 56template <class Point, class RandomAccessContainer = std::vector<Point> >57class catmull_rom58{59 typedef typename Point::value_type value_type;60public: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() const67 {68 return m_max_s;69 }70 71 value_type parameter_at_point(size_t i) const72 {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 85private:86 RandomAccessContainer m_pnts;87 std::vector<value_type> m_s;88 value_type m_max_s;89};90 91template<class Point, class RandomAccessContainer >92catmull_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 else140 {141 m_max_s = m_s[num_pnts];142 }143}144 145 146template<class Point, class RandomAccessContainer >147Point catmull_rom<Point, RandomAccessContainer>::operator()(const typename Point::value_type s) const148{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 211template<class Point, class RandomAccessContainer >212Point catmull_rom<Point, RandomAccessContainer>::prime(const typename Point::value_type s) const213{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-param216 // 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#endif311