5 #include "EngaugeAssert.h"
14 const std::vector<SplinePair> &xy,
15 SplineTCheck splineTCheck)
17 ENGAUGE_ASSERT (t.size() == xy.size());
18 ENGAUGE_ASSERT (xy.size() > 0);
20 if (splineTCheck == SPLINE_ENABLE_T_CHECK) {
24 computeCoefficientsForIntervals (t, xy);
25 computeControlPointsForIntervals ();
32 void Spline::checkTIncrements (
const std::vector<double> &t)
const
34 for (
unsigned int i = 1; i < t.size(); i++) {
35 double tStep = t[i] - t[i-1];
39 ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
43 void Spline::computeCoefficientsForIntervals (
const std::vector<double> &t,
44 const std::vector<SplinePair> &xy)
51 unsigned int n = unsigned (qFloor (xy.size()) - 1);
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);
64 for (i = 1; i < n; i++) {
66 l[i] =
SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
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];
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]);
83 for (i = 0; i < n; i++) {
101 void Spline::computeControlPointsForIntervals ()
103 int i, n = qFloor (m_xy.size()) - 1;
105 for (i = 0; i < n; i++) {
106 unsigned int iU = unsigned (i);
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];
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];
150 double &aUntranslated,
151 double &bUntranslated,
152 double &cUntranslated,
153 double &dUntranslated)
const
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;
173 int numIterations)
const
177 double tLow = m_t[0];
178 double tHigh = m_t[m_xy.size() - 1];
186 for (
unsigned int i = 0; i < m_xy.size(); i++) {
197 double x0 = interpolateCoeff (m_t[0]).
x();
198 double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
202 double x1 = interpolateCoeff (m_t[1]).x();
203 double tStart = (x - x0) / (x1 - x0);
207 }
else if (xNm1 < x) {
210 double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
211 double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2);
212 tLow = m_xy.size() - 1;
213 tHigh = tHigh + 2.0 * (tStart - tLow);
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) {
235 ENGAUGE_ASSERT (m_elements.size() != 0);
237 vector<SplineCoeff>::const_iterator itr;
238 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
239 if (itr != m_elements.begin()) {
248 ENGAUGE_ASSERT (m_xy.size() != 0);
250 for (
unsigned int i = 0; i < unsigned (m_xy.size() - 1); i++) {
252 if (m_t[i] <= t && t <= m_t[i+1]) {
254 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
256 SplinePair xy = onems * onems * onems * m_xy [i] +
257 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
259 s * s * s * m_xy[i + 1];
265 ENGAUGE_ASSERT (
false);
271 ENGAUGE_ASSERT (i < m_p1.size ());
278 ENGAUGE_ASSERT (i < 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, SplineTCheck splineTCheck=SPLINE_ENABLE_T_CHECK)
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.
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...
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.