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() > 0);
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 if (xy.size() > 1) {
00041
00042
00043 int i, j;
00044 int n = (int) xy.size() - 1;
00045
00046 m_t = t;
00047 m_xy = xy;
00048
00049 vector<SplinePair> b(n), d(n), a(n), c(n+1), l(n+1), u(n+1), z(n+1);
00050 vector<SplinePair> h(n+1);
00051
00052 l[0] = SplinePair (1.0);
00053 u[0] = SplinePair (0.0);
00054 z[0] = SplinePair (0.0);
00055 h[0] = t[1] - t[0];
00056
00057 for (i = 1; i < n; i++) {
00058 h[i] = t[i+1] - t[i];
00059 l[i] = SplinePair (2.0) * (t[i+1] - t[i-1]) - h[i-1] * u[i-1];
00060 u[i] = h[i] / l[i];
00061 a[i] = (SplinePair (3.0) / h[i]) * (xy[i+1] - xy[i]) - (SplinePair (3.0) / h[i-1]) * (xy[i] - xy[i-1]);
00062 z[i] = (a[i] - h[i-1] * z[i-1]) / l[i];
00063 }
00064
00065 l[n] = SplinePair (1.0);
00066 z[n] = SplinePair (0.0);
00067 c[n] = SplinePair (0.0);
00068
00069 for (j = n - 1; j >= 0; j--) {
00070 c[j] = z[j] - u[j] * c[j+1];
00071 b[j] = (xy[j+1] - xy[j]) / (h[j]) - (h[j] * (c[j+1] + SplinePair (2.0) * c[j])) / SplinePair (3.0);
00072 d[j] = (c[j+1] - c[j]) / (SplinePair (3.0) * h[j]);
00073 }
00074
00075 for (i = 0; i < n; i++) {
00076 m_elements.push_back(SplineCoeff(t[i],
00077 xy[i],
00078 b[i],
00079 c[i],
00080 d[i]));
00081 }
00082 } else {
00083
00084
00085 m_elements.push_back(SplineCoeff(t[0],
00086 xy[0],
00087 0.0,
00088 0.0,
00089 0.0));
00090 }
00091 }
00092
00093 void Spline::computeControlPointsForIntervals ()
00094 {
00095 int n = (int) m_xy.size() - 1;
00096
00097 for (int i = 0; i < n; i++) {
00098 const SplineCoeff &element = m_elements[i];
00099
00100
00101
00102
00103 SplinePair p1 = m_xy [i] + element.b() /
00104 SplinePair (3.0);
00105
00106
00107
00108 SplinePair p2 = m_xy [i + 1] - (element.b() + SplinePair (2.0) * element.c() + SplinePair (3.0) * element.d()) /
00109 SplinePair (3.0);
00110
00111 m_p1.push_back (p1);
00112 m_p2.push_back (p2);
00113 }
00114 }
00115
00116 SplinePair Spline::findSplinePairForFunctionX (double x,
00117 int numIterations) const
00118 {
00119 SplinePair spCurrent;
00120
00121 double tLow = m_t[0];
00122 double tHigh = m_t[m_xy.size() - 1];
00123
00124
00125
00126
00127
00128
00129
00130 double x0 = interpolateCoeff (m_t[0]).x();
00131 double xNm1 = interpolateCoeff (m_t[m_xy.size() - 1]).x();
00132 if (x < x0) {
00133
00134
00135 double x1 = interpolateCoeff (m_t[1]).x();
00136 double tStart = (x - x0) / (x1 - x0);
00137 tLow = 2.0 * tStart;
00138 tHigh = 0.0;
00139
00140 } else if (xNm1 < x) {
00141
00142
00143 double xNm2 = interpolateCoeff (m_t[m_xy.size() - 2]).x();
00144 double tStart = tHigh + (x - xNm1) / (xNm1 - xNm2);
00145 tLow = m_xy.size() - 1;
00146 tHigh = tHigh + 2.0 * (tStart - tLow);
00147
00148 }
00149
00150
00151 double tCurrent = (tHigh + tLow) / 2.0;
00152 double tDelta = (tHigh - tLow) / 4.0;
00153 for (int iteration = 0; iteration < numIterations; iteration++) {
00154 spCurrent = interpolateCoeff (tCurrent);
00155 if (spCurrent.x() > x) {
00156 tCurrent -= tDelta;
00157 } else {
00158 tCurrent += tDelta;
00159 }
00160 tDelta /= 2.0;
00161 }
00162
00163 return spCurrent;
00164 }
00165
00166 SplinePair Spline::interpolateCoeff (double t) const
00167 {
00168 ENGAUGE_ASSERT (m_elements.size() != 0);
00169
00170 vector<SplineCoeff>::const_iterator itr;
00171 itr = lower_bound(m_elements.begin(), m_elements.end(), t);
00172 if (itr != m_elements.begin()) {
00173 itr--;
00174 }
00175
00176 return itr->eval(t);
00177 }
00178
00179 SplinePair Spline::interpolateControlPoints (double t) const
00180 {
00181 ENGAUGE_ASSERT (m_xy.size() != 0);
00182
00183 for (int i = 0; i < (signed) m_xy.size() - 1; i++) {
00184
00185 if (m_t[i] <= t && t <= m_t[i+1]) {
00186
00187 SplinePair s ((t - m_t[i]) / (m_t[i + 1] - m_t[i]));
00188 SplinePair onems (SplinePair (1.0) - s);
00189 SplinePair xy = onems * onems * onems * m_xy [i] +
00190 SplinePair (3.0) * onems * onems * s * m_p1 [i] +
00191 SplinePair (3.0) * onems * s * s * m_p2 [i] +
00192 s * s * s * m_xy[i + 1];
00193 return xy;
00194 }
00195 }
00196
00197
00198 ENGAUGE_ASSERT (false);
00199 return m_t[0];
00200 }
00201
00202 SplinePair Spline::p1 (unsigned int i) const
00203 {
00204 ENGAUGE_ASSERT (i < (unsigned int) m_p1.size ());
00205
00206 return m_p1 [i];
00207 }
00208
00209 SplinePair Spline::p2 (unsigned int i) const
00210 {
00211 ENGAUGE_ASSERT (i < (unsigned int) m_p2.size ());
00212
00213 return m_p2 [i];
00214 }