5 #include "EngaugeAssert.h" 13 const std::vector<SplinePair> &xy,
14 SplineTCheck splineTCheck)
16 ENGAUGE_ASSERT (t.size() == xy.size());
17 ENGAUGE_ASSERT (xy.size() > 0);
19 if (splineTCheck == SPLINE_ENABLE_T_CHECK) {
23 computeCoefficientsForIntervals (t, xy);
24 computeControlPointsForIntervals ();
31 void Spline::checkTIncrements (
const std::vector<double> &t)
const 33 for (
unsigned int i = 1; i < t.size(); i++) {
34 double tStep = t[i] - t[i-1];
38 ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
42 void Spline::computeCoefficientsForIntervals (
const std::vector<double> &t,
43 const std::vector<SplinePair> &xy)
49 int n = (int) xy.size() - 1;
54 vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
55 vector<SplinePair> h(n+1);
62 for (i = 1; i < n; i++) {
64 l[i] =
SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
66 a[i] = (
SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (
SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
67 z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
74 for (j = n - 1; j >= 0; j--) {
75 c[j] = z[j] - u[j] * c[j+1];
76 b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] +
SplinePair (2.0) * c[j])) /
SplinePair (3.0);
77 d[j] = (c[j+1] - c[j]) / (
SplinePair (3.0) * h[j]);
80 for (i = 0; i < n; i++) {
98 void Spline::computeControlPointsForIntervals ()
100 int i, n = (int) m_xy.size() - 1;
102 for (i = 0; i < n; i++) {
121 for (i = 0; i < (int) m_xy.size () - 1; i++) {
122 LOG4CPP_DEBUG_S ((*mainCat)) <<
"Spline::computeControlPointsForIntervals" <<
" i=" << i
123 <<
" xy=" << m_xy [i]
124 <<
" elementt=" << m_elements[i].t()
125 <<
" elementa=" << m_elements[i].a()
126 <<
" elementb=" << m_elements[i].b()
127 <<
" elementc=" << m_elements[i].c()
128 <<
" elementd=" << m_elements[i].d()
130 <<
" p2=" << m_p2[i];
133 LOG4CPP_DEBUG_S ((*mainCat)) <<
"Spline::computeControlPointsForIntervals" <<
" i=" << i
134 <<
" xy=" << m_xy [i];
142 double &aUntranslated,
143 double &bUntranslated,
144 double &cUntranslated,
145 double &dUntranslated)
const 158 aUntranslated = aTranslated - bTranslated * tI + cTranslated * tI * tI - dTranslated * tI * tI * tI;
159 bUntranslated = bTranslated - 2.0 * cTranslated * tI + 3.0 * dTranslated * tI * tI;
160 cUntranslated = cTranslated - 3.0 * dTranslated * tI;
161 dUntranslated = dTranslated;
165 int numIterations)
const 169 double tLow = m_t[0];
170 double tHigh = m_t[m_xy.size() - 1];
178 double x0 = interpolateCoeff (m_t[0]).
x();
179 double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
183 double x1 = interpolateCoeff (m_t[1]).x();
184 double tStart = (x - x0) / (x1 - x0);
188 }
else if (xNm1 < x) {
191 double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
192 double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2);
193 tLow = m_xy.size() - 1;
194 tHigh = tHigh + 2.0 * (tStart - tLow);
199 double tCurrent = (tHigh + tLow) / 2.0;
200 double tDelta = (tHigh - tLow) / 4.0;
201 for (
int iteration = 0; iteration < numIterations; iteration++) {
202 spCurrent = interpolateCoeff (tCurrent);
203 if (spCurrent.
x() > x) {
216 ENGAUGE_ASSERT (m_elements.size() != 0);
218 vector<SplineCoeff>::const_iterator itr;
219 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
220 if (itr != m_elements.begin()) {
229 ENGAUGE_ASSERT (m_xy.size() != 0);
231 for (
int i = 0; i < (signed) m_xy.size() - 1; i++) {
233 if (m_t[i] <= t && t <= m_t[i+1]) {
235 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
237 SplinePair xy = onems * onems * onems * m_xy [i] +
238 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
240 s * s * s * m_xy[i + 1];
246 ENGAUGE_ASSERT (
false);
252 ENGAUGE_ASSERT (i < (
unsigned int) m_p1.size ());
259 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.
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 b() const
Get method for b.
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 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.