|
| 1 | +#include <stdexcept> |
| 2 | + |
| 3 | +#include "CubicSpline.H" |
| 4 | + |
| 5 | +namespace Deproject |
| 6 | +{ |
| 7 | + |
| 8 | + CubicSpline::CubicSpline(const std::vector<double>& x_in, const std::vector<double>& y_in) { |
| 9 | + set_data(x_in, y_in); |
| 10 | + } |
| 11 | + |
| 12 | + void CubicSpline::set_data(const std::vector<double>& x_in, const std::vector<double>& y_in) { |
| 13 | + x_ = x_in; |
| 14 | + y_ = y_in; |
| 15 | + int n = (int)x_.size(); |
| 16 | + if (n < 2 || (int)y_.size() != n) throw std::runtime_error("CubicSpline: need at least two points and equal-sized x,y."); |
| 17 | + |
| 18 | + y2_.assign(n, 0.0); |
| 19 | + std::vector<double> u(n - 1, 0.0); |
| 20 | + |
| 21 | + // natural spline boundary conditions (second derivatives at endpoints = 0) |
| 22 | + for (int i = 1; i < n - 1; ++i) { |
| 23 | + double sig = (x_[i] - x_[i-1]) / (x_[i+1] - x_[i-1]); |
| 24 | + double p = sig * y2_[i-1] + 2.0; |
| 25 | + y2_[i] = (sig - 1.0) / p; |
| 26 | + double dY1 = (y_[i+1] - y_[i]) / (x_[i+1] - x_[i]); |
| 27 | + double dY0 = (y_[i] - y_[i-1]) / (x_[i] - x_[i-1]); |
| 28 | + u[i] = (6.0 * (dY1 - dY0) / (x_[i+1] - x_[i-1]) - sig * u[i-1]) / p; |
| 29 | + } |
| 30 | + |
| 31 | + for (int k = n - 2; k >= 0; --k) y2_[k] = y2_[k] * y2_[k+1] + u[k]; |
| 32 | + } |
| 33 | + |
| 34 | + int CubicSpline::locate(double xx) const { |
| 35 | + int n = (int)x_.size(); |
| 36 | + if (xx <= x_.front()) return 0; |
| 37 | + if (xx >= x_.back()) return n - 2; |
| 38 | + int lo = 0, hi = n - 1; |
| 39 | + while (hi - lo > 1) { |
| 40 | + int mid = (lo + hi) >> 1; |
| 41 | + if (x_[mid] > xx) hi = mid; else lo = mid; |
| 42 | + } |
| 43 | + return lo; |
| 44 | + } |
| 45 | + |
| 46 | + double CubicSpline::eval(double xx) const { |
| 47 | + int klo = locate(xx); |
| 48 | + int khi = klo + 1; |
| 49 | + double h = x_[khi] - x_[klo]; |
| 50 | + if (h <= 0.0) throw std::runtime_error("CubicSpline::eval: non-increasing x."); |
| 51 | + double A = (x_[khi] - xx) / h; |
| 52 | + double B = (xx - x_[klo]) / h; |
| 53 | + double val = A * y_[klo] + B * y_[khi] |
| 54 | + + ((A*A*A - A) * y2_[klo] + (B*B*B - B) * y2_[khi]) * (h*h) / 6.0; |
| 55 | + return val; |
| 56 | + } |
| 57 | + |
| 58 | + double CubicSpline::deriv(double xx) const { |
| 59 | + int klo = locate(xx); |
| 60 | + int khi = klo + 1; |
| 61 | + double h = x_[khi] - x_[klo]; |
| 62 | + if (h <= 0.0) throw std::runtime_error("CubicSpline::deriv: non-increasing x."); |
| 63 | + double A = (x_[khi] - xx) / h; |
| 64 | + double B = (xx - x_[klo]) / h; |
| 65 | + double dy = (y_[khi] - y_[klo]) / h |
| 66 | + + ( - (3.0*A*A - 1.0) * y2_[klo] + (3.0*B*B - 1.0) * y2_[khi] ) * (h / 6.0); |
| 67 | + return dy; |
| 68 | + } |
| 69 | + |
| 70 | + double CubicSpline::xmin() const { return x_.front(); } |
| 71 | + double CubicSpline::xmax() const { return x_.back(); } |
| 72 | + |
| 73 | +} |
0 commit comments