polynomial_blueprints.cpp
Go to the documentation of this file.
00001 
00014 /*****************************************************************************
00015 ** Includes
00016 *****************************************************************************/
00017 
00018 #include "../../include/ecl/geometry/polynomial.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace ecl {
00025 namespace blueprints {
00026 
00027 /*****************************************************************************
00028 ** Using
00029 *****************************************************************************/
00030 
00031 using ecl::LinearFunction;
00032 using ecl::CubicPolynomial;
00033 using ecl::QuinticPolynomial;
00034 
00035 /*****************************************************************************
00036 ** Implementation [LinearInterpolation][LinearPointSlopeForm]
00037 *****************************************************************************/
00038 
00039 ecl::LinearFunction LinearInterpolation::instantiate() {
00040     LinearFunction function;
00041     apply(function);
00042     return function;
00043 }
00044 
00045 void LinearInterpolation::apply(ecl::LinearFunction& function) const {
00046     LinearFunction::Coefficients &coefficients = function.coefficients();
00047     // First compute the polynomial on [0,xf-xi] (this is easier), then shift it.
00048 
00049     // y = a_1 x + a_0
00050     double a_1 = (y_final-y_initial)/(x_final-x_initial);
00051     double a_0 = y_initial - a_1*x_initial;
00052 
00053     coefficients <<  a_0, a_1;
00054 }
00055 
00056 ecl::LinearFunction LinearPointSlopeForm::instantiate() {
00057     LinearFunction function;
00058     apply(function);
00059     return function;
00060 }
00061 
00062 void LinearPointSlopeForm::apply(ecl::LinearFunction& function) const {
00063     LinearFunction::Coefficients &coefficients = function.coefficients();
00064     // First compute the polynomial on [0,xf-xi] (this is easier), then shift it.
00065 
00066     // y = a_1 x + a_0
00067     double a_1 = slope;
00068     double a_0 = y_final - a_1*x_final;
00069 
00070     coefficients <<  a_0, a_1;
00071 }
00072 
00073 
00074 /*****************************************************************************
00075 ** Implementation [CubicDerivativeInterpolation]
00076 *****************************************************************************/
00077 
00078 
00079 ecl::CubicPolynomial CubicDerivativeInterpolation::instantiate() {
00080     CubicPolynomial cubic;
00081     apply(cubic);
00082     return cubic;
00083 }
00084 
00085 void CubicDerivativeInterpolation::apply(ecl::CubicPolynomial& polynomial) const {
00086     Polynomial<3>::Coefficients &coefficients = polynomial.coefficients();
00087     // First compute the polynomial on [0,xf-xi] (this is easier), then shift it.
00088     double dx = x_final - x_initial;
00089     double dy = y_final - y_initial;
00090     coefficients << y_initial,
00091                     ydot_initial,
00092                     (3/(dx*dx))*(dy) - (2/dx)*ydot_initial - (1/dx)*ydot_final,
00093                     (-2/(dx*dx*dx))*(dy) + (ydot_final + ydot_initial)/(dx*dx);
00094 
00095 //    Matrix<double,4,4> A;
00096 //      Matrix<double,4,1> a,b;
00097 //      b << y_initial, ydot_initial, y_final, ydot_final;
00098 //
00099 //      A <<     1,     0,     0,     0,
00100 //                       0,     1,     0,     0,
00101 //                       1,        dx, dx*dx, dx*dx*dx,
00102 //                       0,     1,  2*dx,  3*dx*dx;
00103 //
00104 //      a = A.inverse()*b;
00105 //      std::cout << coefficients << std::endl;
00106 //      std::cout << a << std::endl;
00107 
00108     if ( x_initial != 0.0 ) {
00109         polynomial.shift_horizontal(x_initial);
00110     }
00111 }
00112 
00113 /*****************************************************************************
00114 ** Implementation [CubicDerivativeInterpolation]
00115 *****************************************************************************/
00116 
00117 ecl::CubicPolynomial CubicSecondDerivativeInterpolation::instantiate() {
00118     CubicPolynomial cubic;
00119     apply(cubic);
00120     return cubic;
00121 }
00122 
00123 void CubicSecondDerivativeInterpolation::apply(ecl::CubicPolynomial& polynomial) const {
00124     Polynomial<3>::Coefficients &coefficients = polynomial.coefficients();
00125     // First compute the polynomial on [0,xf-xi] (this is easier), then shift it.
00126     double dx = x_final - x_initial;
00127     double a_0 = y_initial;
00128     double a_2 = yddot_initial/2;
00129     double a_3 = (yddot_final - yddot_initial)/(6*dx);
00130     double a_1 = (y_final-a_0-a_2*dx*dx - a_3*dx*dx*dx)/dx;
00131     coefficients <<  a_0, a_1, a_2, a_3;
00132     if ( x_initial != 0.0 ) {
00133         polynomial.shift_horizontal(x_initial);
00134     }
00135 }
00136 
00137 /*****************************************************************************
00138 ** Implementation [QuinticInterpolation]
00139 *****************************************************************************/
00140 
00141 ecl::QuinticPolynomial QuinticInterpolation::instantiate() {
00142     QuinticPolynomial quintic;
00143     apply(quintic);
00144     return quintic;
00145 }
00146 
00147 void QuinticInterpolation::apply(ecl::QuinticPolynomial& polynomial) const {
00148     QuinticPolynomial::Coefficients &coefficients = polynomial.coefficients();
00149     // First compute the polynomial on [0,xf-xi] (this is easier), then shift it.
00150 
00151     double dx = x_final - x_initial;
00152     double d2x = dx*dx;
00153     double d3x = d2x*dx;
00154     double d4x = d3x*dx;
00155     double d5x = d4x*dx;
00156     double a_0 = y_initial;
00157     double a_1 = ydot_initial;
00158     double a_2 = yddot_initial/2;
00159     double a_3 = ( 20*(y_final - y_initial) - (8*ydot_final + 12*ydot_initial)*dx - (3*yddot_initial - yddot_final)*dx*dx )
00160                             / (2*d3x);
00161     double a_4 = ( 30*(y_initial - y_final) + (14*ydot_final + 16*ydot_initial)*dx + (- 2*yddot_final + 3*yddot_initial)*dx*dx )
00162                             / (2*d4x);
00163     double a_5 = ( 12*(y_final - y_initial) - (6*ydot_final + 6*ydot_initial)*dx - (yddot_initial - yddot_final)*dx*dx )
00164                             / (2*d5x);
00165 
00166     coefficients <<  a_0, a_1, a_2, a_3, a_4, a_5;
00167 
00168 //    Matrix<double,6,6> A;
00169 //    Matrix<double,6,1> a,b;
00170 //    b << y_initial, ydot_initial, yddot_initial, y_final, ydot_final, yddot_final;
00171 //
00172 //    A <<     1,     0,     0,     0,      0,      0,
00173 //             0,     1,     0,     0,      0,      0,
00174 //             0,     0,     2,     0,      0,      0,
00175 //             1,    dx,   d2x,   d3x,    d4x,    d5x,
00176 //             0,     1,  2*dx, 3*d2x,  4*d3x,  5*d4x,
00177 //             0,     0,    2,   6*dx, 12*d2x, 20*d3x;
00178 //
00179 //    a = A.inverse()*b;
00180 //    std::cout << coefficients << std::endl;
00181 //    std::cout << a << std::endl;
00182 
00183     if ( x_initial != 0.0 ) {
00184         polynomial.shift_horizontal(x_initial);
00185     }
00186 }
00187 
00188 
00189 }; // namespace blueprints
00190 
00191 using blueprints::LinearInterpolation;
00192 using blueprints::LinearPointSlopeForm;
00193 using blueprints::CubicDerivativeInterpolation;
00194 using blueprints::CubicSecondDerivativeInterpolation;
00195 using blueprints::QuinticInterpolation;
00196 
00197 /*****************************************************************************
00198 ** BluePrintFactory[LinearFunction]
00199 *****************************************************************************/
00200 
00201 LinearInterpolation BluePrintFactory< LinearFunction >::Interpolation(const double x_i, const double y_i, const double x_f, const double y_f) {
00202     return LinearInterpolation(x_i, y_i, x_f, y_f);
00203 }
00204 
00205 LinearPointSlopeForm BluePrintFactory< LinearFunction >::PointSlopeForm(const double x_f, const double y_f, const double slope) {
00206     return LinearPointSlopeForm(x_f, y_f, slope);
00207 }
00208 
00209 /*****************************************************************************
00210 ** BluePrintFactory[CubicPolynomial]
00211 *****************************************************************************/
00212 
00213 CubicDerivativeInterpolation BluePrintFactory< CubicPolynomial >::DerivativeInterpolation(const double x_i, const double y_i, const double ydot_i, const double x_f, const double y_f, const double ydot_f) {
00214     return CubicDerivativeInterpolation(x_i, y_i, ydot_i, x_f, y_f, ydot_f);
00215 }
00216 
00217 CubicSecondDerivativeInterpolation BluePrintFactory< CubicPolynomial >::SecondDerivativeInterpolation(const double x_i, const double y_i, const double yddot_i, const double x_f, const double y_f, const double yddot_f) {
00218     return CubicSecondDerivativeInterpolation(x_i, y_i, yddot_i, x_f, y_f, yddot_f);
00219 }
00220 
00221 /*****************************************************************************
00222 ** BluePrintFactory[QuinticPolynomial]
00223 *****************************************************************************/
00224 
00225 QuinticInterpolation BluePrintFactory< QuinticPolynomial >::Interpolation(const double x_i, const double y_i, const double ydot_i, const double yddot_i,
00226                             const double x_f, const double y_f, const double ydot_f, const double yddot_f) {
00227     return QuinticInterpolation(x_i, y_i, ydot_i, yddot_i, x_f, y_f, ydot_f, yddot_f);
00228 }
00229 
00230 
00231 } // namespace ecl


ecl_geometry
Author(s): Daniel Stonier
autogenerated on Wed Aug 26 2015 11:27:46