simple_planner v1.4.0
Loading...
Searching...
No Matches
spline.h
Go to the documentation of this file.
1/*
2 * spline.h
3 *
4 * simple cubic spline interpolation library without external
5 * dependencies
6 *
7 * ---------------------------------------------------------------------
8 * Copyright (C) 2011, 2014, 2016, 2021 Tino Kluge (ttk448 at gmail.com)
9 *
10 * This program is free software; you can redistribute it and/or
11 * modify it under the terms of the GNU General Public License
12 * as published by the Free Software Foundation; either version 2
13 * of the License, or (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with this program. If not, see <http://www.gnu.org/licenses/>.
22 * ---------------------------------------------------------------------
23 *
24 */
25
26#ifndef TK_SPLINE_H
27#define TK_SPLINE_H
28
29#include <algorithm>
30#include <cassert>
31#include <cmath>
32#include <cstdio>
33#include <vector>
34#ifdef HAVE_SSTREAM
35#include <sstream>
36#include <string>
37#endif // HAVE_SSTREAM
38
39// not ideal but disable unused-function warnings
40// (we get them because we have implementations in the header file,
41// and this is because we want to be able to quickly separate them
42// into a cpp file if necessary)
43#pragma GCC diagnostic push
44#pragma GCC diagnostic ignored "-Wunused-function"
45
46// unnamed namespace only because the implementation is in this
47// header file and we don't want to export symbols to the obj files
48namespace {
49
50namespace tk {
51
52// spline interpolation
53class spline {
54 public:
55 // spline types
56 enum spline_type {
57 linear = 10, // linear interpolation
58 cspline = 30, // cubic splines (classical C^2)
59 cspline_hermite = 31 // cubic hermite splines (local, only C^1)
60 };
61
62 // boundary condition type for the spline end-points
63 enum bd_type { first_deriv = 1, second_deriv = 2, not_a_knot = 3 };
64
65 protected:
66 std::vector<double> m_x, m_y; // x,y coordinates of points
67 // interpolation parameters
68 // f(x) = a_i + b_i*(x-x_i) + c_i*(x-x_i)^2 + d_i*(x-x_i)^3
69 // where a_i = y_i, or else it won't go through grid points
70 std::vector<double> m_b, m_c, m_d; // spline coefficients
71 double m_c0; // for left extrapolation
72 spline_type m_type;
73 bd_type m_left, m_right;
74 double m_left_value, m_right_value;
75 bool m_made_monotonic;
76 void set_coeffs_from_b(); // calculate c_i, d_i from b_i
77 size_t find_closest(double x) const; // closest idx so that m_x[idx]<=x
78
79 public:
80 // default constructor: set boundary condition to be zero curvature
81 // at both ends, i.e. natural splines
82 spline()
83 : m_type(cspline),
84 m_left(second_deriv),
85 m_right(second_deriv),
86 m_left_value(0.0),
87 m_right_value(0.0),
88 m_made_monotonic(false) {
89 ;
90 }
91 spline(const std::vector<double>& X,
92 const std::vector<double>& Y,
93 spline_type type = cspline,
94 bool make_monotonic = false,
95 bd_type left = second_deriv,
96 double left_value = 0.0,
97 bd_type right = second_deriv,
98 double right_value = 0.0)
99 : m_type(type),
100 m_left(left),
101 m_right(right),
102 m_left_value(left_value),
103 m_right_value(right_value),
104 m_made_monotonic(false) // false correct here: make_monotonic() sets it
105 {
106 this->set_points(X, Y, m_type);
107 if (make_monotonic) {
108 this->make_monotonic();
109 }
110 }
111
112 // modify boundary conditions: if called it must be before set_points()
113 void set_boundary(bd_type left, double left_value, bd_type right, double right_value);
114
115 // set all data points (cubic_spline=false means linear interpolation)
116 void set_points(const std::vector<double>& x, const std::vector<double>& y, spline_type type = cspline);
117
118 // adjust coefficients so that the spline becomes piecewise monotonic
119 // where possible
120 // this is done by adjusting slopes at grid points by a non-negative
121 // factor and this will break C^2
122 // this can also break boundary conditions if adjustments need to
123 // be made at the boundary points
124 // returns false if no adjustments have been made, true otherwise
125 bool make_monotonic();
126
127 // evaluates the spline at point x
128 double operator()(double x) const;
129 double deriv(int order, double x) const;
130
131 // solves for all x so that: spline(x) = y
132 std::vector<double> solve(double y, bool ignore_extrapolation = true) const;
133
134 // returns the input data points
135 std::vector<double> get_x() const { return m_x; }
136 std::vector<double> get_y() const { return m_y; }
137 double get_x_min() const {
138 assert(!m_x.empty());
139 return m_x.front();
140 }
141 double get_x_max() const {
142 assert(!m_x.empty());
143 return m_x.back();
144 }
145
146#ifdef HAVE_SSTREAM
147 // spline info string, i.e. spline type, boundary conditions etc.
148 std::string info() const;
149#endif // HAVE_SSTREAM
150};
151
152namespace internal {
153
154// band matrix solver
155class band_matrix {
156 private:
157 std::vector<std::vector<double> > m_upper; // upper band
158 std::vector<std::vector<double> > m_lower; // lower band
159 public:
160 // clang-format off
161 band_matrix(){}; // constructor
162 band_matrix(int dim, int n_u, int n_l); // constructor
163 ~band_matrix(){}; // destructor
164 // clang-format on
165 void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l
166 int dim() const; // matrix dimension
167 int num_upper() const { return (int)m_upper.size() - 1; }
168 int num_lower() const { return (int)m_lower.size() - 1; }
169 // access operator
170 double& operator()(int i, int j); // write
171 double operator()(int i, int j) const; // read
172 // we can store an additional diagonal (in m_lower)
173 double& saved_diag(int i);
174 double saved_diag(int i) const;
175 void lu_decompose();
176 std::vector<double> r_solve(const std::vector<double>& b) const;
177 std::vector<double> l_solve(const std::vector<double>& b) const;
178 std::vector<double> lu_solve(const std::vector<double>& b, bool is_lu_decomposed = false);
179};
180
181double get_eps();
182
183std::vector<double> solve_cubic(double a, double b, double c, double d, int newton_iter = 0);
184
185} // namespace internal
186
187// ---------------------------------------------------------------------
188// implementation part, which could be separated into a cpp file
189// ---------------------------------------------------------------------
190
191// spline implementation
192// -----------------------
193
194void spline::set_boundary(spline::bd_type left, double left_value, spline::bd_type right, double right_value) {
195 assert(m_x.size() == 0); // set_points() must not have happened yet
196 m_left = left;
197 m_right = right;
198 m_left_value = left_value;
199 m_right_value = right_value;
200}
201
202void spline::set_coeffs_from_b() {
203 assert(m_x.size() == m_y.size());
204 assert(m_x.size() == m_b.size());
205 assert(m_x.size() > 2);
206 size_t n = m_b.size();
207 if (m_c.size() != n) m_c.resize(n);
208 if (m_d.size() != n) m_d.resize(n);
209
210 for (size_t i = 0; i < n - 1; i++) {
211 const double h = m_x[i + 1] - m_x[i];
212 // from continuity and differentiability condition
213 m_c[i] = (3.0 * (m_y[i + 1] - m_y[i]) / h - (2.0 * m_b[i] + m_b[i + 1])) / h;
214 // from differentiability condition
215 m_d[i] = ((m_b[i + 1] - m_b[i]) / (3.0 * h) - 2.0 / 3.0 * m_c[i]) / h;
216 }
217
218 // for left extrapolation coefficients
219 m_c0 = (m_left == first_deriv) ? 0.0 : m_c[0];
220}
221
222void spline::set_points(const std::vector<double>& x, const std::vector<double>& y, spline_type type) {
223 assert(x.size() == y.size());
224 assert(x.size() >= 3);
225 // not-a-knot with 3 points has many solutions
226 if (m_left == not_a_knot || m_right == not_a_knot) assert(x.size() >= 4);
227 m_type = type;
228 m_made_monotonic = false;
229 m_x = x;
230 m_y = y;
231 int n = (int)x.size();
232 // check strict monotonicity of input vector x
233 for (int i = 0; i < n - 1; i++) {
234 assert(m_x[i] < m_x[i + 1]);
235 }
236
237 if (type == linear) {
238 // linear interpolation
239 m_d.resize(n);
240 m_c.resize(n);
241 m_b.resize(n);
242 for (int i = 0; i < n - 1; i++) {
243 m_d[i] = 0.0;
244 m_c[i] = 0.0;
245 m_b[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]);
246 }
247 // ignore boundary conditions, set slope equal to the last segment
248 m_b[n - 1] = m_b[n - 2];
249 m_c[n - 1] = 0.0;
250 m_d[n - 1] = 0.0;
251 } else if (type == cspline) {
252 // classical cubic splines which are C^2 (twice cont differentiable)
253 // this requires solving an equation system
254
255 // setting up the matrix and right hand side of the equation system
256 // for the parameters b[]
257 int n_upper = (m_left == spline::not_a_knot) ? 2 : 1;
258 int n_lower = (m_right == spline::not_a_knot) ? 2 : 1;
259 internal::band_matrix A(n, n_upper, n_lower);
260 std::vector<double> rhs(n);
261 for (int i = 1; i < n - 1; i++) {
262 A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]);
263 A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]);
264 A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]);
265 rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
266 }
267 // boundary conditions
268 if (m_left == spline::second_deriv) {
269 // 2*c[0] = f''
270 A(0, 0) = 2.0;
271 A(0, 1) = 0.0;
272 rhs[0] = m_left_value;
273 } else if (m_left == spline::first_deriv) {
274 // b[0] = f', needs to be re-expressed in terms of c:
275 // (2c[0]+c[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f')
276 A(0, 0) = 2.0 * (x[1] - x[0]);
277 A(0, 1) = 1.0 * (x[1] - x[0]);
278 rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value);
279 } else if (m_left == spline::not_a_knot) {
280 // f'''(x[1]) exists, i.e. d[0]=d[1], or re-expressed in c:
281 // -h1*c[0] + (h0+h1)*c[1] - h0*c[2] = 0
282 A(0, 0) = -(x[2] - x[1]);
283 A(0, 1) = x[2] - x[0];
284 A(0, 2) = -(x[1] - x[0]);
285 rhs[0] = 0.0;
286 } else {
287 assert(false);
288 }
289 if (m_right == spline::second_deriv) {
290 // 2*c[n-1] = f''
291 A(n - 1, n - 1) = 2.0;
292 A(n - 1, n - 2) = 0.0;
293 rhs[n - 1] = m_right_value;
294 } else if (m_right == spline::first_deriv) {
295 // b[n-1] = f', needs to be re-expressed in terms of c:
296 // (c[n-2]+2c[n-1])(x[n-1]-x[n-2])
297 // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2]))
298 A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]);
299 A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]);
300 rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]));
301 } else if (m_right == spline::not_a_knot) {
302 // f'''(x[n-2]) exists, i.e. d[n-3]=d[n-2], or re-expressed in c:
303 // -h_{n-2}*c[n-3] + (h_{n-3}+h_{n-2})*c[n-2] - h_{n-3}*c[n-1] = 0
304 A(n - 1, n - 3) = -(x[n - 1] - x[n - 2]);
305 A(n - 1, n - 2) = x[n - 1] - x[n - 3];
306 A(n - 1, n - 1) = -(x[n - 2] - x[n - 3]);
307 rhs[0] = 0.0;
308 } else {
309 assert(false);
310 }
311
312 // solve the equation system to obtain the parameters c[]
313 m_c = A.lu_solve(rhs);
314
315 // calculate parameters b[] and d[] based on c[]
316 m_d.resize(n);
317 m_b.resize(n);
318 for (int i = 0; i < n - 1; i++) {
319 m_d[i] = 1.0 / 3.0 * (m_c[i + 1] - m_c[i]) / (x[i + 1] - x[i]);
320 m_b[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - 1.0 / 3.0 * (2.0 * m_c[i] + m_c[i + 1]) * (x[i + 1] - x[i]);
321 }
322 // for the right extrapolation coefficients (zero cubic term)
323 // f_{n-1}(x) = y_{n-1} + b*(x-x_{n-1}) + c*(x-x_{n-1})^2
324 double h = x[n - 1] - x[n - 2];
325 // m_c[n-1] is determined by the boundary condition
326 m_d[n - 1] = 0.0;
327 m_b[n - 1] = 3.0 * m_d[n - 2] * h * h + 2.0 * m_c[n - 2] * h + m_b[n - 2]; // = f'_{n-2}(x_{n-1})
328 if (m_right == first_deriv) m_c[n - 1] = 0.0; // force linear extrapolation
329
330 } else if (type == cspline_hermite) {
331 // hermite cubic splines which are C^1 (cont. differentiable)
332 // and derivatives are specified on each grid point
333 // (here we use 3-point finite differences)
334 m_b.resize(n);
335 m_c.resize(n);
336 m_d.resize(n);
337 // set b to match 1st order derivative finite difference
338 for (int i = 1; i < n - 1; i++) {
339 const double h = m_x[i + 1] - m_x[i];
340 const double hl = m_x[i] - m_x[i - 1];
341 m_b[i] = -h / (hl * (hl + h)) * m_y[i - 1] + (h - hl) / (hl * h) * m_y[i] + hl / (h * (hl + h)) * m_y[i + 1];
342 }
343 // boundary conditions determine b[0] and b[n-1]
344 if (m_left == first_deriv) {
345 m_b[0] = m_left_value;
346 } else if (m_left == second_deriv) {
347 const double h = m_x[1] - m_x[0];
348 m_b[0] = 0.5 * (-m_b[1] - 0.5 * m_left_value * h + 3.0 * (m_y[1] - m_y[0]) / h);
349 } else if (m_left == not_a_knot) {
350 // f''' continuous at x[1]
351 const double h0 = m_x[1] - m_x[0];
352 const double h1 = m_x[2] - m_x[1];
353 m_b[0] = -m_b[1] + 2.0 * (m_y[1] - m_y[0]) / h0 + h0 * h0 / (h1 * h1) * (m_b[1] + m_b[2] - 2.0 * (m_y[2] - m_y[1]) / h1);
354 } else {
355 assert(false);
356 }
357 if (m_right == first_deriv) {
358 m_b[n - 1] = m_right_value;
359 m_c[n - 1] = 0.0;
360 } else if (m_right == second_deriv) {
361 const double h = m_x[n - 1] - m_x[n - 2];
362 m_b[n - 1] = 0.5 * (-m_b[n - 2] + 0.5 * m_right_value * h + 3.0 * (m_y[n - 1] - m_y[n - 2]) / h);
363 m_c[n - 1] = 0.5 * m_right_value;
364 } else if (m_right == not_a_knot) {
365 // f''' continuous at x[n-2]
366 const double h0 = m_x[n - 2] - m_x[n - 3];
367 const double h1 = m_x[n - 1] - m_x[n - 2];
368 m_b[n - 1] = -m_b[n - 2] + 2.0 * (m_y[n - 1] - m_y[n - 2]) / h1 +
369 h1 * h1 / (h0 * h0) * (m_b[n - 3] + m_b[n - 2] - 2.0 * (m_y[n - 2] - m_y[n - 3]) / h0);
370 // f'' continuous at x[n-1]: c[n-1] = 3*d[n-2]*h[n-2] + c[n-1]
371 m_c[n - 1] = (m_b[n - 2] + 2.0 * m_b[n - 1]) / h1 - 3.0 * (m_y[n - 1] - m_y[n - 2]) / (h1 * h1);
372 } else {
373 assert(false);
374 }
375 m_d[n - 1] = 0.0;
376
377 // parameters c and d are determined by continuity and differentiability
378 set_coeffs_from_b();
379
380 } else {
381 assert(false);
382 }
383
384 // for left extrapolation coefficients
385 m_c0 = (m_left == first_deriv) ? 0.0 : m_c[0];
386}
387
388bool spline::make_monotonic() {
389 assert(m_x.size() == m_y.size());
390 assert(m_x.size() == m_b.size());
391 assert(m_x.size() > 2);
392 bool modified = false;
393 const int n = (int)m_x.size();
394 // make sure: input data monotonic increasing --> b_i>=0
395 // input data monotonic decreasing --> b_i<=0
396 for (int i = 0; i < n; i++) {
397 int im1 = std::max(i - 1, 0);
398 int ip1 = std::min(i + 1, n - 1);
399 if (((m_y[im1] <= m_y[i]) && (m_y[i] <= m_y[ip1]) && m_b[i] < 0.0) ||
400 ((m_y[im1] >= m_y[i]) && (m_y[i] >= m_y[ip1]) && m_b[i] > 0.0)) {
401 modified = true;
402 m_b[i] = 0.0;
403 }
404 }
405 // if input data is monotonic (b[i], b[i+1], avg have all the same sign)
406 // ensure a sufficient criteria for monotonicity is satisfied:
407 // sqrt(b[i]^2+b[i+1]^2) <= 3 |avg|, with avg=(y[i+1]-y[i])/h,
408 for (int i = 0; i < n - 1; i++) {
409 double h = m_x[i + 1] - m_x[i];
410 double avg = (m_y[i + 1] - m_y[i]) / h;
411 if (avg == 0.0 && (m_b[i] != 0.0 || m_b[i + 1] != 0.0)) {
412 modified = true;
413 m_b[i] = 0.0;
414 m_b[i + 1] = 0.0;
415 } else if ((m_b[i] >= 0.0 && m_b[i + 1] >= 0.0 && avg > 0.0) || (m_b[i] <= 0.0 && m_b[i + 1] <= 0.0 && avg < 0.0)) {
416 // input data is monotonic
417 double r = sqrt(m_b[i] * m_b[i] + m_b[i + 1] * m_b[i + 1]) / std::fabs(avg);
418 if (r > 3.0) {
419 // sufficient criteria for monotonicity: r<=3
420 // adjust b[i] and b[i+1]
421 modified = true;
422 m_b[i] *= (3.0 / r);
423 m_b[i + 1] *= (3.0 / r);
424 }
425 }
426 }
427
428 if (modified == true) {
429 set_coeffs_from_b();
430 m_made_monotonic = true;
431 }
432
433 return modified;
434}
435
436// return the closest idx so that m_x[idx] <= x (return 0 if x<m_x[0])
437size_t spline::find_closest(double x) const {
438 std::vector<double>::const_iterator it;
439 it = std::upper_bound(m_x.begin(), m_x.end(), x); // *it > x
440 size_t idx = std::max(int(it - m_x.begin()) - 1, 0); // m_x[idx] <= x
441 return idx;
442}
443
444double spline::operator()(double x) const {
445 // polynomial evaluation using Horner's scheme
446 // TODO: consider more numerically accurate algorithms, e.g.:
447 // - Clenshaw
448 // - Even-Odd method by A.C.R. Newbery
449 // - Compensated Horner Scheme
450 size_t n = m_x.size();
451 size_t idx = find_closest(x);
452
453 double h = x - m_x[idx];
454 double interpol;
455 if (x < m_x[0]) {
456 // extrapolation to the left
457 interpol = (m_c0 * h + m_b[0]) * h + m_y[0];
458 } else if (x > m_x[n - 1]) {
459 // extrapolation to the right
460 interpol = (m_c[n - 1] * h + m_b[n - 1]) * h + m_y[n - 1];
461 } else {
462 // interpolation
463 interpol = ((m_d[idx] * h + m_c[idx]) * h + m_b[idx]) * h + m_y[idx];
464 }
465 return interpol;
466}
467
468double spline::deriv(int order, double x) const {
469 assert(order > 0);
470 size_t n = m_x.size();
471 size_t idx = find_closest(x);
472
473 double h = x - m_x[idx];
474 double interpol;
475 if (x < m_x[0]) {
476 // extrapolation to the left
477 switch (order) {
478 case 1:
479 interpol = 2.0 * m_c0 * h + m_b[0];
480 break;
481 case 2:
482 interpol = 2.0 * m_c0;
483 break;
484 default:
485 interpol = 0.0;
486 break;
487 }
488 } else if (x > m_x[n - 1]) {
489 // extrapolation to the right
490 switch (order) {
491 case 1:
492 interpol = 2.0 * m_c[n - 1] * h + m_b[n - 1];
493 break;
494 case 2:
495 interpol = 2.0 * m_c[n - 1];
496 break;
497 default:
498 interpol = 0.0;
499 break;
500 }
501 } else {
502 // interpolation
503 switch (order) {
504 case 1:
505 interpol = (3.0 * m_d[idx] * h + 2.0 * m_c[idx]) * h + m_b[idx];
506 break;
507 case 2:
508 interpol = 6.0 * m_d[idx] * h + 2.0 * m_c[idx];
509 break;
510 case 3:
511 interpol = 6.0 * m_d[idx];
512 break;
513 default:
514 interpol = 0.0;
515 break;
516 }
517 }
518 return interpol;
519}
520
521std::vector<double> spline::solve(double y, bool ignore_extrapolation) const {
522 std::vector<double> x; // roots for the entire spline
523 std::vector<double> root; // roots for each piecewise cubic
524 const size_t n = m_x.size();
525
526 // left extrapolation
527 if (ignore_extrapolation == false) {
528 root = internal::solve_cubic(m_y[0] - y, m_b[0], m_c0, 0.0, 1);
529 for (size_t j = 0; j < root.size(); j++) {
530 if (root[j] < 0.0) {
531 x.push_back(m_x[0] + root[j]);
532 }
533 }
534 }
535
536 // brute force check if piecewise cubic has roots in their resp. segment
537 // TODO: make more efficient
538 for (size_t i = 0; i < n - 1; i++) {
539 root = internal::solve_cubic(m_y[i] - y, m_b[i], m_c[i], m_d[i], 1);
540 for (size_t j = 0; j < root.size(); j++) {
541 double h = (i > 0) ? (m_x[i] - m_x[i - 1]) : 0.0;
542 double eps = internal::get_eps() * 512.0 * std::min(h, 1.0);
543 if ((-eps <= root[j]) && (root[j] < m_x[i + 1] - m_x[i])) {
544 double new_root = m_x[i] + root[j];
545 if (x.size() > 0 && x.back() + eps > new_root) {
546 x.back() = new_root; // avoid spurious duplicate roots
547 } else {
548 x.push_back(new_root);
549 }
550 }
551 }
552 }
553
554 // right extrapolation
555 if (ignore_extrapolation == false) {
556 root = internal::solve_cubic(m_y[n - 1] - y, m_b[n - 1], m_c[n - 1], 0.0, 1);
557 for (size_t j = 0; j < root.size(); j++) {
558 if (0.0 <= root[j]) {
559 x.push_back(m_x[n - 1] + root[j]);
560 }
561 }
562 }
563
564 return x;
565};
566
567#ifdef HAVE_SSTREAM
568std::string spline::info() const {
569 std::stringstream ss;
570 ss << "type " << m_type << ", left boundary deriv " << m_left << " = ";
571 ss << m_left_value << ", right boundary deriv " << m_right << " = ";
572 ss << m_right_value << std::endl;
573 if (m_made_monotonic) {
574 ss << "(spline has been adjusted for piece-wise monotonicity)";
575 }
576 return ss.str();
577}
578#endif // HAVE_SSTREAM
579
580namespace internal {
581
582// band_matrix implementation
583// -------------------------
584
585band_matrix::band_matrix(int dim, int n_u, int n_l) { resize(dim, n_u, n_l); }
586void band_matrix::resize(int dim, int n_u, int n_l) {
587 assert(dim > 0);
588 assert(n_u >= 0);
589 assert(n_l >= 0);
590 m_upper.resize(n_u + 1);
591 m_lower.resize(n_l + 1);
592 for (size_t i = 0; i < m_upper.size(); i++) {
593 m_upper[i].resize(dim);
594 }
595 for (size_t i = 0; i < m_lower.size(); i++) {
596 m_lower[i].resize(dim);
597 }
598}
599int band_matrix::dim() const {
600 if (m_upper.size() > 0) {
601 return m_upper[0].size();
602 } else {
603 return 0;
604 }
605}
606
607// defines the new operator (), so that we can access the elements
608// by A(i,j), index going from i=0,...,dim()-1
609double& band_matrix::operator()(int i, int j) {
610 int k = j - i; // what band is the entry
611 assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
612 assert((-num_lower() <= k) && (k <= num_upper()));
613 // k=0 -> diagonal, k<0 lower left part, k>0 upper right part
614 if (k >= 0)
615 return m_upper[k][i];
616 else
617 return m_lower[-k][i];
618}
619double band_matrix::operator()(int i, int j) const {
620 int k = j - i; // what band is the entry
621 assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim()));
622 assert((-num_lower() <= k) && (k <= num_upper()));
623 // k=0 -> diagonal, k<0 lower left part, k>0 upper right part
624 if (k >= 0)
625 return m_upper[k][i];
626 else
627 return m_lower[-k][i];
628}
629// second diag (used in LU decomposition), saved in m_lower
630double band_matrix::saved_diag(int i) const {
631 assert((i >= 0) && (i < dim()));
632 return m_lower[0][i];
633}
634double& band_matrix::saved_diag(int i) {
635 assert((i >= 0) && (i < dim()));
636 return m_lower[0][i];
637}
638
639// LR-Decomposition of a band matrix
640void band_matrix::lu_decompose() {
641 int i_max, j_max;
642 int j_min;
643 double x;
644
645 // preconditioning
646 // normalize column i so that a_ii=1
647 for (int i = 0; i < this->dim(); i++) {
648 assert(this->operator()(i, i) != 0.0);
649 this->saved_diag(i) = 1.0 / this->operator()(i, i);
650 j_min = std::max(0, i - this->num_lower());
651 j_max = std::min(this->dim() - 1, i + this->num_upper());
652 for (int j = j_min; j <= j_max; j++) {
653 this->operator()(i, j) *= this->saved_diag(i);
654 }
655 this->operator()(i, i) = 1.0; // prevents rounding errors
656 }
657
658 // Gauss LR-Decomposition
659 for (int k = 0; k < this->dim(); k++) {
660 i_max = std::min(this->dim() - 1, k + this->num_lower()); // num_lower not a mistake!
661 for (int i = k + 1; i <= i_max; i++) {
662 assert(this->operator()(k, k) != 0.0);
663 x = -this->operator()(i, k) / this->operator()(k, k);
664 this->operator()(i, k) = -x; // assembly part of L
665 j_max = std::min(this->dim() - 1, k + this->num_upper());
666 for (int j = k + 1; j <= j_max; j++) {
667 // assembly part of R
668 this->operator()(i, j) = this->operator()(i, j) + x * this->operator()(k, j);
669 }
670 }
671 }
672}
673// solves Ly=b
674std::vector<double> band_matrix::l_solve(const std::vector<double>& b) const {
675 assert(this->dim() == (int)b.size());
676 std::vector<double> x(this->dim());
677 int j_start;
678 double sum;
679 for (int i = 0; i < this->dim(); i++) {
680 sum = 0;
681 j_start = std::max(0, i - this->num_lower());
682 for (int j = j_start; j < i; j++) sum += this->operator()(i, j) * x[j];
683 x[i] = (b[i] * this->saved_diag(i)) - sum;
684 }
685 return x;
686}
687// solves Rx=y
688std::vector<double> band_matrix::r_solve(const std::vector<double>& b) const {
689 assert(this->dim() == (int)b.size());
690 std::vector<double> x(this->dim());
691 int j_stop;
692 double sum;
693 for (int i = this->dim() - 1; i >= 0; i--) {
694 sum = 0;
695 j_stop = std::min(this->dim() - 1, i + this->num_upper());
696 for (int j = i + 1; j <= j_stop; j++) sum += this->operator()(i, j) * x[j];
697 x[i] = (b[i] - sum) / this->operator()(i, i);
698 }
699 return x;
700}
701
702std::vector<double> band_matrix::lu_solve(const std::vector<double>& b, bool is_lu_decomposed) {
703 assert(this->dim() == (int)b.size());
704 std::vector<double> x, y;
705 if (is_lu_decomposed == false) {
706 this->lu_decompose();
707 }
708 y = this->l_solve(b);
709 x = this->r_solve(y);
710 return x;
711}
712
713// machine precision of a double, i.e. the successor of 1 is 1+eps
714double get_eps() {
715 //return std::numeric_limits<double>::epsilon(); // __DBL_EPSILON__
716 return 2.2204460492503131e-16; // 2^-52
717}
718
719// solutions for a + b*x = 0
720std::vector<double> solve_linear(double a, double b) {
721 std::vector<double> x; // roots
722 if (b == 0.0) {
723 if (a == 0.0) {
724 // 0*x = 0
725 x.resize(1);
726 x[0] = 0.0; // any x solves it but we need to pick one
727 return x;
728 } else {
729 // 0*x + ... = 0, no solution
730 return x;
731 }
732 } else {
733 x.resize(1);
734 x[0] = -a / b;
735 return x;
736 }
737}
738
739// solutions for a + b*x + c*x^2 = 0
740std::vector<double> solve_quadratic(double a, double b, double c, int newton_iter = 0) {
741 if (c == 0.0) {
742 return solve_linear(a, b);
743 }
744 // rescale so that we solve x^2 + 2p x + q = (x+p)^2 + q - p^2 = 0
745 double p = 0.5 * b / c;
746 double q = a / c;
747 double discr = p * p - q;
748 const double eps = 0.5 * internal::get_eps();
749 double discr_err = (6.0 * (p * p) + 3.0 * fabs(q) + fabs(discr)) * eps;
750
751 std::vector<double> x; // roots
752 if (fabs(discr) <= discr_err) {
753 // discriminant is zero --> one root
754 x.resize(1);
755 x[0] = -p;
756 } else if (discr < 0) {
757 // no root
758 } else {
759 // two roots
760 x.resize(2);
761 x[0] = -p - sqrt(discr);
762 x[1] = -p + sqrt(discr);
763 }
764
765 // improve solution via newton steps
766 for (size_t i = 0; i < x.size(); i++) {
767 for (int k = 0; k < newton_iter; k++) {
768 double f = (c * x[i] + b) * x[i] + a;
769 double f1 = 2.0 * c * x[i] + b;
770 // only adjust if slope is large enough
771 if (fabs(f1) > 1e-8) {
772 x[i] -= f / f1;
773 }
774 }
775 }
776
777 return x;
778}
779
780// solutions for the cubic equation: a + b*x +c*x^2 + d*x^3 = 0
781// this is a naive implementation of the analytic solution without
782// optimisation for speed or numerical accuracy
783// newton_iter: number of newton iterations to improve analytical solution
784// see also
785// gsl: gsl_poly_solve_cubic() in solve_cubic.c
786// octave: roots.m - via eigenvalues of the Frobenius companion matrix
787std::vector<double> solve_cubic(double a, double b, double c, double d, int newton_iter) {
788 if (d == 0.0) {
789 return solve_quadratic(a, b, c, newton_iter);
790 }
791
792 // convert to normalised form: a + bx + cx^2 + x^3 = 0
793 if (d != 1.0) {
794 a /= d;
795 b /= d;
796 c /= d;
797 }
798
799 // convert to depressed cubic: z^3 - 3pz - 2q = 0
800 // via substitution: z = x + c/3
801 std::vector<double> z; // roots of the depressed cubic
802 double p = -(1.0 / 3.0) * b + (1.0 / 9.0) * (c * c);
803 double r = 2.0 * (c * c) - 9.0 * b;
804 double q = -0.5 * a - (1.0 / 54.0) * (c * r);
805 double discr = p * p * p - q * q; // discriminant
806 // calculating numerical round-off errors with assumptions:
807 // - each operation is precise but each intermediate result x
808 // when stored has max error of x*eps
809 // - only multiplication with a power of 2 introduces no new error
810 // - a,b,c,d and some fractions (e.g. 1/3) have rounding errors eps
811 // - p_err << |p|, q_err << |q|, ... (this is violated in rare cases)
812 // would be more elegant to use boost::numeric::interval<double>
813 const double eps = internal::get_eps();
814 double p_err = eps * ((3.0 / 3.0) * fabs(b) + (4.0 / 9.0) * (c * c) + fabs(p));
815 double r_err = eps * (6.0 * (c * c) + 18.0 * fabs(b) + fabs(r));
816 double q_err = 0.5 * fabs(a) * eps + (1.0 / 54.0) * fabs(c) * (r_err + fabs(r) * 3.0 * eps) + fabs(q) * eps;
817 double discr_err = (p * p) * (3.0 * p_err + fabs(p) * 2.0 * eps) + fabs(q) * (2.0 * q_err + fabs(q) * eps) + fabs(discr) * eps;
818
819 // depending on the discriminant we get different solutions
820 if (fabs(discr) <= discr_err) {
821 // discriminant zero: one or two real roots
822 if (fabs(p) <= p_err) {
823 // p and q are zero: single root
824 z.resize(1);
825 z[0] = 0.0; // triple root
826 } else {
827 z.resize(2);
828 z[0] = 2.0 * q / p; // single root
829 z[1] = -0.5 * z[0]; // double root
830 }
831 } else if (discr > 0) {
832 // three real roots: via trigonometric solution
833 z.resize(3);
834 double ac = (1.0 / 3.0) * acos(q / (p * sqrt(p)));
835 double sq = 2.0 * sqrt(p);
836 z[0] = sq * cos(ac);
837 z[1] = sq * cos(ac - 2.0 * M_PI / 3.0);
838 z[2] = sq * cos(ac - 4.0 * M_PI / 3.0);
839 } else if (discr < 0.0) {
840 // single real root: via Cardano's fromula
841 z.resize(1);
842 double sgnq = (q >= 0 ? 1 : -1);
843 double basis = fabs(q) + sqrt(-discr);
844 double C = sgnq * pow(basis, 1.0 / 3.0); // c++11 has std::cbrt()
845 z[0] = C + p / C;
846 }
847 for (size_t i = 0; i < z.size(); i++) {
848 // convert depressed cubic roots to original cubic: x = z - c/3
849 z[i] -= (1.0 / 3.0) * c;
850 // improve solution via newton steps
851 for (int k = 0; k < newton_iter; k++) {
852 double f = ((z[i] + c) * z[i] + b) * z[i] + a;
853 double f1 = (3.0 * z[i] + 2.0 * c) * z[i] + b;
854 // only adjust if slope is large enough
855 if (fabs(f1) > 1e-8) {
856 z[i] -= f / f1;
857 }
858 }
859 }
860 // ensure if a=0 we get exactly x=0 as root
861 // TODO: remove this fudge
862 if (a == 0.0) {
863 assert(z.size() > 0); // cubic should always have at least one root
864 double xmin = fabs(z[0]);
865 size_t imin = 0;
866 for (size_t i = 1; i < z.size(); i++) {
867 if (xmin > fabs(z[i])) {
868 xmin = fabs(z[i]);
869 imin = i;
870 }
871 }
872 z[imin] = 0.0; // replace the smallest absolute value with 0
873 }
874 std::sort(z.begin(), z.end());
875 return z;
876}
877
878} // namespace internal
879
880} // namespace tk
881
882} // namespace
883
884#pragma GCC diagnostic pop
885
886#endif /* TK_SPLINE_H */
tk
Definition spline.h:50