00001
00014
00015
00016
00017
00018 #include "../../include/ecl/geometry/polynomial.hpp"
00019
00020
00021
00022
00023
00024 namespace ecl {
00025 namespace blueprints {
00026
00027
00028
00029
00030
00031 using ecl::LinearFunction;
00032 using ecl::CubicPolynomial;
00033 using ecl::QuinticPolynomial;
00034
00035
00036
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
00048
00049
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
00065
00066
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
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
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
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108 if ( x_initial != 0.0 ) {
00109 polynomial.shift_horizontal(x_initial);
00110 }
00111 }
00112
00113
00114
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
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
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
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
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 if ( x_initial != 0.0 ) {
00184 polynomial.shift_horizontal(x_initial);
00185 }
00186 }
00187
00188
00189 };
00190
00191 using blueprints::LinearInterpolation;
00192 using blueprints::LinearPointSlopeForm;
00193 using blueprints::CubicDerivativeInterpolation;
00194 using blueprints::CubicSecondDerivativeInterpolation;
00195 using blueprints::QuinticInterpolation;
00196
00197
00198
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
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
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 }