Engauge Digitizer  2
Spline.cpp
1 /* "THE BEER-WARE LICENSE" (Revision 42): Devin Lane wrote this file. As long as you retain
2  * this notice you can do whatever you want with this stuff. If we meet some day, and you
3  * think this stuff is worth it, you can buy me a beer in return. */
4 
5 #include "EngaugeAssert.h"
6 #include <iostream>
7 #include "Spline.h"
8 
9 using namespace std;
10 
11 Spline::Spline(const std::vector<double> &t,
12  const std::vector<SplinePair> &xy)
13 {
14  ENGAUGE_ASSERT (t.size() == xy.size());
15  ENGAUGE_ASSERT (xy.size() > 0); // Need at least one point for this class to not fail with a crash
16 
17  checkTIncrements (t);
18  computeCoefficientsForIntervals (t, xy);
19  computeControlPointsForIntervals ();
20 }
21 
22 Spline::~Spline()
23 {
24 }
25 
26 void Spline::checkTIncrements (const std::vector<double> &t) const
27 {
28  for (unsigned int i = 1; i < t.size(); i++) {
29  double tStep = t[i] - t[i-1];
30 
31  // Failure here means the increment is not one, which it should be. The epsilon is much larger than roundoff
32  // could produce
33  ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
34  }
35 }
36 
37 void Spline::computeCoefficientsForIntervals (const std::vector<double> &t,
38  const std::vector<SplinePair> &xy)
39 {
40  if (xy.size() > 1) {
41 
42  // There are enough points to compute the coefficients
43  int i, j;
44  int n = (int) xy.size() - 1;
45 
46  m_t = t;
47  m_xy = xy;
48 
49  vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
50  vector<SplinePair> h(n+1);
51 
52  l[0] = SplinePair (1.0);
53  u[0] = SplinePair (0.0);
54  z[0] = SplinePair (0.0);
55  h[0] = t[1] - t[0];
56 
57  for (i = 1; i < n; i++) {
58  h[i] = t[i+1] - t[i];
59  l[i] = SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
60  u[i] = h[i] / l[i];
61  a[i] = (SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
62  z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
63  }
64 
65  l[n] = SplinePair (1.0);
66  z[n] = SplinePair (0.0);
67  c[n] = SplinePair (0.0);
68 
69  for (j = n - 1; j >= 0; j--) {
70  c[j] = z[j] - u[j] * c[j+1];
71  b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] + SplinePair (2.0) * c[j])) / SplinePair (3.0);
72  d[j] = (c[j+1] - c[j]) / (SplinePair (3.0) * h[j]);
73  }
74 
75  for (i = 0; i < n; i++) {
76  m_elements.push_back(SplineCoeff(t[i],
77  xy[i],
78  b[i],
79  c[i],
80  d[i]));
81  }
82  } else {
83 
84  // There is only one point so we have to hack a coefficient entry
85  m_elements.push_back(SplineCoeff(t[0],
86  xy[0],
87  0.0,
88  0.0,
89  0.0));
90  }
91 }
92 
93 void Spline::computeControlPointsForIntervals ()
94 {
95  int n = (int) m_xy.size() - 1;
96 
97  for (int i = 0; i < n; i++) {
98  const SplineCoeff &element = m_elements[i];
99 
100  // Derivative at P0 from (1-s)^3*P0+(1-s)^2*s*P1+(1-s)*s^2*P2+s^3*P3 with s=0 evaluates to 3(P1-P0). That
101  // derivative must match the derivative of y=a+b*(t-ti)+c*(t-ti)^2+d*(t-ti)^3 with t=ti which evaluates to b.
102  // So 3(P1-P0)=b
103  SplinePair p1 = m_xy [i] + element.b() /
104  SplinePair (3.0);
105 
106  // Derivative at P2 from (1-s)^3*P0+(1-s)^2*s*P1+(1-s)*s^2*P2+s^3*P3 with s=1 evaluates to 3(P3-P2). That
107  // derivative must match the derivative of y=a+b*(t-ti)+c*(t-ti)^2+d*(t-ti)^3 with t=ti+1 which evaluates to b+2*c+3*d
108  SplinePair p2 = m_xy [i + 1] - (element.b() + SplinePair (2.0) * element.c() + SplinePair (3.0) * element.d()) /
109  SplinePair (3.0);
110 
111  m_p1.push_back (p1);
112  m_p2.push_back (p2);
113  }
114 }
115 
117  int numIterations) const
118 {
119  SplinePair spCurrent;
120 
121  double tLow = m_t[0];
122  double tHigh = m_t[m_xy.size() - 1];
123 
124  // This method implicitly assumes that the x values are monotonically increasing
125 
126  // Extrapolation that is performed if x is out of bounds. As a starting point, we assume that the t
127  // values and x values behave the same, which is linearly. This assumption works best when user
128  // has set the points so the spline line is linear at the endpoints - which is also preferred since
129  // higher order polynomials are typically unstable and can "explode" off into unwanted directions
130  double x0 = interpolateCoeff (m_t[0]).x();
131  double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
132  if (x < x0) {
133 
134  // Extrapolate with x < x(0) < x(N-1) which correspond to s, s0 and sNm1
135  double x1 = interpolateCoeff (m_t[1]).x();
136  double tStart = (x - x0) / (x1 - x0); // This will be less than zero. x=x0 for t=0 and x=x1 for t=1
137  tLow = 2.0 * tStart;
138  tHigh = 0.0;
139 
140  } else if (xNm1 < x) {
141 
142  // Extrapolate with x(0) < x(N-1) < x which correspond to s0, sNm1 and s
143  double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
144  double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2); // This is greater than one. x=xNm2 for t=0 and x=xNm1 for t=1
145  tLow = m_xy.size() - 1;
146  tHigh = tHigh + 2.0 * (tStart - tLow);
147 
148  }
149 
150  // Interpolation using bisection search
151  double tCurrent = (tHigh + tLow) / 2.0;
152  double tDelta = (tHigh - tLow) / 4.0;
153  for (int iteration = 0; iteration < numIterations; iteration++) {
154  spCurrent = interpolateCoeff (tCurrent);
155  if (spCurrent.x() > x) {
156  tCurrent -= tDelta;
157  } else {
158  tCurrent += tDelta;
159  }
160  tDelta /= 2.0;
161  }
162 
163  return spCurrent;
164 }
165 
167 {
168  ENGAUGE_ASSERT (m_elements.size() != 0);
169 
170  vector<SplineCoeff>::const_iterator itr;
171  itr = lower_bound(m_elements.begin(), m_elements.end(), t);
172  if (itr != m_elements.begin()) {
173  itr--;
174  }
175 
176  return itr->eval(t);
177 }
178 
180 {
181  ENGAUGE_ASSERT (m_xy.size() != 0);
182 
183  for (int i = 0; i < (signed) m_xy.size() - 1; i++) {
184 
185  if (m_t[i] <= t && t <= m_t[i+1]) {
186 
187  SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
188  SplinePair onems (SplinePair (1.0) - s);
189  SplinePair xy = onems * onems * onems * m_xy [i] +
190  SplinePair (3.0) * onems * onems * s * m_p1 [i] +
191  SplinePair (3.0) * onems * s * s * m_p2 [i] +
192  s * s * s * m_xy[i + 1];
193  return xy;
194  }
195  }
196 
197  // Input argument is out of bounds
198  ENGAUGE_ASSERT (false);
199  return m_t[0];
200 }
201 
202 SplinePair Spline::p1 (unsigned int i) const
203 {
204  ENGAUGE_ASSERT (i < (unsigned int) m_p1.size ());
205 
206  return m_p1 [i];
207 }
208 
209 SplinePair Spline::p2 (unsigned int i) const
210 {
211  ENGAUGE_ASSERT (i < (unsigned int) m_p2.size ());
212 
213  return m_p2 [i];
214 }
SplinePair interpolateControlPoints(double t) const
Return interpolated y for specified x, for testing.
Definition: Spline.cpp:179
SplinePair interpolateCoeff(double t) const
Return interpolated y for specified x.
Definition: Spline.cpp:166
SplinePair c() const
Get method for c.
Definition: SplineCoeff.cpp:40
SplinePair b() const
Get method for b.
Definition: SplineCoeff.cpp:35
Spline(const std::vector< double > &t, const std::vector< SplinePair > &xy)
Initialize spline with independent (t) and dependent (x and y) value vectors.
Definition: Spline.cpp:11
SplinePair d() const
Get method for d.
Definition: SplineCoeff.cpp:45
SplinePair p2(unsigned int i) const
Bezier p2 control point for specified interval. P0 is m_xy[i] and P3 is m_xy[i+1].
Definition: Spline.cpp:209
SplinePair p1(unsigned int i) const
Bezier p1 control point for specified interval. P0 is m_xy[i] and P3 is m_xy[i+1].
Definition: Spline.cpp:202
SplinePair findSplinePairForFunctionX(double x, int numIterations) const
Use bisection algorithm to iteratively find the SplinePair interpolated to best match the specified x...
Definition: Spline.cpp:116
Four element vector of a,b,c,d coefficients and the associated x value, for one interval of a set of ...
Definition: SplineCoeff.h:14
double x() const
Get method for x.
Definition: SplinePair.cpp:66
Single X/Y pair for cubic spline interpolation initialization and calculations.
Definition: SplinePair.h:11