1 // Copyright Nick Thompson, 2019 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 #ifndef BOOST_MATH_INTERPOLATORS_CARDINAL_QUINTIC_B_SPLINE_DETAIL_HPP 8 #define BOOST_MATH_INTERPOLATORS_CARDINAL_QUINTIC_B_SPLINE_DETAIL_HPP 9 #include <cmath> 10 #include <vector> 11 #include <utility> 12 #include <boost/math/special_functions/cardinal_b_spline.hpp> 13 14 namespace boost{ namespace math{ namespace interpolators{ namespace detail{ 15 16 17 template <class Real> 18 class cardinal_quintic_b_spline_detail 19 { 20 public: 21 // If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them. 22 // y[0] = y(a), y[n -1] = y(b), step_size = (b - a)/(n -1). 23 cardinal_quintic_b_spline_detail(const Real * const y,size_t n,Real t0,Real h,std::pair<Real,Real> left_endpoint_derivatives,std::pair<Real,Real> right_endpoint_derivatives)24 cardinal_quintic_b_spline_detail(const Real* const y, 25 size_t n, 26 Real t0 /* initial time, left endpoint */, 27 Real h /*spacing, stepsize*/, 28 std::pair<Real, Real> left_endpoint_derivatives, 29 std::pair<Real, Real> right_endpoint_derivatives) 30 { 31 static_assert(!std::is_integral<Real>::value, "The quintic B-spline interpolator only works with floating point types."); 32 if (h <= 0) { 33 throw std::logic_error("Spacing must be > 0."); 34 } 35 m_inv_h = 1/h; 36 m_t0 = t0; 37 38 if (n < 8) { 39 throw std::logic_error("The quntic B-spline interpolator requires at least 8 points."); 40 } 41 42 using std::isnan; 43 // This interpolator has error of order h^6, so the derivatives should be estimated with the same error. 44 // See: https://en.wikipedia.org/wiki/Finite_difference_coefficient 45 if (isnan(left_endpoint_derivatives.first)) { 46 Real tmp = -49*y[0]/20 + 6*y[1] - 15*y[2]/2 + 20*y[3]/3 - 15*y[4]/4 + 6*y[5]/5 - y[6]/6; 47 left_endpoint_derivatives.first = tmp/h; 48 } 49 if (isnan(right_endpoint_derivatives.first)) { 50 Real tmp = 49*y[n-1]/20 - 6*y[n-2] + 15*y[n-3]/2 - 20*y[n-4]/3 + 15*y[n-5]/4 - 6*y[n-6]/5 + y[n-7]/6; 51 right_endpoint_derivatives.first = tmp/h; 52 } 53 if(isnan(left_endpoint_derivatives.second)) { 54 Real tmp = 469*y[0]/90 - 223*y[1]/10 + 879*y[2]/20 - 949*y[3]/18 + 41*y[4] - 201*y[5]/10 + 1019*y[6]/180 - 7*y[7]/10; 55 left_endpoint_derivatives.second = tmp/(h*h); 56 } 57 if (isnan(right_endpoint_derivatives.second)) { 58 Real tmp = 469*y[n-1]/90 - 223*y[n-2]/10 + 879*y[n-3]/20 - 949*y[n-4]/18 + 41*y[n-5] - 201*y[n-6]/10 + 1019*y[n-7]/180 - 7*y[n-8]/10; 59 right_endpoint_derivatives.second = tmp/(h*h); 60 } 61 62 // This is really challenging my mental limits on by-hand row reduction. 63 // I debated bringing in a dependency on a sparse linear solver, but given that that would cause much agony for users I decided against it. 64 65 std::vector<Real> rhs(n+4); 66 rhs[0] = 20*y[0] - 12*h*left_endpoint_derivatives.first + 2*h*h*left_endpoint_derivatives.second; 67 rhs[1] = 60*y[0] - 12*h*left_endpoint_derivatives.first; 68 for (size_t i = 2; i < n + 2; ++i) { 69 rhs[i] = 120*y[i-2]; 70 } 71 rhs[n+2] = 60*y[n-1] + 12*h*right_endpoint_derivatives.first; 72 rhs[n+3] = 20*y[n-1] + 12*h*right_endpoint_derivatives.first + 2*h*h*right_endpoint_derivatives.second; 73 74 std::vector<Real> diagonal(n+4, 66); 75 diagonal[0] = 1; 76 diagonal[1] = 18; 77 diagonal[n+2] = 18; 78 diagonal[n+3] = 1; 79 80 std::vector<Real> first_superdiagonal(n+4, 26); 81 first_superdiagonal[0] = 10; 82 first_superdiagonal[1] = 33; 83 first_superdiagonal[n+2] = 1; 84 // There is one less superdiagonal than diagonal; make sure that if we read it, it shows up as a bug: 85 first_superdiagonal[n+3] = std::numeric_limits<Real>::quiet_NaN(); 86 87 std::vector<Real> second_superdiagonal(n+4, 1); 88 second_superdiagonal[0] = 9; 89 second_superdiagonal[1] = 8; 90 second_superdiagonal[n+2] = std::numeric_limits<Real>::quiet_NaN(); 91 second_superdiagonal[n+3] = std::numeric_limits<Real>::quiet_NaN(); 92 93 std::vector<Real> first_subdiagonal(n+4, 26); 94 first_subdiagonal[0] = std::numeric_limits<Real>::quiet_NaN(); 95 first_subdiagonal[1] = 1; 96 first_subdiagonal[n+2] = 33; 97 first_subdiagonal[n+3] = 10; 98 99 std::vector<Real> second_subdiagonal(n+4, 1); 100 second_subdiagonal[0] = std::numeric_limits<Real>::quiet_NaN(); 101 second_subdiagonal[1] = std::numeric_limits<Real>::quiet_NaN(); 102 second_subdiagonal[n+2] = 8; 103 second_subdiagonal[n+3] = 9; 104 105 106 for (size_t i = 0; i < n+2; ++i) { 107 Real di = diagonal[i]; 108 diagonal[i] = 1; 109 first_superdiagonal[i] /= di; 110 second_superdiagonal[i] /= di; 111 rhs[i] /= di; 112 113 // Eliminate first subdiagonal: 114 Real nfsub = -first_subdiagonal[i+1]; 115 // Superfluous: 116 first_subdiagonal[i+1] /= nfsub; 117 // Not superfluous: 118 diagonal[i+1] /= nfsub; 119 first_superdiagonal[i+1] /= nfsub; 120 second_superdiagonal[i+1] /= nfsub; 121 rhs[i+1] /= nfsub; 122 123 diagonal[i+1] += first_superdiagonal[i]; 124 first_superdiagonal[i+1] += second_superdiagonal[i]; 125 rhs[i+1] += rhs[i]; 126 // Superfluous, but clarifying: 127 first_subdiagonal[i+1] = 0; 128 129 // Eliminate second subdiagonal: 130 Real nssub = -second_subdiagonal[i+2]; 131 first_subdiagonal[i+2] /= nssub; 132 diagonal[i+2] /= nssub; 133 first_superdiagonal[i+2] /= nssub; 134 second_superdiagonal[i+2] /= nssub; 135 rhs[i+2] /= nssub; 136 137 first_subdiagonal[i+2] += first_superdiagonal[i]; 138 diagonal[i+2] += second_superdiagonal[i]; 139 rhs[i+2] += rhs[i]; 140 // Superfluous, but clarifying: 141 second_subdiagonal[i+2] = 0; 142 } 143 144 // Eliminate last subdiagonal: 145 Real dnp2 = diagonal[n+2]; 146 diagonal[n+2] = 1; 147 first_superdiagonal[n+2] /= dnp2; 148 rhs[n+2] /= dnp2; 149 Real nfsubnp3 = -first_subdiagonal[n+3]; 150 diagonal[n+3] /= nfsubnp3; 151 rhs[n+3] /= nfsubnp3; 152 153 diagonal[n+3] += first_superdiagonal[n+2]; 154 rhs[n+3] += rhs[n+2]; 155 156 m_alpha.resize(n + 4, std::numeric_limits<Real>::quiet_NaN()); 157 158 m_alpha[n+3] = rhs[n+3]/diagonal[n+3]; 159 m_alpha[n+2] = rhs[n+2] - first_superdiagonal[n+2]*m_alpha[n+3]; 160 for (int64_t i = int64_t(n+1); i >= 0; --i) { 161 m_alpha[i] = rhs[i] - first_superdiagonal[i]*m_alpha[i+1] - second_superdiagonal[i]*m_alpha[i+2]; 162 } 163 164 } 165 operator ()(Real t) const166 Real operator()(Real t) const { 167 using std::ceil; 168 using std::floor; 169 using boost::math::cardinal_b_spline; 170 // tf = t0 + (n-1)*h 171 // alpha.size() = n+4 172 if (t < m_t0 || t > m_t0 + (m_alpha.size()-5)/m_inv_h) { 173 const char* err_msg = "Tried to evaluate the cardinal quintic b-spline outside the domain of of interpolation; extrapolation does not work."; 174 throw std::domain_error(err_msg); 175 } 176 Real x = (t-m_t0)*m_inv_h; 177 // Support of B_5 is [-3, 3]. So -3 < x - j + 2 < 3, so x-1 < j < x+5. 178 // TODO: Zero pad m_alpha so that only the domain check is necessary. 179 int64_t j_min = std::max(int64_t(0), int64_t(ceil(x-1))); 180 int64_t j_max = std::min(int64_t(m_alpha.size() - 1), int64_t(floor(x+5)) ); 181 Real s = 0; 182 for (int64_t j = j_min; j <= j_max; ++j) { 183 // TODO: Use Cox 1972 to generate all integer translates of B5 simultaneously. 184 s += m_alpha[j]*cardinal_b_spline<5, Real>(x - j + 2); 185 } 186 return s; 187 } 188 prime(Real t) const189 Real prime(Real t) const { 190 using std::ceil; 191 using std::floor; 192 using boost::math::cardinal_b_spline_prime; 193 if (t < m_t0 || t > m_t0 + (m_alpha.size()-5)/m_inv_h) { 194 const char* err_msg = "Tried to evaluate the cardinal quintic b-spline outside the domain of of interpolation; extrapolation does not work."; 195 throw std::domain_error(err_msg); 196 } 197 Real x = (t-m_t0)*m_inv_h; 198 // Support of B_5 is [-3, 3]. So -3 < x - j + 2 < 3, so x-1 < j < x+5 199 int64_t j_min = std::max(int64_t(0), int64_t(ceil(x-1))); 200 int64_t j_max = std::min(int64_t(m_alpha.size() - 1), int64_t(floor(x+5)) ); 201 Real s = 0; 202 for (int64_t j = j_min; j <= j_max; ++j) { 203 s += m_alpha[j]*cardinal_b_spline_prime<5, Real>(x - j + 2); 204 } 205 return s*m_inv_h; 206 207 } 208 double_prime(Real t) const209 Real double_prime(Real t) const { 210 using std::ceil; 211 using std::floor; 212 using boost::math::cardinal_b_spline_double_prime; 213 if (t < m_t0 || t > m_t0 + (m_alpha.size()-5)/m_inv_h) { 214 const char* err_msg = "Tried to evaluate the cardinal quintic b-spline outside the domain of of interpolation; extrapolation does not work."; 215 throw std::domain_error(err_msg); 216 } 217 Real x = (t-m_t0)*m_inv_h; 218 // Support of B_5 is [-3, 3]. So -3 < x - j + 2 < 3, so x-1 < j < x+5 219 int64_t j_min = std::max(int64_t(0), int64_t(ceil(x-1))); 220 int64_t j_max = std::min(int64_t(m_alpha.size() - 1), int64_t(floor(x+5)) ); 221 Real s = 0; 222 for (int64_t j = j_min; j <= j_max; ++j) { 223 s += m_alpha[j]*cardinal_b_spline_double_prime<5, Real>(x - j + 2); 224 } 225 return s*m_inv_h*m_inv_h; 226 } 227 228 t_max() const229 Real t_max() const { 230 return m_t0 + (m_alpha.size()-5)/m_inv_h; 231 } 232 233 private: 234 std::vector<Real> m_alpha; 235 Real m_inv_h; 236 Real m_t0; 237 }; 238 239 }}}} 240 #endif 241