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 #ifdef __LIBRETRO__
25 #else
26 #include <cassert>
27 #include <limits>
28 #endif
29
30 namespace reSIDfp
31 {
32
Spline(const Point input[],size_t inputLength)33 Spline::Spline(const Point input[], size_t inputLength) :
34 params(inputLength),
35 c(¶ms[0])
36 {
37 assert(inputLength > 2);
38
39 const size_t coeffLength = inputLength - 1;
40
41 std::vector<double> dxs(coeffLength);
42 std::vector<double> ms(coeffLength);
43
44 // Get consecutive differences and slopes
45 for (size_t i = 0; i < coeffLength; i++)
46 {
47 assert(input[i].x < input[i + 1].x);
48
49 const double dx = input[i + 1].x - input[i].x;
50 const double dy = input[i + 1].y - input[i].y;
51 dxs[i] = dx;
52 ms[i] = dy/dx;
53 }
54
55 // Get degree-1 coefficients
56 params[0].c = ms[0];
57 for (size_t i = 1; i < coeffLength; i++)
58 {
59 const double m = ms[i - 1];
60 const double mNext = ms[i];
61 if (m * mNext <= 0)
62 {
63 params[i].c = 0.0;
64 }
65 else
66 {
67 const double dx = dxs[i - 1];
68 const double dxNext = dxs[i];
69 const double common = dx + dxNext;
70 params[i].c = 3.0 * common / ((common + dxNext) / m + (common + dx) / mNext);
71 }
72 }
73 params[coeffLength].c = ms[coeffLength - 1];
74
75 // Get degree-2 and degree-3 coefficients
76 for (size_t i = 0; i < coeffLength; i++)
77 {
78 params[i].x1 = input[i].x;
79 params[i].x2 = input[i + 1].x;
80 params[i].d = input[i].y;
81
82 const double c1 = params[i].c;
83 const double m = ms[i];
84 const double invDx = 1.0 / dxs[i];
85 const double common = c1 + params[i + 1].c - m - m;
86 params[i].b = (m - c1 - common) * invDx;
87 params[i].a = common * invDx * invDx;
88 }
89
90 // Fix the upper range, because we interpolate outside original bounds if necessary.
91 params[coeffLength - 1].x2 = std::numeric_limits<double>::max();
92 }
93
evaluate(double x) const94 Spline::Point Spline::evaluate(double x) const
95 {
96 if ((x < c->x1) || (x > c->x2))
97 {
98 for (size_t i = 0; i < params.size(); i++)
99 {
100 if (x <= params[i].x2)
101 {
102 c = ¶ms[i];
103 break;
104 }
105 }
106 }
107
108 // Interpolate
109 const double diff = x - c->x1;
110
111 Point out;
112
113 // y = a*x^3 + b*x^2 + c*x + d
114 out.x = ((c->a * diff + c->b) * diff + c->c) * diff + c->d;
115
116 // dy = 3*a*x^2 + 2*b*x + c
117 out.y = (3.0 * c->a * diff + 2.0 * c->b) * diff + c->c;
118
119 return out;
120 }
121
122 } // namespace reSIDfp
123