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(&params[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 = &params[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