ecl_geometry: ecl_geometry: cubic_spline_blueprints.cpp Source File

00001 
00012 /*****************************************************************************
00013 ** Includes
00014 *****************************************************************************/
00015 
00016 #include <ecl/containers/array.hpp>
00017 #include <ecl/exceptions/standard_exception.hpp>
00018 #include "../../include/ecl/geometry/cubic_spline.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace ecl {
00025 namespace blueprints {
00026 
00027 /*****************************************************************************
00028 ** Using
00029 *****************************************************************************/
00030 
00031 using ecl::Array;
00032 using ecl::OutOfRangeError;
00033 using ecl::StandardException;
00034 
00035 /*****************************************************************************
00036 ** Implementation [C2CubicSpline]
00037 *****************************************************************************/
00038 /*
00039  * This algorithm can be found in page 115 of "Numerical Recipes in C"
00040  * [The art of scientific computing] by Press, Vetterling, Tuekolsky, Flannery.
00041  */
00042 C2CubicSpline::C2CubicSpline(const ecl::Array<double>& x_set, const ecl::Array<double>& y_set,
00043                                 const double ydot_0, const double ydot_f) ecl_assert_throw_decl(StandardException) :
00044                                     x_data(x_set),
00045                                     y_data(y_set)
00046 {
00047     ecl_assert_throw( x_data.size() == y_data.size(), StandardException(LOC,OutOfRangeError) );
00048     unsigned int n = x_data.size();
00049     yddot_data.resize(n);
00050     Array<double> u(n);  // u is a temporary used in the algorithm
00051 
00052     // Initial boundary conditions
00053     yddot_data[0] = -0.5;
00054     u[0] = (3.0/(x_data[1]-x_data[0])) * ((y_data[1]-y_data[0])/(x_data[1]-x_data[0])-ydot_0);
00055     // Set up the tridiagonal matrix to solve for the accelerations
00056     for (unsigned int i = 1; i <= n-2; ++i){
00057         double sig = (x_data[i]-x_data[i-1]) / (x_data[i+1]-x_data[i-1]);
00058         double p = sig*yddot_data[i-1]+2.0;
00059         yddot_data[i] = (sig-1.0)/p;
00060         u[i] = (y_data[i+1]-y_data[i])/(x_data[i+1]-x_data[i]) -
00061                (y_data[i]-y_data[i-1])/(x_data[i]-x_data[i-1]);
00062         u[i] = (6.0*u[i]/(x_data[i+1]-x_data[i-1]) - sig*u[i-1])/p;
00063     }
00064     // Final boundary conditions
00065     double qn = 0.5;
00066     u[n-1] = (3.0/(x_data[n-1]-x_data[n-2])) * (ydot_f - (y_data[n-1]-y_data[n-2])/(x_data[n-1]-x_data[n-2]));
00067 
00068     // Back substitution loop of the tridiagonal algorithm
00069     yddot_data[n-1] = ( u[n-1] - qn*u[n-2]) / ( qn*yddot_data[n-2] + 1.0 );
00070     for ( int k = n-2; k >= 0; --k ) {
00071         yddot_data[k] = yddot_data[k]*yddot_data[k+1] + u[k];
00072     }
00073 }
00074 
00075 /*
00076  * This is a special case of the above. You'll note that only initial and
00077  * boundary conditions change here.
00078  */
00079 C2CubicSpline::C2CubicSpline(const ecl::Array<double>& x_set, const ecl::Array<double>& y_set) ecl_assert_throw_decl(StandardException) :
00080                                     x_data(x_set),
00081                                     y_data(y_set)
00082 {
00083     ecl_assert_throw( x_data.size() == y_data.size(), StandardException(LOC,OutOfRangeError) );
00084     unsigned int n = x_data.size();
00085     yddot_data.resize(n);
00086     Array<double> u(n);  // u is a temporary used in the algorithm
00087 
00088     // Initial boundary conditions
00089     yddot_data[0] = 0.0;
00090     u[0] = 0.0;
00091     // Set up the tridiagonal matrix to solve for the accelerations
00092     for (unsigned int i = 1; i <= n-2; ++i){
00093         double sig = (x_data[i]-x_data[i-1]) / (x_data[i+1]-x_data[i-1]);
00094         double p = sig*yddot_data[i-1]+2.0;
00095         yddot_data[i] = (sig-1.0)/p;
00096         u[i] = (y_data[i+1]-y_data[i])/(x_data[i+1]-x_data[i]) -
00097                (y_data[i]-y_data[i-1])/(x_data[i]-x_data[i-1]);
00098         u[i] = (6.0*u[i]/(x_data[i+1]-x_data[i-1]) - sig*u[i-1])/p;
00099     }
00100     // Final boundary conditions
00101     double qn = 0.0;
00102     u[n-1] = 0.0;
00103 
00104     // Back substitution loop of the tridiagonal algorithm
00105     yddot_data[n-1] = ( u[n-1] - qn*u[n-2]) / ( qn*yddot_data[n-2] + 1.0 );
00106     for ( int k = n-2; k >= 0; --k ) {
00107         yddot_data[k] = yddot_data[k]*yddot_data[k+1] + u[k];
00108     }
00109 }
00110 
00111 ecl::CubicSpline C2CubicSpline::instantiate() {
00112     CubicSpline cubic;
00113     apply(cubic);
00114     return cubic;
00115 };
00116 
00117 void C2CubicSpline::apply(ecl::CubicSpline& spline) const {
00118 
00119     spline.discretised_domain = x_data;
00120     spline.cubic_polynomials.resize(x_data.size()-1); // One less polynomials than there is points
00121     for (unsigned int i = 0; i < spline.cubic_polynomials.size(); ++i ) {
00122         spline.cubic_polynomials[i] = CubicPolynomial::SecondDerivativeInterpolation(
00123                         x_data[i],   y_data[i],   yddot_data[i],
00124                         x_data[i+1], y_data[i+1], yddot_data[i+1]  );
00125     }
00126 };
00127 
00128 
00129 /*****************************************************************************
00130 ** Implementation [DerivativeHeuristicCubicSpline]
00131 *****************************************************************************/
00132 
00133 DerivativeHeuristicCubicSpline::DerivativeHeuristicCubicSpline(const ecl::Array<double>& x_set, const ecl::Array<double>& y_set,
00134         const double ydot_0, const double ydot_f) ecl_assert_throw_decl(StandardException) :
00135         x_data(x_set),
00136         y_data(y_set)
00137 {
00138     ecl_assert_throw( x_data.size() == y_data.size(), StandardException(LOC,OutOfRangeError) );
00139     /*********************
00140     ** Via Velocities
00141     **********************/
00142     ydot_data.resize(x_data.size());
00143     ydot_data[0] = ydot_0;
00144     for (unsigned int i = 1; i < (x_data.size()-1); ++i ) { // Skip first and last.
00145         double ydot_before, ydot_after;
00146         ydot_before = (y_data[i]-y_data[i-1])/(x_data[i]-x_data[i-1]);
00147         ydot_after  = (y_data[i+1]-y_data[i])/(x_data[i+1]-x_data[i]);
00148         ydot_data[i] = (ydot_before + ydot_after)/2;
00149     }
00150     ydot_data[x_data.size()-1] = ydot_f;
00151 }
00152 
00153 ecl::CubicSpline DerivativeHeuristicCubicSpline::instantiate() {
00154     CubicSpline cubic;
00155     apply(cubic);
00156     return cubic;
00157 };
00158 
00159 void DerivativeHeuristicCubicSpline::apply(ecl::CubicSpline& spline) const {
00160 
00161     spline.discretised_domain = x_data;
00162     spline.cubic_polynomials.resize(x_data.size()-1); // One less polynomials than there is points
00163     for (unsigned int i = 0; i < spline.cubic_polynomials.size(); ++i ) {
00164         spline.cubic_polynomials[i] = CubicPolynomial::DerivativeInterpolation(
00165                         x_data[i],   y_data[i],   ydot_data[i],
00166                         x_data[i+1], y_data[i+1], ydot_data[i+1]  );
00167     }
00168 };
00169 
00170 } // namespace blueprints
00171 
00172 /*****************************************************************************
00173 ** Implementation [BluePrintFactory][CubicSpline]
00174 *****************************************************************************/
00175 
00176 using blueprints::C2CubicSpline;
00177 using blueprints::DerivativeHeuristicCubicSpline;
00178 
00179 C2CubicSpline BluePrintFactory< CubicSpline >::Natural(const Array<double>& x_set, const Array<double>& y_set) {
00180     return C2CubicSpline(x_set, y_set);
00181 };
00182 
00183             C2CubicSpline BluePrintFactory< CubicSpline >::ContinuousDerivatives(
00184             const Array<double>& x_set, const Array<double>& y_set, const double ydot_0, const double ydot_f) {
00185     return C2CubicSpline(x_set, y_set, ydot_0, ydot_f);
00186 };
00187 
00188 DerivativeHeuristicCubicSpline BluePrintFactory< CubicSpline >::DerivativeHeuristic(
00189             const Array<double>& x_set, const Array<double>& y_set, const double ydot_0, const double ydot_f) {
00190     return DerivativeHeuristicCubicSpline(x_set, y_set, ydot_0, ydot_f);
00191 }
00192 
00193 } // namespace ecl