5 #include "EngaugeAssert.h" 12 const std::vector<SplinePair> &xy)
14 ENGAUGE_ASSERT (t.size() == xy.size());
15 ENGAUGE_ASSERT (xy.size() > 0);
18 computeCoefficientsForIntervals (t, xy);
19 computeControlPointsForIntervals ();
26 void Spline::checkTIncrements (
const std::vector<double> &t)
const 28 for (
unsigned int i = 1; i < t.size(); i++) {
29 double tStep = t[i] - t[i-1];
33 ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
37 void Spline::computeCoefficientsForIntervals (
const std::vector<double> &t,
38 const std::vector<SplinePair> &xy)
44 int n = (int) xy.size() - 1;
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);
57 for (i = 1; i < n; i++) {
59 l[i] =
SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
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];
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]);
75 for (i = 0; i < n; i++) {
93 void Spline::computeControlPointsForIntervals ()
95 int n = (int) m_xy.size() - 1;
97 for (
int i = 0; i < n; i++) {
117 int numIterations)
const 121 double tLow = m_t[0];
122 double tHigh = m_t[m_xy.size() - 1];
130 double x0 = interpolateCoeff (m_t[0]).
x();
131 double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
135 double x1 = interpolateCoeff (m_t[1]).x();
136 double tStart = (x - x0) / (x1 - x0);
140 }
else if (xNm1 < x) {
143 double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
144 double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2);
145 tLow = m_xy.size() - 1;
146 tHigh = tHigh + 2.0 * (tStart - tLow);
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) {
168 ENGAUGE_ASSERT (m_elements.size() != 0);
170 vector<SplineCoeff>::const_iterator itr;
171 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
172 if (itr != m_elements.begin()) {
181 ENGAUGE_ASSERT (m_xy.size() != 0);
183 for (
int i = 0; i < (signed) m_xy.size() - 1; i++) {
185 if (m_t[i] <= t && t <= m_t[i+1]) {
187 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
189 SplinePair xy = onems * onems * onems * m_xy [i] +
190 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
192 s * s * s * m_xy[i + 1];
198 ENGAUGE_ASSERT (
false);
204 ENGAUGE_ASSERT (i < (
unsigned int) m_p1.size ());
211 ENGAUGE_ASSERT (i < (
unsigned int) m_p2.size ());
SplinePair interpolateControlPoints(double t) const
Return interpolated y for specified x, for testing.
SplinePair interpolateCoeff(double t) const
Return interpolated y for specified x.
SplinePair c() const
Get method for c.
SplinePair b() const
Get method for b.
Spline(const std::vector< double > &t, const std::vector< SplinePair > &xy)
Initialize spline with independent (t) and dependent (x and y) value vectors.
SplinePair d() const
Get method for d.
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].
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].
SplinePair findSplinePairForFunctionX(double x, int numIterations) const
Use bisection algorithm to iteratively find the SplinePair interpolated to best match the specified x...
Four element vector of a,b,c,d coefficients and the associated x value, for one interval of a set of ...
double x() const
Get method for x.
Single X/Y pair for cubic spline interpolation initialization and calculations.