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