math/interpolators/catmull_rom.hpp

0001 // Copyright Nick Thompson, 2017
0002 // Use, modification and distribution are subject to the
0003 // Boost Software License, Version 1.0.
0004 // (See accompanying file LICENSE_1_0.txt
0005 // or copy at http://www.boost.org/LICENSE_1_0.txt)
0006
0007 // This computes the Catmull-Rom spline from a list of points.
0008
0009 #ifndef BOOST_MATH_INTERPOLATORS_CATMULL_ROM
0010 #define BOOST_MATH_INTERPOLATORS_CATMULL_ROM
0011
0012 #include <cmath>
0013 #include <vector>
0014 #include <algorithm>
0015 #include <iterator>
0016 #include <stdexcept>
0017 #include <limits>
0018
0019 namespace std_workaround {
0020
0021 #if defined(__cpp_lib_nonmember_container_access) || (defined(_MSC_VER) && (_MSC_VER >= 1900))
0022    using std::size;
0023 #else
0024    template <class C>
0025    inline constexpr std::size_t size(const C& c)
0026    {
0027       return c.size();
0028    }
0029    template <class T, std::size_t N>
0030    inline constexpr std::size_t size(const T(&array)[N]) noexcept
0031    {
0032       return N;
0033    }
0034 #endif
0035 }
0036
0037 namespace boost{ namespace math{
0038
0039     namespace detail
0040     {
0041         template<class Point>
0042         typename Point::value_type alpha_distance(Point const & p1, Point const & p2, typename Point::value_type alpha)
0043         {
0044             using std::pow;
0045             using std_workaround::size;
0046             typename Point::value_type dsq = 0;
0047             for (size_t i = 0; i < size(p1); ++i)
0048             {
0049                 typename Point::value_type dx = p1[i] - p2[i];
0050                 dsq += dx*dx;
0051             }
0052             return pow(dsq, alpha/2);
0053         }
0054     }
0055
0056 template <class Point, class RandomAccessContainer = std::vector<Point> >
0057 class catmull_rom
0058 {
0059    typedef typename Point::value_type value_type;
0060 public:
0061
0062     catmull_rom(RandomAccessContainer&& points, bool closed = false, value_type alpha = (value_type) 1/ (value_type) 2);
0063
0064     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) {}
0065
0066     value_type max_parameter() const
0067     {
0068         return m_max_s;
0069     }
0070
0071     value_type parameter_at_point(size_t i) const
0072     {
0073         return m_s[i+1];
0074     }
0075
0076     Point operator()(const value_type s) const;
0077
0078     Point prime(const value_type s) const;
0079
0080     RandomAccessContainer&& get_points()
0081     {
0082         return std::move(m_pnts);
0083     }
0084
0085 private:
0086     RandomAccessContainer m_pnts;
0087     std::vector<value_type> m_s;
0088     value_type m_max_s;
0089 };
0090
0091 template<class Point, class RandomAccessContainer >
0092 catmull_rom<Point, RandomAccessContainer>::catmull_rom(RandomAccessContainer&& points, bool closed, typename Point::value_type alpha) : m_pnts(std::move(points))
0093 {
0094     std::size_t num_pnts = m_pnts.size();
0095     //std::cout << "Number of points = " << num_pnts << "\n";
0096     if (num_pnts < 4)
0097     {
0098         throw std::domain_error("The Catmull-Rom curve requires at least 4 points.");
0099     }
0100     if (alpha < 0 || alpha > 1)
0101     {
0102         throw std::domain_error("The parametrization alpha must be in the range [0,1].");
0103     }
0104
0105     using std::abs;
0106     m_s.resize(num_pnts+3);
0107     m_pnts.resize(num_pnts+3);
0108     //std::cout << "Number of points now = " << m_pnts.size() << "\n";
0109
0110     m_pnts[num_pnts+1] = m_pnts[0];
0111     m_pnts[num_pnts+2] = m_pnts[1];
0112
0113     auto tmp = m_pnts[num_pnts-1];
0114     for (auto i = num_pnts; i > 0; --i)
0115     {
0116         m_pnts[i] = m_pnts[i - 1];
0117     }
0118     m_pnts[0] = tmp;
0119
0120     m_s[0] = -detail::alpha_distance<Point>(m_pnts[0], m_pnts[1], alpha);
0121     if (abs(m_s[0]) < std::numeric_limits<typename Point::value_type>::epsilon())
0122     {
0123         throw std::domain_error("The first and last point should not be the same.\n");
0124     }
0125     m_s[1] = 0;
0126     for (size_t i = 2; i < m_s.size(); ++i)
0127     {
0128         typename Point::value_type d = detail::alpha_distance<Point>(m_pnts[i], m_pnts[i-1], alpha);
0129         if (abs(d) < std::numeric_limits<typename Point::value_type>::epsilon())
0130         {
0131             throw std::domain_error("The control points of the Catmull-Rom curve are too close together; this will lead to ill-conditioning.\n");
0132         }
0133         m_s[i] = m_s[i-1] + d;
0134     }
0135     if(closed)
0136     {
0137         m_max_s = m_s[num_pnts+1];
0138     }
0139     else
0140     {
0141         m_max_s = m_s[num_pnts];
0142     }
0143 }
0144
0145
0146 template<class Point, class RandomAccessContainer >
0147 Point catmull_rom<Point, RandomAccessContainer>::operator()(const typename Point::value_type s) const
0148 {
0149     using std_workaround::size;
0150     if (s < 0 || s > m_max_s)
0151     {
0152         throw std::domain_error("Parameter outside bounds.");
0153     }
0154     auto it = std::upper_bound(m_s.begin(), m_s.end(), s);
0155     //Now *it >= s. We want the index such that m_s[i] <= s < m_s[i+1]:
0156     size_t i = std::distance(m_s.begin(), it - 1);
0157
0158     // Only denom21 is used twice:
0159     typename Point::value_type denom21 = 1/(m_s[i+1] - m_s[i]);
0160     typename Point::value_type s0s = m_s[i-1] - s;
0161     typename Point::value_type s1s = m_s[i] - s;
0162     typename Point::value_type s2s = m_s[i+1] - s;
0163     size_t ip2 = i + 2;
0164     // When the curve is closed and we evaluate at the end, the endpoint is in fact the startpoint.
0165     if (ip2 == m_s.size()) {
0166         ip2 = 0;
0167     }
0168     typename Point::value_type s3s = m_s[ip2] - s;
0169
0170     Point A1_or_A3;
0171     typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
0172     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0173     {
0174         A1_or_A3[j] = denom*(s1s*m_pnts[i-1][j] - s0s*m_pnts[i][j]);
0175     }
0176
0177     Point A2_or_B2;
0178     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0179     {
0180         A2_or_B2[j] = denom21*(s2s*m_pnts[i][j] - s1s*m_pnts[i+1][j]);
0181     }
0182
0183     Point B1_or_C;
0184     denom = 1/(m_s[i+1] - m_s[i-1]);
0185     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0186     {
0187         B1_or_C[j] = denom*(s2s*A1_or_A3[j] - s0s*A2_or_B2[j]);
0188     }
0189
0190     denom = 1/(m_s[ip2] - m_s[i+1]);
0191     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0192     {
0193         A1_or_A3[j] = denom*(s3s*m_pnts[i+1][j] - s2s*m_pnts[ip2][j]);
0194     }
0195
0196     Point B2;
0197     denom = 1/(m_s[ip2] - m_s[i]);
0198     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0199     {
0200         B2[j] = denom*(s3s*A2_or_B2[j] - s1s*A1_or_A3[j]);
0201     }
0202
0203     for(size_t j = 0; j < size(m_pnts[0]); ++j)
0204     {
0205         B1_or_C[j] = denom21*(s2s*B1_or_C[j] - s1s*B2[j]);
0206     }
0207
0208     return B1_or_C;
0209 }
0210
0211 template<class Point, class RandomAccessContainer >
0212 Point catmull_rom<Point, RandomAccessContainer>::prime(const typename Point::value_type s) const
0213 {
0214     using std_workaround::size;
0215     // https://math.stackexchange.com/questions/843595/how-can-i-calculate-the-derivative-of-a-catmull-rom-spline-with-nonuniform-param
0216     // http://denkovacs.com/2016/02/catmull-rom-spline-derivatives/
0217     if (s < 0 || s > m_max_s)
0218     {
0219         throw std::domain_error("Parameter outside bounds.\n");
0220     }
0221     auto it = std::upper_bound(m_s.begin(), m_s.end(), s);
0222     //Now *it >= s. We want the index such that m_s[i] <= s < m_s[i+1]:
0223     size_t i = std::distance(m_s.begin(), it - 1);
0224     Point A1;
0225     typename Point::value_type denom = 1/(m_s[i] - m_s[i-1]);
0226     typename Point::value_type k1 = (m_s[i]-s)*denom;
0227     typename Point::value_type k2 = (s - m_s[i-1])*denom;
0228     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0229     {
0230         A1[j] = k1*m_pnts[i-1][j] + k2*m_pnts[i][j];
0231     }
0232
0233     Point A1p;
0234     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0235     {
0236         A1p[j] = denom*(m_pnts[i][j] - m_pnts[i-1][j]);
0237     }
0238
0239     Point A2;
0240     denom = 1/(m_s[i+1] - m_s[i]);
0241     k1 = (m_s[i+1]-s)*denom;
0242     k2 = (s - m_s[i])*denom;
0243     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0244     {
0245         A2[j] = k1*m_pnts[i][j] + k2*m_pnts[i+1][j];
0246     }
0247
0248     Point A2p;
0249     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0250     {
0251         A2p[j] = denom*(m_pnts[i+1][j] - m_pnts[i][j]);
0252     }
0253
0254
0255     Point B1;
0256     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0257     {
0258         B1[j] = k1*A1[j] + k2*A2[j];
0259     }
0260
0261     Point A3;
0262     denom = 1/(m_s[i+2] - m_s[i+1]);
0263     k1 = (m_s[i+2]-s)*denom;
0264     k2 = (s - m_s[i+1])*denom;
0265     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0266     {
0267         A3[j] = k1*m_pnts[i+1][j] + k2*m_pnts[i+2][j];
0268     }
0269
0270     Point A3p;
0271     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0272     {
0273         A3p[j] = denom*(m_pnts[i+2][j] - m_pnts[i+1][j]);
0274     }
0275
0276     Point B2;
0277     denom = 1/(m_s[i+2] - m_s[i]);
0278     k1 = (m_s[i+2]-s)*denom;
0279     k2 = (s - m_s[i])*denom;
0280     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0281     {
0282         B2[j] = k1*A2[j] + k2*A3[j];
0283     }
0284
0285     Point B1p;
0286     denom = 1/(m_s[i+1] - m_s[i-1]);
0287     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0288     {
0289         B1p[j] = denom*(A2[j] - A1[j] + (m_s[i+1]- s)*A1p[j] + (s-m_s[i-1])*A2p[j]);
0290     }
0291
0292     Point B2p;
0293     denom = 1/(m_s[i+2] - m_s[i]);
0294     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0295     {
0296         B2p[j] = denom*(A3[j] - A2[j] + (m_s[i+2] - s)*A2p[j] + (s - m_s[i])*A3p[j]);
0297     }
0298
0299     Point Cp;
0300     denom = 1/(m_s[i+1] - m_s[i]);
0301     for (size_t j = 0; j < size(m_pnts[0]); ++j)
0302     {
0303         Cp[j] = denom*(B2[j] - B1[j] + (m_s[i+1] - s)*B1p[j] + (s - m_s[i])*B2p[j]);
0304     }
0305     return Cp;
0306 }
0307
0308
0309 }}
0310 #endif