1 /*
2 * This file is part of libsidplayfp, a SID player engine.
3 *
4 * Copyright 2011-2015 Leandro Nini <drfiemost@users.sourceforge.net>
5 * Copyright 2007-2010 Antti Lankila
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 */
21
22 #include "Spline.h"
23
24 #include <cassert>
25 #include <limits>
26
27 namespace reSIDfp
28 {
29
Spline(const Point input[],size_t inputLength)30 Spline::Spline(const Point input[], size_t inputLength) :
31 params(inputLength),
32 c(¶ms[0])
33 {
34 assert(inputLength > 2);
35
36 const size_t coeffLength = inputLength - 1;
37
38 std::vector<double> dxs(coeffLength);
39 std::vector<double> ms(coeffLength);
40
41 // Get consecutive differences and slopes
42 for (size_t i = 0; i < coeffLength; i++)
43 {
44 assert(input[i].x < input[i + 1].x);
45
46 const double dx = input[i + 1].x - input[i].x;
47 const double dy = input[i + 1].y - input[i].y;
48 dxs[i] = dx;
49 ms[i] = dy/dx;
50 }
51
52 // Get degree-1 coefficients
53 params[0].c = ms[0];
54 for (size_t i = 1; i < coeffLength; i++)
55 {
56 const double m = ms[i - 1];
57 const double mNext = ms[i];
58 if (m * mNext <= 0)
59 {
60 params[i].c = 0.0;
61 }
62 else
63 {
64 const double dx = dxs[i - 1];
65 const double dxNext = dxs[i];
66 const double common = dx + dxNext;
67 params[i].c = 3.0 * common / ((common + dxNext) / m + (common + dx) / mNext);
68 }
69 }
70 params[coeffLength].c = ms[coeffLength - 1];
71
72 // Get degree-2 and degree-3 coefficients
73 for (size_t i = 0; i < coeffLength; i++)
74 {
75 params[i].x1 = input[i].x;
76 params[i].x2 = input[i + 1].x;
77 params[i].d = input[i].y;
78
79 const double c1 = params[i].c;
80 const double m = ms[i];
81 const double invDx = 1.0 / dxs[i];
82 const double common = c1 + params[i + 1].c - m - m;
83 params[i].b = (m - c1 - common) * invDx;
84 params[i].a = common * invDx * invDx;
85 }
86
87 // Fix the upper range, because we interpolate outside original bounds if necessary.
88 params[coeffLength - 1].x2 = std::numeric_limits<double>::max();
89 }
90
evaluate(double x) const91 Spline::Point Spline::evaluate(double x) const
92 {
93 if ((x < c->x1) || (x > c->x2))
94 {
95 for (size_t i = 0; i < params.size(); i++)
96 {
97 if (x <= params[i].x2)
98 {
99 c = ¶ms[i];
100 break;
101 }
102 }
103 }
104
105 // Interpolate
106 const double diff = x - c->x1;
107
108 Point out;
109
110 // y = a*x^3 + b*x^2 + c*x + d
111 out.x = ((c->a * diff + c->b) * diff + c->c) * diff + c->d;
112
113 // dy = 3*a*x^2 + 2*b*x + c
114 out.y = (3.0 * c->a * diff + 2.0 * c->b) * diff + c->c;
115
116 return out;
117 }
118
119 } // namespace reSIDfp
120