Template class for using float/double

examples/LinearInterpolation/LinearInterpolation.ino

@@ -0,0 +1,25 @@
+#include <spline.h>
+
+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<double> 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) {}

spline.h

@@ -6,37 +6,168 @@
 #ifndef spline_h
 #define spline_h
 
-#include "WProgram.h"
+#include <math.h>
 
 #define Hermite 10
 #define Catmull 11
 
+template<class T>
 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 <typename T>
+Spline<T>::Spline(void) {
+  _prev_point = 0;
+}
+
+template <typename T>
+Spline<T>::Spline( T x[], T y[], int numPoints, int degree )
+{
+  setPoints(x, y, numPoints);
+  setDegree(degree);
+  _prev_point = 0;
+}
+
+template <typename T>
+Spline<T>::Spline( T x[], T y[], T m[], int numPoints )
+{
+  setPoints(x,y,m,numPoints);
+  setDegree(Hermite);
+  _prev_point = 0;
+}
+
+template <typename T>
+void Spline<T>::setPoints( T x[], T y[], int numPoints ) {
+  _x = x;
+  _y = y;
+  _length = numPoints;
+}
+
+template <typename T>
+void Spline<T>::setPoints( T x[], T y[], T m[], int numPoints ) {
+  _x = x;
+  _y = y;
+  _m = m;
+  _length = numPoints;
+}
+
+template <typename T>
+void Spline<T>::setDegree( int degree ){
+  _degree = degree;
+}
+
+template <typename T>
+T Spline<T>::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 <typename T>
+T Spline<T>::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 <typename T>
+T Spline<T>::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<double>::hermite_00( double t ) { return (2*pow(t,3)) - (3*pow(t,2)) + 1;}
+template <>
+inline double Spline<double>::hermite_10( double t ) { return pow(t,3) - (2*pow(t,2)) + t; }
+template <>
+inline double Spline<double>::hermite_01( double t ) { return (3*pow(t,2)) - (2*pow(t,3)); }
+template <>
+inline double Spline<double>::hermite_11( double t ) { return pow(t,3) - pow(t,2); }
+
+template <>
+inline float Spline<float>::hermite_00( float t ) { return (2*powf(t,3)) - (3*powf(t,2)) + 1;}
+template <>
+inline float Spline<float>::hermite_10( float t ) { return powf(t,3) - (2*powf(t,2)) + t; }
+template <>
+inline float Spline<float>::hermite_01( float t ) { return (3*powf(t,2)) - (2*powf(t,3)); }
+template <>
+inline float Spline<float>::hermite_11( float t ) { return powf(t,3) - powf(t,2); }
+
+template <typename T>
+T Spline<T>::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