smooth_linear_spline.cpp
Go to the documentation of this file.
00001 
00011 /*****************************************************************************
00012 ** Includes
00013 *****************************************************************************/
00014 
00015 #include <iostream>
00016 #include <cmath>
00017 #include <ecl/exceptions/standard_exception.hpp>
00018 #include "../../include/ecl/geometry/smooth_linear_spline.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace ecl {
00025 
00026 /*****************************************************************************
00027 ** Implementation
00028 *****************************************************************************/
00029 
00030 SmoothLinearSpline::SmoothLinearSpline(const Array<double>& x_data, const Array<double>& y_data, double a_max) throw (DataException<int>) {
00031 
00032         if ( x_data.size() != y_data.size() ) {
00033                 throw DataException<int>(LOC,OutOfRangeError,"Input domain and range sets were not the same size.", 0);
00034         }
00035 
00036     /*********************
00037     ** Basic Parameters
00038     **********************/
00039         // n = number of linear segments
00040         // 2*n = number of points (2 end points + 2*(n-1) corners) remember we're including the polynomial corners in this calculation
00041     unsigned int n = x_data.size()-1;
00042 
00043     /******************************************
00044         ** Storage
00045         *******************************************/
00046     segments.resize(n);
00047     corners.resize(n-1);
00048     discretised_domain.resize(2*n);
00049 
00050     /******************************************
00051         ** Build Segments
00052         *******************************************/
00053     for ( unsigned int i = 0; i < n; ++i ) {
00054         segments[i] = LinearFunction::Interpolation(x_data[i],y_data[i],x_data[i+1],y_data[i+1]);
00055     }
00056         discretised_domain[0] = x_data[0];    // Initial point
00057         discretised_domain[2*n-1] = x_data[n];  // Last point
00058 
00059         /******************************************
00060         ** Build Corners (this might throw)
00061         *******************************************/
00062     for ( unsigned int i = 1; i < n; ++i ) {
00063         if ( segments[i-1].coefficients()[1] == segments[i].coefficients()[1] ) { // slope identical
00064                 // Just set the quintic to be a linear function.
00065                 corners[i-1].coefficients() << segments[i-1].coefficients()[0], segments[i-1].coefficients()[1], 0.0, 0.0, 0.0, 0.0;
00066                 discretised_domain[1+2*(i-1)] = x_data[i];
00067                 discretised_domain[1+2*(i-1)+1] = x_data[i];
00068         } else {
00069                         for ( unsigned int j = 1; j <= 5; ++j ) {
00070                                 double x_l, x_r;
00071                                 if ( i == 1 ) { // First one, we can use the whole segment
00072                                         x_l = x_data[i] - j*(x_data[i]-x_data[i-1])/5;
00073                                 } else {
00074                                         x_l = x_data[i] - j*(x_data[i]-x_data[i-1])/10;
00075                                 }
00076                                 if ( i == (n-1) ) { // Last one, we can use the whole segment
00077                                         x_r = x_data[i] + j*(x_data[i+1]-x_data[i])/5;
00078                                 } else {
00079                                         x_r = x_data[i] + j*(x_data[i+1]-x_data[i])/10;
00080                                 }
00081                                 double y_l = segments[i-1](x_l);
00082                                 double y_r = segments[i](x_r);
00083                                 double ydot_l = segments[i-1].derivative(x_l);
00084                                 double ydot_r = segments[i].derivative(x_r);
00085                                 corners[i-1] = QuinticPolynomial::Interpolation(x_l,y_l,ydot_l,0.0,
00086                                                                                                                                 x_r,y_r,ydot_r,0.0);
00087                                 if ( ( fabs( Maximum<CubicPolynomial>()(x_l,x_r,corners[i-1].derivative().derivative())) < fabs(a_max) ) &&
00088                                                 ( fabs( Minimum<CubicPolynomial>()(x_l,x_r,corners[i-1].derivative().derivative())) < fabs(a_max) ) ) {
00089                                 discretised_domain[1+2*(i-1)] = x_l;
00090                                 discretised_domain[1+2*(i-1)+1] = x_r;
00091                                         break;
00092                                 }
00093                                 /*********************
00094                                 ** Debugging
00095                                 **********************/
00096 //                              if ( i == 1 ) {
00097 //                                      double max = fabs( Maximum<CubicPolynomial>()(x_l,x_r,corners[i-1].derivative().derivative()));
00098 //                                      if ( fabs( Minimum<CubicPolynomial>()(x_l,x_r,corners[i-1].derivative().derivative())) > max ) {
00099 //                                              max = fabs( Minimum<CubicPolynomial>()(x_l,x_r,corners[i-1].derivative().derivative()));
00100 //                                      }
00101 //                                      std::cout << "Max: " << max << std::endl;
00102 //                              }
00103 
00104                                 if ( j == 5 ) { // Cannot exceed more than half the length of the segment.
00105                                         throw DataException<int>(LOC,ConstructorError,"Max acceleration bound could not be satisfied at corner specified by the data element.",i);
00106                                 }
00107                         }
00108         }
00109     }
00110 }
00111 
00112 double SmoothLinearSpline::operator()(const double &x) const ecl_assert_throw_decl(StandardException) {
00113     ecl_assert_throw( ( ( x >= discretised_domain.front() ) && ( x <= discretised_domain.back() ) ), StandardException(LOC,OutOfRangeError) );
00114     int index = 0;
00115     while ( x > discretised_domain[index+1] ) {
00116         ++index;
00117     }
00118     if ( index % 2 == 0 ) { // linear
00119         return segments[index/2](x);
00120     } else { // quintic
00121         return corners[(index-1)/2](x);
00122     }
00123 }
00124 
00125 double SmoothLinearSpline::derivative(const double &x) const ecl_assert_throw_decl(StandardException) {
00126     ecl_assert_throw( ( ( x >= discretised_domain.front() ) && ( x <= discretised_domain.back() ) ), StandardException(LOC,OutOfRangeError) );
00127     int index = 0;
00128     while ( x > discretised_domain[index+1] ) {
00129         ++index;
00130     }
00131     if ( index % 2 == 0 ) { // linear
00132         return segments[index/2].derivative(x);
00133     } else { // quintic
00134         return corners[(index-1)/2].derivative(x);
00135     }
00136 }
00137 
00138 double SmoothLinearSpline::dderivative(const double &x) const ecl_assert_throw_decl(StandardException) {
00139     ecl_assert_throw( ( ( x >= discretised_domain.front() ) && ( x <= discretised_domain.back() ) ), StandardException(LOC,OutOfRangeError) );
00140     int index = 0;
00141     while ( x > discretised_domain[index+1] ) {
00142         ++index;
00143     }
00144     if ( index % 2 == 0 ) { // linear
00145         return segments[index/2].dderivative(x);
00146     } else { // quintic
00147         return corners[(index-1)/2].dderivative(x);
00148     }
00149 }
00150 
00151 } // namespace ecl
00152 
00153 
00154 
00155 


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