5 #include "EngaugeAssert.h" 12 const std::vector<SplinePair> &xy)
14 ENGAUGE_ASSERT (t.size() == xy.size());
15 ENGAUGE_ASSERT (xy.size() >= 3);
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)
41 int n = (int) xy.size() - 1;
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);
54 for (i = 1; i < n; i++) {
56 l[i] =
SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
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];
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]);
72 for (i = 0; i < n; i++) {
81 void Spline::computeControlPointsForIntervals ()
83 int n = (int) m_xy.size() - 1;
85 for (
int i = 0; i < n; i++) {
105 int numIterations)
const 109 double tLow = m_t[0];
110 double tHigh = m_t[m_xy.size() - 1];
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) {
129 ENGAUGE_ASSERT (m_elements.size() != 0);
131 vector<SplineCoeff>::const_iterator itr;
132 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
133 if (itr != m_elements.begin()) {
142 ENGAUGE_ASSERT (m_xy.size() != 0);
144 for (
int i = 0; i < (signed) m_xy.size() - 1; i++) {
146 if (m_t[i] <= t && t <= m_t[i+1]) {
148 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
150 SplinePair xy = onems * onems * onems * m_xy [i] +
151 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
153 s * s * s * m_xy[i + 1];
159 ENGAUGE_ASSERT (
false);
165 ENGAUGE_ASSERT (i < (
unsigned int) m_p1.size ());
172 ENGAUGE_ASSERT (i < (
unsigned int) m_p2.size ());
SplinePair interpolateCoeff(double t) const
Return interpolated y for specified x.
SplinePair c() const
Get method for c.
SplinePair interpolateControlPoints(double t) const
Return interpolated y for specified x, for testing.
Spline(const std::vector< double > &t, const std::vector< SplinePair > &xy)
Initialize spline with independent (t) and dependent (x and y) value vectors.
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 b() const
Get method for b.
SplinePair findSplinePairForFunctionX(double x, int numIterations) const
Use bisection algorithm to iteratively find the SplinePair interpolated to best match the specified x...
double x() const
Get method for x.
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].
Four element vector of a,b,c,d coefficients and the associated x value, for one interval of a set of ...
Single X/Y pair for cubic spline interpolation initialization and calculations.