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() >= 3);
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  int i, j;
41  int n = (int) xy.size() - 1;
42 
43  m_t = t;
44  m_xy = xy;
45 
46  vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
47  vector<SplinePair> h(n+1);
48 
49  l[0] = SplinePair (1.0);
50  u[0] = SplinePair (0.0);
51  z[0] = SplinePair (0.0);
52  h[0] = t[1] - t[0];
53 
54  for (i = 1; i < n; i++) {
55  h[i] = t[i+1] - t[i];
56  l[i] = SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
57  u[i] = h[i] / l[i];
58  a[i] = (SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
59  z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
60  }
61 
62  l[n] = SplinePair (1.0);
63  z[n] = SplinePair (0.0);
64  c[n] = SplinePair (0.0);
65 
66  for (j = n - 1; j >= 0; j--) {
67  c[j] = z[j] - u[j] * c[j+1];
68  b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] + SplinePair (2.0) * c[j])) / SplinePair (3.0);
69  d[j] = (c[j+1] - c[j]) / (SplinePair (3.0) * h[j]);
70  }
71 
72  for (i = 0; i < n; i++) {
73  m_elements.push_back(SplineCoeff(t[i],
74  xy[i],
75  b[i],
76  c[i],
77  d[i]));
78  }
79 }
80 
81 void Spline::computeControlPointsForIntervals ()
82 {
83  int n = (int) m_xy.size() - 1;
84 
85  for (int i = 0; i < n; i++) {
86  const SplineCoeff &element = m_elements[i];
87 
88  // 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
89  // 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.
90  // So 3(P1-P0)=b
91  SplinePair p1 = m_xy [i] + element.b() /
92  SplinePair (3.0);
93 
94  // 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
95  // 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
96  SplinePair p2 = m_xy [i + 1] - (element.b() + SplinePair (2.0) * element.c() + SplinePair (3.0) * element.d()) /
97  SplinePair (3.0);
98 
99  m_p1.push_back (p1);
100  m_p2.push_back (p2);
101  }
102 }
103 
105  int numIterations) const
106 {
107  SplinePair spCurrent;
108 
109  double tLow = m_t[0];
110  double tHigh = m_t[m_xy.size() - 1];
111 
112  double tCurrent = (tHigh + tLow) / 2.0;
113  double tDelta = (tHigh - tLow) / 4.0;
114  for (int iteration = 0; iteration < numIterations; iteration++) {
115  spCurrent = interpolateCoeff (tCurrent);
116  if (spCurrent.x() > x) {
117  tCurrent -= tDelta;
118  } else {
119  tCurrent += tDelta;
120  }
121  tDelta /= 2.0;
122  }
123 
124  return spCurrent;
125 }
126 
128 {
129  ENGAUGE_ASSERT (m_elements.size() != 0);
130 
131  vector<SplineCoeff>::const_iterator itr;
132  itr = lower_bound(m_elements.begin(), m_elements.end(), t);
133  if (itr != m_elements.begin()) {
134  itr--;
135  }
136 
137  return itr->eval(t);
138 }
139 
141 {
142  ENGAUGE_ASSERT (m_xy.size() != 0);
143 
144  for (int i = 0; i < (signed) m_xy.size() - 1; i++) {
145 
146  if (m_t[i] <= t && t <= m_t[i+1]) {
147 
148  SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
149  SplinePair onems (SplinePair (1.0) - s);
150  SplinePair xy = onems * onems * onems * m_xy [i] +
151  SplinePair (3.0) * onems * onems * s * m_p1 [i] +
152  SplinePair (3.0) * onems * s * s * m_p2 [i] +
153  s * s * s * m_xy[i + 1];
154  return xy;
155  }
156  }
157 
158  // Input argument is out of bounds
159  ENGAUGE_ASSERT (false);
160  return m_t[0];
161 }
162 
163 SplinePair Spline::p1 (unsigned int i) const
164 {
165  ENGAUGE_ASSERT (i < (unsigned int) m_p1.size ());
166 
167  return m_p1 [i];
168 }
169 
170 SplinePair Spline::p2 (unsigned int i) const
171 {
172  ENGAUGE_ASSERT (i < (unsigned int) m_p2.size ());
173 
174  return m_p2 [i];
175 }
SplinePair interpolateCoeff(double t) const
Return interpolated y for specified x.
Definition: Spline.cpp:127
SplinePair c() const
Get method for c.
Definition: SplineCoeff.cpp:40
SplinePair interpolateControlPoints(double t) const
Return interpolated y for specified x, for testing.
Definition: Spline.cpp:140
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 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:163
SplinePair b() const
Get method for b.
Definition: SplineCoeff.cpp:35
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:104
double x() const
Get method for x.
Definition: SplinePair.cpp:66
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:170
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
Single X/Y pair for cubic spline interpolation initialization and calculations.
Definition: SplinePair.h:11