Debian Codegraph @

libctsupport/interpolator.cpp

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