1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
|
/*****************************************************************************
** This is part of the CTSim program
** Copyright (c) 1983-2009 Kevin Rosenberg
**
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU General Public License (version 2) as
** published by the Free Software Foundation.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program; if not, write to the Free Software
** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
******************************************************************************/
#include "ctsupport.h"
#include "interpolator.h"
CubicPolyInterpolator::CubicPolyInterpolator (const double* const y, const int n)
: m_pdY(y), m_n(n)
{
if (m_n < 2)
sys_error (ERR_SEVERE, "Too few points (%d) in CubicPolyInterpolator", m_n);
}
CubicPolyInterpolator::~CubicPolyInterpolator ()
{
}
double
CubicPolyInterpolator::interpolate (double x)
{
int lo = static_cast<int>(floor(x)) - 1;
int hi = lo + 3;
if (lo < -1) {
#ifdef DEBUG
sys_error (ERR_WARNING, "x=%f, out of range [CubicPolyInterpolator]", x);
#endif
return (0);
} else if (lo == -1) // linear interpolate at between x = 0 & 1
return m_pdY[0] + x * (m_pdY[1] - m_pdY[0]);
if (hi > m_n) {
#ifdef DEBUG
sys_error (ERR_WARNING, "x=%f, out of range [CubicPolyInterpolator]", x);
#endif
return (0);
} else if (hi == m_n) {// linear interpolate between x = (n-2) and (n-1)
double frac = x - (lo + 1);
return m_pdY[m_n - 2] + frac * (m_pdY[m_n - 1] - m_pdY[m_n - 2]);
}
// Lagrange formula for N=4 (cubic)
double xd_0 = x - lo;
double xd_1 = x - (lo + 1);
double xd_2 = x - (lo + 2);
double xd_3 = x - (lo + 3);
static double oneSixth = (1. / 6.);
double y = xd_1 * xd_2 * xd_3 * -oneSixth * m_pdY[lo];
y += xd_0 * xd_2 * xd_3 * 0.5 * m_pdY[lo+1];
y += xd_0 * xd_1 * xd_3 * -0.5 * m_pdY[lo+2];
y += xd_0 * xd_1 * xd_2 * oneSixth * m_pdY[lo+3];
return (y);
}
CubicSplineInterpolator::CubicSplineInterpolator (const double* const y, const int n)
: m_pdY(y), m_n(n)
{
// Precalculate 2nd derivative of y and put in m_pdY2
// Calculated by solving set of simultaneous CubicSpline spline equations
// Only n-2 CubicSpline spline equations, but able to make two more
// equations by setting second derivative to 0 at ends
m_pdY2 = new double [n];
m_pdY2[0] = 0; // second deriviative = 0 at beginning and end
m_pdY2[n-1] = 0;
double* temp = new double [n - 1];
temp[0] = 0;
int i;
for (i = 1; i < n - 1; i++) {
double t = 2 + (0.5 * m_pdY2[i-1]);
temp[i] = y[i+1] + y[i-1] - y[i] - y[i];
temp[i] = (3 * temp[i] - 0.5 * temp[i-1]) / t;
m_pdY2[i] = -0.5 / t;
}
for (i = n - 2; i >= 0; i--)
m_pdY2[i] = temp[i] + m_pdY2[i] * m_pdY2[i + 1];
delete temp;
}
CubicSplineInterpolator::~CubicSplineInterpolator ()
{
delete m_pdY2;
}
double
CubicSplineInterpolator::interpolate (double x)
{
const static double oneSixth = (1. / 6.);
int lo = static_cast<int>(floor(x));
int hi = lo + 1;
if (lo < 0 || hi >= m_n) {
#ifdef DEBUG
sys_error (ERR_SEVERE, "x out of bounds [CubicSplineInterpolator::interpolate]");
#endif
return (0);
}
double loFr = hi - x;
double hiFr = 1 - loFr;
double y = loFr * m_pdY[lo] + hiFr * m_pdY[hi];
y += oneSixth * ((loFr*loFr*loFr - loFr) * m_pdY2[lo] + (hiFr*hiFr*hiFr - hiFr) * m_pdY2[hi]);
return y;
}
|
