parent
bf75622652
commit
e85dbd43b8
@ -1,120 +0,0 @@ |
||||
#include "Arduino.h" |
||||
#include "spline.h" |
||||
#include <math.h> |
||||
|
||||
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 ); |
||||
} |
||||
} |
||||
} |
||||
return (0.0); |
||||
} |
||||
|
||||
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]); |
||||
} |
||||
} |
||||
return (0.0); |
||||
} |
||||
|
||||
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]); |
||||
} |
||||
} |
||||
@ -1,42 +0,0 @@ |
||||
/*
|
||||
Library for 1-d splines |
||||
Copyright Ryan Michael |
||||
Licensed under the GPLv3 |
||||
*/ |
||||
#ifndef spline_h |
||||
#define spline_h |
||||
|
||||
#include "Arduino.h" |
||||
|
||||
#define Hermite 10 |
||||
#define Catmull 11 |
||||
|
||||
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 ); |
||||
void setDegree( int degree ); |
||||
|
||||
private: |
||||
float calc( float, int); |
||||
float* _x; |
||||
float* _y; |
||||
float* _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 ); |
||||
}; |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue