Alexandria  2.27.0
SDC-CH common library for the Euclid project
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
spline.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2022 Euclid Science Ground Segment
3  *
4  * This library is free software; you can redistribute it and/or modify it under
5  * the terms of the GNU Lesser General Public License as published by the Free
6  * Software Foundation; either version 3.0 of the License, or (at your option)
7  * any later version.
8  *
9  * This library is distributed in the hope that it will be useful, but WITHOUT
10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
11  * FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
12  * details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this library; if not, write to the Free Software Foundation, Inc.,
16  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
17  */
18 
28 #include <limits>
29 
30 namespace Euclid {
31 namespace MathUtils {
32 
33 class CubicInterpolator final : public PiecewiseBase {
34 public:
37  : PiecewiseBase(std::move(knots))
38  , m_coef0(std::move(coef0))
39  , m_coef1(std::move(coef1))
40  , m_coef2(std::move(coef2))
41  , m_coef3(std::move(coef3)) {}
42 
43  virtual ~CubicInterpolator() = default;
44 
45  double operator()(double x) const override {
46  auto i = findKnot(x);
47  if (i < 0 || i >= static_cast<ssize_t>(m_knots.size())) {
48  return 0.;
49  }
50  if (i == 0) {
51  return m_coef3.front() * x * x * x + m_coef2.front() * x * x + m_coef1.front() * x + m_coef0.front();
52  }
53  --i;
54  return m_coef3[i] * x * x * x + m_coef2[i] * x * x + m_coef1[i] * x + m_coef0[i];
55  }
56 
57  void operator()(const std::vector<double>& xs, std::vector<double>& out) const override {
58  out.resize(xs.size());
59  // Find the first X that is within the range
60  auto first_x = std::lower_bound(xs.begin(), xs.end(), m_knots.front());
61  auto n_less = first_x - xs.begin();
62 
63  // Opening 0s
64  auto o = out.begin() + n_less;
65  std::fill(out.begin(), o, 0.);
66 
67  // To avoid if within the loop, treat values exactly equal to the first knot here
68  auto x = xs.begin() + n_less;
69  while (x < xs.end() && *x == m_knots.front()) {
70  auto x2 = *x * *x;
71  *o = m_coef3.front() * x2 * *x + m_coef2.front() * x2 + m_coef1.front() * *x + m_coef0.front();
72  ++o, ++x;
73  }
74  if (x == xs.end()) {
75  return;
76  }
77 
78  // Interpolate values within range
79  auto current_knot = std::lower_bound(m_knots.begin(), m_knots.end(), *x);
80  while (o != out.end() && current_knot < m_knots.end()) {
81  auto i = current_knot - m_knots.begin();
82  auto x2 = *x * *x;
83  --i;
84  *o = m_coef3[i] * x2 * *x + m_coef2[i] * x2 + m_coef1[i] * *x + m_coef0[i];
85  ++o, ++x;
86  current_knot = std::find_if(current_knot, m_knots.end(), [x](double k) { return k >= *x; });
87  }
88 
89  // Trailing 0s
90  std::fill(o, out.end(), 0.);
91  }
92 
95  }
96 
97  double integrate(const double a, const double b) const override {
98  if (a == b) {
99  return 0;
100  }
101  int direction = 1;
102  double min = a;
103  double max = b;
104  if (min > max) {
105  direction = -1;
106  min = b;
107  max = a;
108  }
109  double result = 0;
110  auto knotIter = std::upper_bound(m_knots.begin(), m_knots.end(), min);
111  if (knotIter != m_knots.begin()) {
112  --knotIter;
113  }
114  auto i = knotIter - m_knots.begin();
115  while (++knotIter != m_knots.end()) {
116  auto prevKnotIter = knotIter - 1;
117  if (max <= *prevKnotIter) {
118  break;
119  }
120  if (min < *knotIter) {
121  double down = (min > *prevKnotIter) ? min : *prevKnotIter;
122  double up = (max < *knotIter) ? max : *knotIter;
123  result += antiderivative(i, up) - antiderivative(i, down);
124  }
125  ++i;
126  }
127  return direction * result;
128  }
129 
130 private:
132 
133  double antiderivative(int i, double x) const {
134  double x2 = x * x;
135  return m_coef0[i] * x + m_coef1[i] * x2 / 2. + m_coef2[i] * x2 * x / 3. + m_coef3[i] * x2 * x2 / 4.;
136  }
137 };
138 
140  bool extrapolate) {
141  std::vector<double> knots(x);
142 
143  // Number of intervals
144  int n = x.size() - 1;
145 
146  // Differences between knot points
147  std::vector<double> h(n, 0.);
148  for (int i = 0; i < n; i++)
149  h[i] = x[i + 1] - x[i];
150 
151  std::vector<double> mu(n, 0.);
152  std::vector<double> z(n + 1, 0.);
153  for (int i = 1; i < n; ++i) {
154  double g = 2. * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
155  mu[i] = h[i] / g;
156  z[i] = (3. * (y[i + 1] * h[i - 1] - y[i] * (x[i + 1] - x[i - 1]) + y[i - 1] * h[i]) / (h[i - 1] * h[i]) -
157  h[i - 1] * z[i - 1]) /
158  g;
159  }
160 
161  // cubic spline coefficients
162  std::vector<double> coef0(n, 0.);
163  std::vector<double> coef1(n, 0.);
164  std::vector<double> coef2(n + 1, 0.);
165  std::vector<double> coef3(n, 0.);
166 
167  z[n] = 0.;
168  coef2[n] = 0.;
169 
170  for (int j = n - 1; j >= 0; j--) {
171  coef0[j] = y[j];
172  coef2[j] = z[j] - mu[j] * coef2[j + 1];
173  coef1[j] = (y[j + 1] - y[j]) / h[j] - h[j] * (coef2[j + 1] + 2. * coef2[j]) / 3.;
174  coef3[j] = (coef2[j + 1] - coef2[j]) / (3. * h[j]);
175  }
176 
177  // The above were taken from SplineInterpolator from Apache commons math. These
178  // polynomials need to be shifted by -x[i] in our case.
179  for (int i = 0; i < n; i++) {
180  double x_1 = -x[i];
181  double x_2 = x_1 * x_1;
182  double x_3 = x_1 * x_2;
183  coef0[i] = coef0[i] + coef1[i] * x_1 + coef2[i] * x_2 + coef3[i] * x_3;
184  coef1[i] = coef1[i] + 2. * coef2[i] * x_1 + 3. * coef3[i] * x_2;
185  coef2[i] = coef2[i] + 3. * coef3[i] * x_1;
186  // d[i] keeps the same value
187  }
188 
189  if (extrapolate) {
192  }
193 
195  new CubicInterpolator(std::move(knots), std::move(coef0), std::move(coef1), std::move(coef2), std::move(coef3)));
196 }
197 
198 } // namespace MathUtils
199 } // end of namespace Euclid
CubicInterpolator(std::vector< double > knots, std::vector< double > coef0, std::vector< double > coef1, std::vector< double > coef2, std::vector< double > coef3)
Definition: spline.cpp:35
ssize_t findKnot(double x) const
Definition: Piecewise.h:60
std::unique_ptr< NAryFunction > clone() const override
Definition: spline.cpp:93
std::vector< double > m_coef3
Definition: spline.cpp:131
T front(T...args)
T upper_bound(T...args)
std::vector< double > m_coef0
Definition: spline.cpp:131
T end(T...args)
T lower_bound(T...args)
double operator()(double x) const override
Definition: spline.cpp:45
T resize(T...args)
T lowest(T...args)
std::vector< double > m_coef1
Definition: spline.cpp:131
T move(T...args)
std::unique_ptr< Function > splineInterpolation(const std::vector< double > &x, const std::vector< double > &y, bool extrapolate)
Performs cubic spline interpolation for the given set of data points.
Definition: spline.cpp:139
T find_if(T...args)
T size(T...args)
std::vector< double > m_coef2
Definition: spline.cpp:131
STL class.
T begin(T...args)
constexpr double g
T back(T...args)
std::vector< double > m_knots
A vector where the knots are kept.
Definition: Piecewise.h:74
double antiderivative(int i, double x) const
Definition: spline.cpp:133
void operator()(const std::vector< double > &xs, std::vector< double > &out) const override
Definition: spline.cpp:57
T fill(T...args)
Represents a piecewise function.
Definition: Piecewise.h:48
double integrate(const double a, const double b) const override
Definition: spline.cpp:97