ceph/src/boost/boost/math/interpolators/catmull_rom.hpp

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