| 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 |
#ifndef CUBIC_B_SPLINE_DETAIL_HPP |
| 8 |
#define CUBIC_B_SPLINE_DETAIL_HPP |
| 9 |
|
| 10 |
#include <limits> |
| 11 |
#include <cmath> |
| 12 |
#include <vector> |
| 13 |
#include <memory> |
| 14 |
#include <boost/math/constants/constants.hpp> |
| 15 |
#include <boost/math/special_functions/fpclassify.hpp> |
| 16 |
|
| 17 |
namespace boost{ namespace math{ namespace detail{ |
| 18 |
|
| 19 |
|
| 20 |
template <class Real> |
| 21 |
class cubic_b_spline_imp |
| 22 |
{ |
| 23 |
public: |
| 24 |
// If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them. |
| 25 |
// f[0] = f(a), f[length -1] = b, step_size = (b - a)/(length -1). |
| 26 |
template <class BidiIterator> |
| 27 |
cubic_b_spline_imp(BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size, |
| 28 |
Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(), |
| 29 |
Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN()); |
| 30 |
|
| 31 |
Real operator()(Real x) const; |
| 32 |
|
| 33 |
Real prime(Real x) const; |
| 34 |
|
| 35 |
Real double_prime(Real x) const; |
| 36 |
|
| 37 |
private: |
| 38 |
std::vector<Real> m_beta; |
| 39 |
Real m_h_inv; |
| 40 |
Real m_a; |
| 41 |
Real m_avg; |
| 42 |
}; |
| 43 |
|
| 44 |
|
| 45 |
|
| 46 |
template <class Real> |
| 47 |
Real b3_spline(Real x) |
| 48 |
{ |
| 49 |
using std::abs; |
| 50 |
Real absx = abs(x); |
| 51 |
if (absx < 1) |
| 52 |
{ |
| 53 |
Real y = 2 - absx; |
| 54 |
Real z = 1 - absx; |
| 55 |
return boost::math::constants::sixth<Real>()*(y*y*y - 4*z*z*z); |
| 56 |
} |
| 57 |
if (absx < 2) |
| 58 |
{ |
| 59 |
Real y = 2 - absx; |
| 60 |
return boost::math::constants::sixth<Real>()*y*y*y; |
| 61 |
} |
| 62 |
return static_cast<Real>(0); |
| 63 |
} |
| 64 |
|
| 65 |
template<class Real> |
| 66 |
Real b3_spline_prime(Real x) |
| 67 |
{ |
| 68 |
if (x < 0) |
| 69 |
{ |
| 70 |
return -b3_spline_prime(-x); |
| 71 |
} |
| 72 |
|
| 73 |
if (x < 1) |
| 74 |
{ |
| 75 |
return x*(3*boost::math::constants::half<Real>()*x - 2); |
| 76 |
} |
| 77 |
if (x < 2) |
| 78 |
{ |
| 79 |
return -boost::math::constants::half<Real>()*(2 - x)*(2 - x); |
| 80 |
} |
| 81 |
return static_cast<Real>(0); |
| 82 |
} |
| 83 |
|
| 84 |
template<class Real> |
| 85 |
Real b3_spline_double_prime(Real x) |
| 86 |
{ |
| 87 |
if (x < 0) |
| 88 |
{ |
| 89 |
return b3_spline_double_prime(-x); |
| 90 |
} |
| 91 |
|
| 92 |
if (x < 1) |
| 93 |
{ |
| 94 |
return 3*x - 2; |
| 95 |
} |
| 96 |
if (x < 2) |
| 97 |
{ |
| 98 |
return (2 - x); |
| 99 |
} |
| 100 |
return static_cast<Real>(0); |
| 101 |
} |
| 102 |
|
| 103 |
|
| 104 |
template <class Real> |
| 105 |
template <class BidiIterator> |
| 106 |
cubic_b_spline_imp<Real>::cubic_b_spline_imp(BidiIterator f, BidiIterator end_p, Real left_endpoint, Real step_size, |
| 107 |
Real left_endpoint_derivative, Real right_endpoint_derivative) : m_a(left_endpoint), m_avg(0) |
| 108 |
{ |
| 109 |
using boost::math::constants::third; |
| 110 |
|
| 111 |
std::size_t length = end_p - f; |
| 112 |
|
| 113 |
if (length < 5) |
| 114 |
{ |
| 115 |
if (boost::math::isnan(left_endpoint_derivative) || boost::math::isnan(right_endpoint_derivative)) |
| 116 |
{ |
| 117 |
throw std::logic_error("Interpolation using a cubic b spline with derivatives estimated at the endpoints requires at least 5 points.\n"); |
| 118 |
} |
| 119 |
if (length < 3) |
| 120 |
{ |
| 121 |
throw std::logic_error("Interpolation using a cubic b spline requires at least 3 points.\n"); |
| 122 |
} |
| 123 |
} |
| 124 |
|
| 125 |
if (boost::math::isnan(left_endpoint)) |
| 126 |
{ |
| 127 |
throw std::logic_error("Left endpoint is NAN; this is disallowed.\n"); |
| 128 |
} |
| 129 |
if (left_endpoint + length*step_size >= (std::numeric_limits<Real>::max)()) |
| 130 |
{ |
| 131 |
throw std::logic_error("Right endpoint overflows the maximum representable number of the specified precision.\n"); |
| 132 |
} |
| 133 |
if (step_size <= 0) |
| 134 |
{ |
| 135 |
throw std::logic_error("The step size must be strictly > 0.\n"); |
| 136 |
} |
| 137 |
// Storing the inverse of the stepsize does provide a measurable speedup. |
| 138 |
// It's not huge, but nonetheless worthwhile. |
| 139 |
m_h_inv = 1/step_size; |
| 140 |
|
| 141 |
// Following Kress's notation, s'(a) = a1, s'(b) = b1 |
| 142 |
Real a1 = left_endpoint_derivative; |
| 143 |
// See the finite-difference table on Wikipedia for reference on how |
| 144 |
// to construct high-order estimates for one-sided derivatives: |
| 145 |
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Forward_and_backward_finite_difference |
| 146 |
// Here, we estimate then to O(h^4), as that is the maximum accuracy we could obtain from this method. |
| 147 |
if (boost::math::isnan(a1)) |
| 148 |
{ |
| 149 |
// For simple functions (linear, quadratic, so on) |
| 150 |
// almost all the error comes from derivative estimation. |
| 151 |
// This does pairwise summation which gives us another digit of accuracy over naive summation. |
| 152 |
Real t0 = 4*(f[1] + third<Real>()*f[3]); |
| 153 |
Real t1 = -(25*third<Real>()*f[0] + f[4])/4 - 3*f[2]; |
| 154 |
a1 = m_h_inv*(t0 + t1); |
| 155 |
} |
| 156 |
|
| 157 |
Real b1 = right_endpoint_derivative; |
| 158 |
if (boost::math::isnan(b1)) |
| 159 |
{ |
| 160 |
size_t n = length - 1; |
| 161 |
Real t0 = 4*(f[n-3] + third<Real>()*f[n - 1]); |
| 162 |
Real t1 = -(25*third<Real>()*f[n - 4] + f[n])/4 - 3*f[n - 2]; |
| 163 |
|
| 164 |
b1 = m_h_inv*(t0 + t1); |
| 165 |
} |
| 166 |
|
| 167 |
// s(x) = \sum \alpha_i B_{3}( (x- x_i - a)/h ) |
| 168 |
// Of course we must reindex from Kress's notation, since he uses negative indices which make C++ unhappy. |
| 169 |
m_beta.resize(length + 2, std::numeric_limits<Real>::quiet_NaN()); |
| 170 |
|
| 171 |
// Since the splines have compact support, they decay to zero very fast outside the endpoints. |
| 172 |
// This is often very annoying; we'd like to evaluate the interpolant a little bit outside the |
| 173 |
// boundary [a,b] without massive error. |
| 174 |
// A simple way to deal with this is just to subtract the DC component off the signal, so we need the average. |
| 175 |
// This algorithm for computing the average is recommended in |
| 176 |
// http://www.heikohoffmann.de/htmlthesis/node134.html |
| 177 |
Real t = 1; |
| 178 |
for (size_t i = 0; i < length; ++i) |
| 179 |
{ |
| 180 |
if (boost::math::isnan(f[i])) |
| 181 |
{ |
| 182 |
std::string err = "This function you are trying to interpolate is a nan at index " + std::to_string(i) + "\n"; |
| 183 |
throw std::logic_error(err); |
| 184 |
} |
| 185 |
m_avg += (f[i] - m_avg) / t; |
| 186 |
t += 1; |
| 187 |
} |
| 188 |
|
| 189 |
|
| 190 |
// Now we must solve an almost-tridiagonal system, which requires O(N) operations. |
| 191 |
// There are, in fact 5 diagonals, but they only differ from zero on the first and last row, |
| 192 |
// so we can patch up the tridiagonal row reduction algorithm to deal with two special rows. |
| 193 |
// See Kress, equations 8.41 |
| 194 |
// The the "tridiagonal" matrix is: |
| 195 |
// 1 0 -1 |
| 196 |
// 1 4 1 |
| 197 |
// 1 4 1 |
| 198 |
// 1 4 1 |
| 199 |
// .... |
| 200 |
// 1 4 1 |
| 201 |
// 1 0 -1 |
| 202 |
// Numerical estimate indicate that as N->Infinity, cond(A) -> 6.9, so this matrix is good. |
| 203 |
std::vector<Real> rhs(length + 2, std::numeric_limits<Real>::quiet_NaN()); |
| 204 |
std::vector<Real> super_diagonal(length + 2, std::numeric_limits<Real>::quiet_NaN()); |
| 205 |
|
| 206 |
rhs[0] = -2*step_size*a1; |
| 207 |
rhs[rhs.size() - 1] = -2*step_size*b1; |
| 208 |
|
| 209 |
super_diagonal[0] = 0; |
| 210 |
|
| 211 |
for(size_t i = 1; i < rhs.size() - 1; ++i) |
| 212 |
{ |
| 213 |
rhs[i] = 6*(f[i - 1] - m_avg); |
| 214 |
super_diagonal[i] = 1; |
| 215 |
} |
| 216 |
|
| 217 |
|
| 218 |
// One step of row reduction on the first row to patch up the 5-diagonal problem: |
| 219 |
// 1 0 -1 | r0 |
| 220 |
// 1 4 1 | r1 |
| 221 |
// mapsto: |
| 222 |
// 1 0 -1 | r0 |
| 223 |
// 0 4 2 | r1 - r0 |
| 224 |
// mapsto |
| 225 |
// 1 0 -1 | r0 |
| 226 |
// 0 1 1/2| (r1 - r0)/4 |
| 227 |
super_diagonal[1] = 0.5; |
| 228 |
rhs[1] = (rhs[1] - rhs[0])/4; |
| 229 |
|
| 230 |
// Now do a tridiagonal row reduction the standard way, until just before the last row: |
| 231 |
for (size_t i = 2; i < rhs.size() - 1; ++i) |
| 232 |
{ |
| 233 |
Real diagonal = 4 - super_diagonal[i - 1]; |
| 234 |
rhs[i] = (rhs[i] - rhs[i - 1])/diagonal; |
| 235 |
super_diagonal[i] /= diagonal; |
| 236 |
} |
| 237 |
|
| 238 |
// Now the last row, which is in the form |
| 239 |
// 1 sd[n-3] 0 | rhs[n-3] |
| 240 |
// 0 1 sd[n-2] | rhs[n-2] |
| 241 |
// 1 0 -1 | rhs[n-1] |
| 242 |
Real final_subdiag = -super_diagonal[rhs.size() - 3]; |
| 243 |
rhs[rhs.size() - 1] = (rhs[rhs.size() - 1] - rhs[rhs.size() - 3])/final_subdiag; |
| 244 |
Real final_diag = -1/final_subdiag; |
| 245 |
// Now we're here: |
| 246 |
// 1 sd[n-3] 0 | rhs[n-3] |
| 247 |
// 0 1 sd[n-2] | rhs[n-2] |
| 248 |
// 0 1 final_diag | (rhs[n-1] - rhs[n-3])/diag |
| 249 |
|
| 250 |
final_diag = final_diag - super_diagonal[rhs.size() - 2]; |
| 251 |
rhs[rhs.size() - 1] = rhs[rhs.size() - 1] - rhs[rhs.size() - 2]; |
| 252 |
|
| 253 |
|
| 254 |
// Back substitutions: |
| 255 |
m_beta[rhs.size() - 1] = rhs[rhs.size() - 1]/final_diag; |
| 256 |
for(size_t i = rhs.size() - 2; i > 0; --i) |
| 257 |
{ |
| 258 |
m_beta[i] = rhs[i] - super_diagonal[i]*m_beta[i + 1]; |
| 259 |
} |
| 260 |
m_beta[0] = m_beta[2] + rhs[0]; |
| 261 |
} |
| 262 |
|
| 263 |
template<class Real> |
| 264 |
Real cubic_b_spline_imp<Real>::operator()(Real x) const |
| 265 |
{ |
| 266 |
// See Kress, 8.40: Since B3 has compact support, we don't have to sum over all terms, |
| 267 |
// just the (at most 5) whose support overlaps the argument. |
| 268 |
Real z = m_avg; |
| 269 |
Real t = m_h_inv*(x - m_a) + 1; |
| 270 |
|
| 271 |
using std::max; |
| 272 |
using std::min; |
| 273 |
using std::ceil; |
| 274 |
using std::floor; |
| 275 |
|
| 276 |
size_t k_min = static_cast<size_t>((max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)))); |
| 277 |
size_t k_max = static_cast<size_t>((max)((min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2))), 0l)); |
| 278 |
for (size_t k = k_min; k <= k_max; ++k) |
| 279 |
{ |
| 280 |
z += m_beta[k]*b3_spline(t - k); |
| 281 |
} |
| 282 |
|
| 283 |
return z; |
| 284 |
} |
| 285 |
|
| 286 |
template<class Real> |
| 287 |
Real cubic_b_spline_imp<Real>::prime(Real x) const |
| 288 |
{ |
| 289 |
Real z = 0; |
| 290 |
Real t = m_h_inv*(x - m_a) + 1; |
| 291 |
|
| 292 |
using std::max; |
| 293 |
using std::min; |
| 294 |
using std::ceil; |
| 295 |
using std::floor; |
| 296 |
|
| 297 |
size_t k_min = static_cast<size_t>((max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)))); |
| 298 |
size_t k_max = static_cast<size_t>((min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2)))); |
| 299 |
|
| 300 |
for (size_t k = k_min; k <= k_max; ++k) |
| 301 |
{ |
| 302 |
z += m_beta[k]*b3_spline_prime(t - k); |
| 303 |
} |
| 304 |
return z*m_h_inv; |
| 305 |
} |
| 306 |
|
| 307 |
template<class Real> |
| 308 |
Real cubic_b_spline_imp<Real>::double_prime(Real x) const |
| 309 |
{ |
| 310 |
Real z = 0; |
| 311 |
Real t = m_h_inv*(x - m_a) + 1; |
| 312 |
|
| 313 |
using std::max; |
| 314 |
using std::min; |
| 315 |
using std::ceil; |
| 316 |
using std::floor; |
| 317 |
|
| 318 |
size_t k_min = static_cast<size_t>((max)(static_cast<long>(0), boost::math::ltrunc(ceil(t - 2)))); |
| 319 |
size_t k_max = static_cast<size_t>((min)(static_cast<long>(m_beta.size() - 1), boost::math::ltrunc(floor(t + 2)))); |
| 320 |
|
| 321 |
for (size_t k = k_min; k <= k_max; ++k) |
| 322 |
{ |
| 323 |
z += m_beta[k]*b3_spline_double_prime(t - k); |
| 324 |
} |
| 325 |
return z*m_h_inv*m_h_inv; |
| 326 |
} |
| 327 |
|
| 328 |
|
| 329 |
}}} |
| 330 |
#endif |