14 const std::vector<SplinePair> &xy,
24 computeCoefficientsForIntervals (t, xy);
25 computeControlPointsForIntervals ();
32void 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];
43void 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);
59 l[0] = SplinePair (1.0);
60 u[0] = SplinePair (0.0);
61 z[0] = SplinePair (0.0);
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];
72 l[n] = SplinePair (1.0);
73 z[n] = SplinePair (0.0);
74 c[n] = SplinePair (0.0);
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++) {
84 m_elements.push_back(SplineCoeff(t[i],
93 m_elements.push_back(SplineCoeff(t[0],
101void Spline::computeControlPointsForIntervals ()
103 int i, n = qFloor (m_xy.size()) - 1;
105 for (i = 0; i < n; i++) {
106 unsigned int iU = unsigned (i);
107 const SplineCoeff &element = m_elements[iU];
112 SplinePair
p1 = m_xy [iU] + element.
b() /
117 SplinePair
p2 = m_xy [iU + 1] - (element.
b() + SplinePair (2.0) * element.
c() + SplinePair (3.0) * element.
d()) /
125 for (i = 0; i < qFloor (m_xy.size ()) - 1; i++) {
126 unsigned int iU = unsigned (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);
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++) {
203 double tStart = (x - x0) / (x1 - x0);
207 }
else if (xNm1 < 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++) {
222 if (spCurrent.
x() > x) {
237 vector<SplineCoeff>::const_iterator itr;
238 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
239 if (itr != m_elements.begin()) {
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];
#define ENGAUGE_ASSERT(cond)
Drop in replacement for Q_ASSERT.
log4cpp::Category * mainCat
SplinePair d() const
Get method for d.
SplinePair c() const
Get method for c.
SplinePair b() const
Get method for b.
Single X/Y pair for cubic spline interpolation initialization and calculations.
double x() const
Get method for x.
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 interpolateControlPoints(double t) const
Return interpolated y for specified x, for testing.
SplinePair findSplinePairForFunctionX(double x, int numIterations) const
Use bisection algorithm to iteratively find the SplinePair interpolated to best match the specified x...
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].
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 interpolateCoeff(double t) const
Return interpolated y for specified x.
#define LOG4CPP_DEBUG_S(logger)