00001
00002
00003
00004
00005 #include "EngaugeAssert.h"
00006 #include <iostream>
00007 #include "Spline.h"
00008
00009 using namespace std;
00010
00011 Spline::Spline(const std::vector<double> &t,
00012 const std::vector<SplinePair> &xy)
00013 {
00014 ENGAUGE_ASSERT (t.size() == xy.size());
00015 ENGAUGE_ASSERT (xy.size() >= 3);
00016
00017 checkTIncrements (t);
00018 computeCoefficientsForIntervals (t, xy);
00019 computeControlPointsForIntervals ();
00020 }
00021
00022 Spline::~Spline()
00023 {
00024 }
00025
00026 void Spline::checkTIncrements (const std::vector<double> &t) const
00027 {
00028 for (unsigned int i = 1; i < t.size(); i++) {
00029 double tStep = t[i] - t[i-1];
00030
00031
00032
00033 ENGAUGE_ASSERT (qAbs (tStep - 1.0) < 0.0001);
00034 }
00035 }
00036
00037 void Spline::computeCoefficientsForIntervals (const std::vector<double> &t,
00038 const std::vector<SplinePair> &xy)
00039 {
00040 int i, j;
00041 int n = (int) xy.size() - 1;
00042
00043 m_t = t;
00044 m_xy = xy;
00045
00046 vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
00047 vector<SplinePair> h(n+1);
00048
00049 l[0] = SplinePair (1.0);
00050 u[0] = SplinePair (0.0);
00051 z[0] = SplinePair (0.0);
00052 h[0] = t[1] - t[0];
00053
00054 for (i = 1; i < n; i++) {
00055 h[i] = t[i+1] - t[i];
00056 l[i] = SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
00057 u[i] = h[i] / l[i];
00058 a[i] = (SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
00059 z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
00060 }
00061
00062 l[n] = SplinePair (1.0);
00063 z[n] = SplinePair (0.0);
00064 c[n] = SplinePair (0.0);
00065
00066 for (j = n - 1; j >= 0; j--) {
00067 c[j] = z[j] - u[j] * c[j+1];
00068 b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] + SplinePair (2.0) * c[j])) / SplinePair (3.0);
00069 d[j] = (c[j+1] - c[j]) / (SplinePair (3.0) * h[j]);
00070 }
00071
00072 for (i = 0; i < n; i++) {
00073 m_elements.push_back(SplineCoeff(t[i],
00074 xy[i],
00075 b[i],
00076 c[i],
00077 d[i]));
00078 }
00079 }
00080
00081 void Spline::computeControlPointsForIntervals ()
00082 {
00083 int n = (int) m_xy.size() - 1;
00084
00085 for (int i = 0; i < n; i++) {
00086 const SplineCoeff &element = m_elements[i];
00087
00088
00089
00090
00091 SplinePair p1 = m_xy [i] + element.b() /
00092 SplinePair (3.0);
00093
00094
00095
00096 SplinePair p2 = m_xy [i + 1] - (element.b() + SplinePair (2.0) * element.c() + SplinePair (3.0) * element.d()) /
00097 SplinePair (3.0);
00098
00099 m_p1.push_back (p1);
00100 m_p2.push_back (p2);
00101 }
00102 }
00103
00104 SplinePair Spline::findSplinePairForFunctionX (double x,
00105 int numIterations) const
00106 {
00107 SplinePair spCurrent;
00108
00109 double tLow = m_t[0];
00110 double tHigh = m_t[m_xy.size() - 1];
00111
00112 double tCurrent = (tHigh + tLow) / 2.0;
00113 double tDelta = (tHigh - tLow) / 4.0;
00114 for (int iteration = 0; iteration < numIterations; iteration++) {
00115 spCurrent = interpolateCoeff (tCurrent);
00116 if (spCurrent.x() > x) {
00117 tCurrent -= tDelta;
00118 } else {
00119 tCurrent += tDelta;
00120 }
00121 tDelta /= 2.0;
00122 }
00123
00124 return spCurrent;
00125 }
00126
00127 SplinePair Spline::interpolateCoeff (double t) const
00128 {
00129 ENGAUGE_ASSERT (m_elements.size() != 0);
00130
00131 vector<SplineCoeff>::const_iterator itr;
00132 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
00133 if (itr != m_elements.begin()) {
00134 itr--;
00135 }
00136
00137 return itr->eval(t);
00138 }
00139
00140 SplinePair Spline::interpolateControlPoints (double t) const
00141 {
00142 ENGAUGE_ASSERT (m_xy.size() != 0);
00143
00144 for (int i = 0; i < (signed) m_xy.size() - 1; i++) {
00145
00146 if (m_t[i] <= t && t <= m_t[i+1]) {
00147
00148 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
00149 SplinePair onems (SplinePair (1.0) - s);
00150 SplinePair xy = onems * onems * onems * m_xy [i] +
00151 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
00152 SplinePair (3.0) * onems * s * s * m_p2 [i] +
00153 s * s * s * m_xy[i + 1];
00154 return xy;
00155 }
00156 }
00157
00158
00159 ENGAUGE_ASSERT (false);
00160 return m_t[0];
00161 }
00162
00163 SplinePair Spline::p1 (unsigned int i) const
00164 {
00165 ENGAUGE_ASSERT (i < (unsigned int) m_p1.size ());
00166
00167 return m_p1 [i];
00168 }
00169
00170 SplinePair Spline::p2 (unsigned int i) const
00171 {
00172 ENGAUGE_ASSERT (i < (unsigned int) m_p2.size ());
00173
00174 return m_p2 [i];
00175 }