Engauge Digitizer  2
 All Classes Functions Variables Typedefs Enumerations Friends Pages
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 "Logger.h"
8 #include "Spline.h"
9 
10 using namespace std;
11 
12 Spline::Spline(const std::vector<double> &t,
13  const std::vector<SplinePair> &xy,
14  SplineTCheck splineTCheck)
15 {
16  ENGAUGE_ASSERT (t.size() == xy.size());
17  ENGAUGE_ASSERT (xy.size() > 0); // Need at least one point for this class to not fail with a crash
18 
19  if (splineTCheck == SPLINE_ENABLE_T_CHECK) {
20  // In normal production this check should be performed
21  checkTIncrements (t);
22  }
23  computeCoefficientsForIntervals (t, xy);
24  computeControlPointsForIntervals ();
25 }
26 
27 Spline::~Spline()
28 {
29 }
30 
31 void Spline::checkTIncrements (const std::vector<double> &t) const
32 {
33  for (unsigned int i = 1; i < t.size(); i++) {
34  double tStep = t[i] - t[i-1];
35 
36  // Failure here means the increment is not one, which it should be. The epsilon is much larger than roundoff
37  // could produce
38  ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
39  }
40 }
41 
42 void Spline::computeCoefficientsForIntervals (const std::vector<double> &t,
43  const std::vector<SplinePair> &xy)
44 {
45  if (xy.size() > 1) {
46 
47  // There are enough points to compute the coefficients
48  int i, j;
49  int n = (int) xy.size() - 1;
50 
51  m_t = t;
52  m_xy = xy;
53 
54  vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
55  vector<SplinePair> h(n+1);
56 
57  l[0] = SplinePair (1.0);
58  u[0] = SplinePair (0.0);
59  z[0] = SplinePair (0.0);
60  h[0] = t[1] - t[0];
61 
62  for (i = 1; i < n; i++) {
63  h[i] = t[i+1] - t[i];
64  l[i] = SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
65  u[i] = h[i] / l[i];
66  a[i] = (SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
67  z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
68  }
69 
70  l[n] = SplinePair (1.0);
71  z[n] = SplinePair (0.0);
72  c[n] = SplinePair (0.0);
73 
74  for (j = n - 1; j >= 0; j--) {
75  c[j] = z[j] - u[j] * c[j+1];
76  b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] + SplinePair (2.0) * c[j])) / SplinePair (3.0);
77  d[j] = (c[j+1] - c[j]) / (SplinePair (3.0) * h[j]);
78  }
79 
80  for (i = 0; i < n; i++) {
81  m_elements.push_back(SplineCoeff(t[i],
82  xy[i],
83  b[i],
84  c[i],
85  d[i]));
86  }
87  } else {
88 
89  // There is only one point so we have to hack a coefficient entry
90  m_elements.push_back(SplineCoeff(t[0],
91  xy[0],
92  0.0,
93  0.0,
94  0.0));
95  }
96 }
97 
98 void Spline::computeControlPointsForIntervals ()
99 {
100  int i, n = (int) m_xy.size() - 1;
101 
102  for (i = 0; i < n; i++) {
103  const SplineCoeff &element = m_elements[i];
104 
105  // 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
106  // 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.
107  // So 3(P1-P0)=b
108  SplinePair p1 = m_xy [i] + element.b() /
109  SplinePair (3.0);
110 
111  // 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
112  // 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
113  SplinePair p2 = m_xy [i + 1] - (element.b() + SplinePair (2.0) * element.c() + SplinePair (3.0) * element.d()) /
114  SplinePair (3.0);
115 
116  m_p1.push_back (p1);
117  m_p2.push_back (p2);
118  }
119 
120  // Debug logging
121  for (i = 0; i < (int) m_xy.size () - 1; i++) {
122  LOG4CPP_DEBUG_S ((*mainCat)) << "Spline::computeControlPointsForIntervals" << " i=" << i
123  << " xy=" << m_xy [i]
124  << " elementt=" << m_elements[i].t()
125  << " elementa=" << m_elements[i].a()
126  << " elementb=" << m_elements[i].b()
127  << " elementc=" << m_elements[i].c()
128  << " elementd=" << m_elements[i].d()
129  << " p1=" << m_p1[i]
130  << " p2=" << m_p2[i];
131  }
132  i = m_xy.size() - 1;
133  LOG4CPP_DEBUG_S ((*mainCat)) << "Spline::computeControlPointsForIntervals" << " i=" << i
134  << " xy=" << m_xy [i];
135 }
136 
138  double bTranslated,
139  double cTranslated,
140  double dTranslated,
141  double tI,
142  double &aUntranslated,
143  double &bUntranslated,
144  double &cUntranslated,
145  double &dUntranslated) const
146 {
147  // Expanding the equation using t translated as (t-ti) we get the equation using untranslated (t) as follows
148  // xy=d*t^3+
149  // (c-3*d*ti)*t^2+
150  // (b-2*c*ti+3*d*ti^2)*t+
151  // (a-b*ti+c*ti^2-d*ti^3)
152  // which matches up with
153  // xy=d*t^3+
154  // c*t^2+
155  // b*t+
156  // a
157  // Both equations are given in the header file documentation for this method
158  aUntranslated = aTranslated - bTranslated * tI + cTranslated * tI * tI - dTranslated * tI * tI * tI;
159  bUntranslated = bTranslated - 2.0 * cTranslated * tI + 3.0 * dTranslated * tI * tI;
160  cUntranslated = cTranslated - 3.0 * dTranslated * tI;
161  dUntranslated = dTranslated;
162 }
163 
165  int numIterations) const
166 {
167  SplinePair spCurrent;
168 
169  double tLow = m_t[0];
170  double tHigh = m_t[m_xy.size() - 1];
171 
172  // This method implicitly assumes that the x values are monotonically increasing
173 
174  // Extrapolation that is performed if x is out of bounds. As a starting point, we assume that the t
175  // values and x values behave the same, which is linearly. This assumption works best when user
176  // has set the points so the spline line is linear at the endpoints - which is also preferred since
177  // higher order polynomials are typically unstable and can "explode" off into unwanted directions
178  double x0 = interpolateCoeff (m_t[0]).x();
179  double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
180  if (x < x0) {
181 
182  // Extrapolate with x < x(0) < x(N-1) which correspond to s, s0 and sNm1
183  double x1 = interpolateCoeff (m_t[1]).x();
184  double tStart = (x - x0) / (x1 - x0); // This will be less than zero. x=x0 for t=0 and x=x1 for t=1
185  tLow = 2.0 * tStart;
186  tHigh = 0.0;
187 
188  } else if (xNm1 < x) {
189 
190  // Extrapolate with x(0) < x(N-1) < x which correspond to s0, sNm1 and s
191  double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
192  double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2); // This is greater than one. x=xNm2 for t=0 and x=xNm1 for t=1
193  tLow = m_xy.size() - 1;
194  tHigh = tHigh + 2.0 * (tStart - tLow);
195 
196  }
197 
198  // Interpolation using bisection search
199  double tCurrent = (tHigh + tLow) / 2.0;
200  double tDelta = (tHigh - tLow) / 4.0;
201  for (int iteration = 0; iteration < numIterations; iteration++) {
202  spCurrent = interpolateCoeff (tCurrent);
203  if (spCurrent.x() > x) {
204  tCurrent -= tDelta;
205  } else {
206  tCurrent += tDelta;
207  }
208  tDelta /= 2.0;
209  }
210 
211  return spCurrent;
212 }
213 
215 {
216  ENGAUGE_ASSERT (m_elements.size() != 0);
217 
218  vector<SplineCoeff>::const_iterator itr;
219  itr = lower_bound(m_elements.begin(), m_elements.end(), t);
220  if (itr != m_elements.begin()) {
221  itr--;
222  }
223 
224  return itr->eval(t);
225 }
226 
228 {
229  ENGAUGE_ASSERT (m_xy.size() != 0);
230 
231  for (int i = 0; i < (signed) m_xy.size() - 1; i++) {
232 
233  if (m_t[i] <= t && t <= m_t[i+1]) {
234 
235  SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
236  SplinePair onems (SplinePair (1.0) - s);
237  SplinePair xy = onems * onems * onems * m_xy [i] +
238  SplinePair (3.0) * onems * onems * s * m_p1 [i] +
239  SplinePair (3.0) * onems * s * s * m_p2 [i] +
240  s * s * s * m_xy[i + 1];
241  return xy;
242  }
243  }
244 
245  // Input argument is out of bounds
246  ENGAUGE_ASSERT (false);
247  return m_t[0];
248 }
249 
250 SplinePair Spline::p1 (unsigned int i) const
251 {
252  ENGAUGE_ASSERT (i < (unsigned int) m_p1.size ());
253 
254  return m_p1 [i];
255 }
256 
257 SplinePair Spline::p2 (unsigned int i) const
258 {
259  ENGAUGE_ASSERT (i < (unsigned int) m_p2.size ());
260 
261  return m_p2 [i];
262 }
SplinePair interpolateCoeff(double t) const
Return interpolated y for specified x.
Definition: Spline.cpp:214
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:227
Spline(const std::vector< double > &t, const std::vector< SplinePair > &xy, SplineTCheck splineTCheck=SPLINE_ENABLE_T_CHECK)
Initialize spline with independent (t) and dependent (x and y) value vectors.
Definition: Spline.cpp:12
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:250
SplinePair b() const
Get method for b.
Definition: SplineCoeff.cpp:35
void computeUntranslatedCoefficients(double aTranslated, double bTranslated, double cTranslated, double dTranslated, double tI, double &aUntranslated, double &bUntranslated, double &cUntranslated, double &dUntranslated) const
From coefficients in xy=d*(t-ti)^3+c*(t-ti)^2+b*(t-ti)+a we compute and return the coefficients in xy...
Definition: Spline.cpp:137
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:164
double x() const
Get method for x.
Definition: SplinePair.cpp:75
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:257
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:13