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 for (
unsigned int i = 0; i < m_xy.size(); i++) {
189 double x0 = interpolateCoeff (m_t[0]).
x();
190 double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
194 double x1 = interpolateCoeff (m_t[1]).x();
195 double tStart = (x - x0) / (x1 - x0);
199 }
else if (xNm1 < x) {
202 double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
203 double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2);
204 tLow = m_xy.size() - 1;
205 tHigh = tHigh + 2.0 * (tStart - tLow);
210 double tCurrent = (tHigh + tLow) / 2.0;
211 double tDelta = (tHigh - tLow) / 4.0;
212 for (
int iteration = 0; iteration < numIterations; iteration++) {
213 spCurrent = interpolateCoeff (tCurrent);
214 if (spCurrent.
x() > x) {
227 ENGAUGE_ASSERT (m_elements.size() != 0);
229 vector<SplineCoeff>::const_iterator itr;
230 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
231 if (itr != m_elements.begin()) {
240 ENGAUGE_ASSERT (m_xy.size() != 0);
242 for (
int i = 0; i < (signed) m_xy.size() - 1; i++) {
244 if (m_t[i] <= t && t <= m_t[i+1]) {
246 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
248 SplinePair xy = onems * onems * onems * m_xy [i] +
249 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
251 s * s * s * m_xy[i + 1];
257 ENGAUGE_ASSERT (
false);
263 ENGAUGE_ASSERT (i < (
unsigned int) m_p1.size ());
270 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, 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.