diff --git a/examples/LinearInterpolation/LinearInterpolation.ino b/examples/LinearInterpolation/LinearInterpolation.ino new file mode 100644 index 0000000..774f0d1 --- /dev/null +++ b/examples/LinearInterpolation/LinearInterpolation.ino @@ -0,0 +1,25 @@ +#include + +const int NUM_POINTS = 39; + +void setup(void) +{ + Serial.begin(115200); + + const double real[NUM_POINTS] = {0.00000, 0.07671, 0.18448, 0.32294, 0.48787, 0.74062, 1.16904, 1.64501, 1.99595, 2.19846, 2.32158, 2.40366, 2.46771, 2.53982, 2.61921, 2.72085, 2.85931, 3.03382, 3.25589, 3.50020, 3.74145, 3.94473, 4.10735, 4.25348, 4.38273, 4.51084, 4.63395, 4.76129, 4.90358, 5.06544, 5.24494, 5.42520, 5.59204, 5.74776, 5.88238, 6.01739, 6.12977, 6.22680, 6.28319}; + const double ideal[NUM_POINTS] = {0.00000, 0.16535, 0.33069, 0.49604, 0.66139, 0.82673, 0.99208, 1.15743, 1.32278, 1.48812, 1.65347, 1.81882, 1.98416, 2.14951, 2.31486, 2.48020, 2.64555, 2.81090, 2.97625, 3.14159, 3.30694, 3.47229, 3.63763, 3.80298, 3.96833, 4.13367, 4.29902, 4.46437, 4.62972, 4.79506, 4.96041, 5.12576, 5.29110, 5.45645, 5.62180, 5.78714, 5.95249, 6.11784, 6.28319}; + + Spline spline(real, ideal, NUM_POINTS); // Using Linear interpolation as default + + for( int j = 0; j < 5; j++ ) + { + for( int i = 0; i <= NUM_POINTS; i+= 1 ) + { + double temp = spline.value(real[i]); + // Use Serial Plotter to see the output + Serial.println(temp); + } + } +} + +void loop(void) {} diff --git a/spline.cpp b/spline.cpp deleted file mode 100644 index 4ddfa48..0000000 --- a/spline.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include "WProgram.h" -#include "spline.h" -#include - -Spline::Spline(void) { - _prev_point = 0; -} - -Spline::Spline( float x[], float y[], int numPoints, int degree ) -{ - setPoints(x,y,numPoints); - setDegree(degree); - _prev_point = 0; -} - -Spline::Spline( float x[], float y[], float m[], int numPoints ) -{ - setPoints(x,y,m,numPoints); - setDegree(Hermite); - _prev_point = 0; -} - -void Spline::setPoints( float x[], float y[], int numPoints ) { - _x = x; - _y = y; - _length = numPoints; -} - -void Spline::setPoints( float x[], float y[], float m[], int numPoints ) { - _x = x; - _y = y; - _m = m; - _length = numPoints; -} - -void Spline::setDegree( int degree ){ - _degree = degree; -} - -float Spline::value( float x ) -{ - if( _x[0] > x ) { - return _y[0]; - } - else if ( _x[_length-1] < x ) { - return _y[_length-1]; - } - else { - for(int i = 0; i < _length; i++ ) - { - int index = ( i + _prev_point ) % _length; - - if( _x[index] == x ) { - _prev_point = index; - return _y[index]; - } else if( (_x[index] < x) && (x < _x[index+1]) ) { - _prev_point = index; - return calc( x, index ); - } - } - } -} - -float Spline::calc( float x, int i ) -{ - switch( _degree ) { - case 0: - return _y[i]; - case 1: - if( _x[i] == _x[i+1] ) { - // Avoids division by 0 - return _y[i]; - } else { - return _y[i] + (_y[i+1] - _y[i]) * ( x - _x[i]) / ( _x[i+1] - _x[i] ); - } - case Hermite: - return hermite( ((x-_x[i]) / (_x[i+1]-_x[i])), _y[i], _y[i+1], _m[i], _m[i+1], _x[i], _x[i+1] ); - case Catmull: - if( i == 0 ) { - // x prior to spline start - first point used to determine tangent - return _y[1]; - } else if( i == _length-2 ) { - // x after spline end - last point used to determine tangent - return _y[_length-2]; - } else { - float t = (x-_x[i]) / (_x[i+1]-_x[i]); - float m0 = (i==0 ? 0 : catmull_tangent(i) ); - float m1 = (i==_length-1 ? 0 : catmull_tangent(i+1) ); - return hermite( t, _y[i], _y[i+1], m0, m1, _x[i], _x[i+1]); - } - } -} - -float Spline::hermite( float t, float p0, float p1, float m0, float m1, float x0, float x1 ) { - return (hermite_00(t)*p0) + (hermite_10(t)*(x1-x0)*m0) + (hermite_01(t)*p1) + (hermite_11(t)*(x1-x0)*m1); -} -float Spline::hermite_00( float t ) { return (2*pow(t,3)) - (3*pow(t,2)) + 1;} -float Spline::hermite_10( float t ) { return pow(t,3) - (2*pow(t,2)) + t; } -float Spline::hermite_01( float t ) { return (3*pow(t,2)) - (2*pow(t,3)); } -float Spline::hermite_11( float t ) { return pow(t,3) - pow(t,2); } - -float Spline::catmull_tangent( int i ) -{ - if( _x[i+1] == _x[i-1] ) { - // Avoids division by 0 - return 0; - } else { - return (_y[i+1] - _y[i-1]) / (_x[i+1] - _x[i-1]); - } -} diff --git a/spline.h b/spline.h index 7638932..dae42f3 100644 --- a/spline.h +++ b/spline.h @@ -6,37 +6,168 @@ #ifndef spline_h #define spline_h -#include "WProgram.h" +#include #define Hermite 10 #define Catmull 11 +template class Spline { public: Spline( void ); - Spline( float x[], float y[], int numPoints, int degree = 1 ); - Spline( float x[], float y[], float m[], int numPoints ); - float value( float x ); - void setPoints( float x[], float y[], int numPoints ); - void setPoints( float x[], float y[], float m[], int numPoints ); + Spline( T x[], T y[], int numPoints, int degree = 1 ); + Spline( T x[], T y[], T m[], int numPoints ); + T value( T x ); + void setPoints( T x[], T y[], int numPoints ); + void setPoints( T x[], T y[], T m[], int numPoints ); void setDegree( int degree ); private: - float calc( float, int); - float* _x; - float* _y; - float* _m; + T calc( T, int); + T* _x; + T* _y; + T* _m; int _degree; int _length; int _prev_point; - float hermite( float t, float p0, float p1, float m0, float m1, float x0, float x1 ); - float hermite_00( float t ); - float hermite_10( float t ); - float hermite_01( float t ); - float hermite_11( float t ); - float catmull_tangent( int i ); + T hermite( T t, T p0, T p1, T m0, T m1, T x0, T x1 ); + inline T hermite_00( T t ); + inline T hermite_10( T t ); + inline T hermite_01( T t ); + inline T hermite_11( T t ); + T catmull_tangent( int i ); }; -#endif +template +Spline::Spline(void) { + _prev_point = 0; +} + +template +Spline::Spline( T x[], T y[], int numPoints, int degree ) +{ + setPoints(x, y, numPoints); + setDegree(degree); + _prev_point = 0; +} + +template +Spline::Spline( T x[], T y[], T m[], int numPoints ) +{ + setPoints(x,y,m,numPoints); + setDegree(Hermite); + _prev_point = 0; +} + +template +void Spline::setPoints( T x[], T y[], int numPoints ) { + _x = x; + _y = y; + _length = numPoints; +} + +template +void Spline::setPoints( T x[], T y[], T m[], int numPoints ) { + _x = x; + _y = y; + _m = m; + _length = numPoints; +} + +template +void Spline::setDegree( int degree ){ + _degree = degree; +} + +template +T Spline::value( T x ) +{ + if( _x[0] > x ) { + return _y[0]; + } + else if ( _x[_length-1] < x ) { + return _y[_length-1]; + } + else { + for(int i = 0; i < _length; i++ ) + { + int index = ( i + _prev_point ) % _length; + + if( _x[index] == x ) { + _prev_point = index; + return _y[index]; + } else if( (_x[index] < x) && (x < _x[index+1]) ) { + _prev_point = index; + return calc( x, index ); + } + } + } +} + +template +T Spline::calc( T x, int i ) +{ + switch( _degree ) { + case 0: + return _y[i]; + case 1: + if( _x[i] == _x[i+1] ) { + // Avoids division by 0 + return _y[i]; + } else { + return _y[i] + (_y[i+1] - _y[i]) * ( x - _x[i]) / ( _x[i+1] - _x[i] ); + } + case Hermite: + return hermite( ((x-_x[i]) / (_x[i+1]-_x[i])), _y[i], _y[i+1], _m[i], _m[i+1], _x[i], _x[i+1] ); + case Catmull: + if( i == 0 ) { + // x prior to spline start - first point used to determine tangent + return _y[1]; + } else if( i == _length-2 ) { + // x after spline end - last point used to determine tangent + return _y[_length-2]; + } else { + T t = (x-_x[i]) / (_x[i+1]-_x[i]); + T m0 = (i==0 ? 0 : catmull_tangent(i) ); + T m1 = (i==_length-1 ? 0 : catmull_tangent(i+1) ); + return hermite( t, _y[i], _y[i+1], m0, m1, _x[i], _x[i+1]); + } + } +} + +template +T Spline::hermite( T t, T p0, T p1, T m0, T m1, T x0, T x1 ) { + return (hermite_00(t)*p0) + (hermite_10(t)*(x1-x0)*m0) + (hermite_01(t)*p1) + (hermite_11(t)*(x1-x0)*m1); +} +template <> +inline double Spline::hermite_00( double t ) { return (2*pow(t,3)) - (3*pow(t,2)) + 1;} +template <> +inline double Spline::hermite_10( double t ) { return pow(t,3) - (2*pow(t,2)) + t; } +template <> +inline double Spline::hermite_01( double t ) { return (3*pow(t,2)) - (2*pow(t,3)); } +template <> +inline double Spline::hermite_11( double t ) { return pow(t,3) - pow(t,2); } + +template <> +inline float Spline::hermite_00( float t ) { return (2*powf(t,3)) - (3*powf(t,2)) + 1;} +template <> +inline float Spline::hermite_10( float t ) { return powf(t,3) - (2*powf(t,2)) + t; } +template <> +inline float Spline::hermite_01( float t ) { return (3*powf(t,2)) - (2*powf(t,3)); } +template <> +inline float Spline::hermite_11( float t ) { return powf(t,3) - powf(t,2); } + +template +T Spline::catmull_tangent( int i ) +{ + if( _x[i+1] == _x[i-1] ) { + // Avoids division by 0 + return 0; + } else { + return (_y[i+1] - _y[i-1]) / (_x[i+1] - _x[i-1]); + } +} + +#endif \ No newline at end of file