Replaced spline interpolation with interpolation from https://github.com/tuxcell/interpolationArduino
parent
78edeeb89c
commit
711f4be2ff
@ -0,0 +1,209 @@ |
||||
/*
|
||||
* interpolation.h |
||||
* |
||||
* interpolation - An interpolation library for Arduino. |
||||
* Author: Jose Gama 2015 |
||||
|
||||
* |
||||
* This library is free software; you can redistribute it |
||||
* and/or modify it under the terms of the GNU Lesser |
||||
* General Public License as published by the Free Software |
||||
* Foundation; either version 3 of the License, or (at |
||||
* your option) any later version. |
||||
* |
||||
* This library is distributed in the hope that it will |
||||
* be useful, but WITHOUT ANY WARRANTY; without even the |
||||
* implied warranty of MERCHANTABILITY or FITNESS FOR A |
||||
* PARTICULAR PURPOSE. See the GNU Lesser General Public |
||||
* License for more details. |
||||
* |
||||
* You should have received a copy of the GNU Lesser |
||||
* General Public License along with this library; if not, |
||||
* write to the Free Software Foundation, Inc., |
||||
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
||||
* |
||||
*/ |
||||
|
||||
/*
|
||||
* From: https://github.com/tuxcell/interpolationArduino
|
||||
* replaced all doubles by float (wirtz@parasitstudio.de) |
||||
*/ |
||||
|
||||
#include "interpolation.h" |
||||
|
||||
interpolation::interpolation(void) { |
||||
_valInterp = 0; |
||||
_lenXY = 0; |
||||
} |
||||
interpolation::interpolation( float x[], float y[], int lenXY){ |
||||
_x = x;_y = y;_lenXY = lenXY; |
||||
_valInterp = 0; |
||||
} |
||||
interpolation::interpolation( float x[], float y[], int lenXY, float valInterp){ |
||||
_x = x;_y = y;_lenXY = lenXY; |
||||
_valInterp = valInterp; |
||||
} |
||||
|
||||
void interpolation::valueI( float valInterp ) { |
||||
_valInterp = valInterp; |
||||
} |
||||
void interpolation::valuelenXY( int lenXY ) { |
||||
_lenXY = lenXY; |
||||
} |
||||
void interpolation::valueX( float x[]) { |
||||
_x = x; |
||||
} |
||||
void interpolation::valueY( float y[]) { |
||||
_y = y; |
||||
} |
||||
void interpolation::valueXM( float XM[]) { |
||||
_XM = XM; |
||||
} |
||||
void interpolation::valueZ( float Z[]) { |
||||
_Z = Z; |
||||
} |
||||
float interpolation::LinearInterpolate() {return(LinearInterp( _x, _y, _lenXY, _valInterp));} |
||||
float interpolation::CosineInterpolate() {return(CosineInterp( _x, _y, _lenXY, _valInterp));} |
||||
float interpolation::CubicInterpolate() {return(CubicInterp( _x, _y, _lenXY, _valInterp));} |
||||
float interpolation::LagrangeInterpolate() {return(LagrangeInterp( _x, _y, _lenXY, _valInterp));} |
||||
float interpolation::QuadraticInterpolate() {return(QuadraticInterp( _x, _y, _lenXY, _valInterp));} |
||||
float interpolation::AkimaInterpolate() {return(AkimaInterp( _x, _y, _XM, _Z, _lenXY, _valInterp));} |
||||
|
||||
|
||||
float interpolation::LinearInterp( float* x, float* y, int n, float p ) |
||||
{ |
||||
//http://paulbourke.net/miscellaneous/interpolation/
|
||||
int i; |
||||
float mu; |
||||
for( i = 0; i < n-1; i++ ) |
||||
{ |
||||
if (( x[i] <= p && x[i+1] >= p )||( x[i] >= p && x[i+1] <= p )) |
||||
{ |
||||
mu=(p - x[i])/(x[i] - x[i+1]); |
||||
if (mu<0) mu=-mu; |
||||
return(y[i]*(1-mu)+y[i+1]*mu); |
||||
} |
||||
} |
||||
return 0; // Not in Range
|
||||
} |
||||
|
||||
float interpolation::CosineInterp (float* x, float* y, int n, float p ) |
||||
{ |
||||
int i; |
||||
float mu, mu2; |
||||
for( i = 0; i < n-1; i++ ) |
||||
{ |
||||
if (( x[i] <= p && x[i+1] >= p )||( x[i] >= p && x[i+1] <= p )) |
||||
{ |
||||
mu=(p - x[i])/(x[i] - x[i+1]); |
||||
if (mu<0) mu=-mu; |
||||
mu2 = (1.0-cos(3.1415926535897*mu))/2.0; |
||||
return(y[i]*(1.0-mu2)+y[i+1]*mu2); |
||||
} |
||||
} |
||||
return 0; // Not in Range
|
||||
} |
||||
|
||||
float interpolation::CubicInterp(float* x, float* y, int n, float p ) |
||||
{ |
||||
int i; |
||||
float a0,a1,a2,a3,mu, mu2; |
||||
for( i = 0; i < n-1; i++ ) |
||||
{ |
||||
if (( x[i] <= p && x[i+1] >= p )||( x[i] >= p && x[i+1] <= p )) |
||||
{ |
||||
mu=(p - x[i])/(x[i] - x[i+1]); |
||||
if (mu<0) mu=-mu; |
||||
mu2 = mu*mu; |
||||
a0 = y[i+2] - y[i+1] - y[i-1] + y[i]; |
||||
a1 = y[i-1] - y[i] - a0; |
||||
a2 = y[i+1] - y[i-1]; |
||||
a3 = y[i]; |
||||
return(a0*mu*mu2+a1*mu2+a2*mu+a3); |
||||
} |
||||
} |
||||
return 0; // Not in Range
|
||||
} |
||||
|
||||
float interpolation::LagrangeInterp( float* x, float* y, int n, float p ) |
||||
{ |
||||
//http://www.dailyfreecode.com/code/lagranges-interpolation-method-finding-2376.aspx
|
||||
int i, j, k; |
||||
float t, r=0; |
||||
for(i=0;i<n;i++) |
||||
{ |
||||
t = 1; |
||||
k = i; |
||||
for(j=0;j<n;j++) |
||||
{ |
||||
if(k==j) |
||||
{ |
||||
continue; |
||||
} |
||||
else |
||||
{ |
||||
t = t * ((p-x[j])/(x[k]-x[j])); |
||||
} |
||||
} |
||||
r+=y[i]*t; |
||||
} |
||||
return r; // Not in Range
|
||||
} |
||||
|
||||
float interpolation::QuadraticInterp(float* x, float* y, int n, float p ) |
||||
{ |
||||
//view-source:http://www.johndcook.com/quadratic_interpolator.html
|
||||
int i; |
||||
float xi2, k; |
||||
for( i = 0; i < n-1; i++ ) |
||||
{ |
||||
if (( x[i] <= p && x[i+1] >= p )||( x[i] >= p && x[i+1] <= p )) |
||||
{ |
||||
if (i<(n-3)) xi2=x[i+2]; else xi2=0; |
||||
k = y[i]*(p - x[i+1])*(p - xi2)/((x[i] - x[i+1])*(x[i] - xi2)); |
||||
k += y[i+1]*(p - x[i])*(p - xi2)/((x[i+1] - x[i])*(x[i+1] - xi2)); |
||||
k += y[i+2]*(p - x[i])*(p - x[i+1])/((xi2 - x[i])*(xi2 - x[i+1])); |
||||
return(k); |
||||
} |
||||
} |
||||
return 0; // Not in Range
|
||||
} |
||||
|
||||
float interpolation::AkimaInterp( float* x, float* y, float* XM, float* Z, int n, float p ) { |
||||
//http://jean-pierre.moreau.pagesperso-orange.fr/Cplus/akima_cpp.txt
|
||||
int i; |
||||
float a,b,r; |
||||
//special case p=0
|
||||
if (p==0.0) { |
||||
return(0); |
||||
} |
||||
//Check to see if interpolation point is correct
|
||||
if (p<x[1] || p>=x[n-3]) { |
||||
return(-330); |
||||
} |
||||
x[0]=2.0*x[1]-x[2]; |
||||
//Calculate Akima coefficients, a and b
|
||||
for (i=1; i<n; i++) |
||||
//Shift i to i+2
|
||||
XM[i+2]=(y[i+1]-y[i])/(x[i+1]-x[i]); |
||||
XM[n+2]=2.0*XM[n+1]-XM[n]; |
||||
XM[n+3]=2.0*XM[n+2]-XM[n+1]; |
||||
XM[2]=2.0*XM[3]-XM[4]; |
||||
XM[1]=2.0*XM[2]-XM[3]; |
||||
for (i=1; i<n+1; i++) { |
||||
a=fabs(XM[i+3]-XM[i+2]); |
||||
b=fabs(XM[i+1]-XM[i]); |
||||
if (a+b==0) Z[i]=(a*XM[i+1]+b*XM[i+2])/(a+b); else Z[i]=(XM[i+2]+XM[i+1])/2.0; |
||||
} |
||||
//Find relevant table interval
|
||||
i=0; |
||||
while (p>x[i]) i++; |
||||
i--; |
||||
|
||||
//Begin interpolation
|
||||
b=x[i+1]-x[i]; |
||||
a=p-x[i]; |
||||
r=y[i]+Z[i]*a+(3.0*XM[i+2]-2.0*Z[i]-Z[i+1])*a*a/b; |
||||
r=r+(Z[i]+Z[i+1]-2.0*XM[i+2])*a*a*a/(b*b); |
||||
return(r); |
||||
} |
||||
@ -0,0 +1,79 @@ |
||||
/*
|
||||
* interpolation.h |
||||
* |
||||
* interpolation - An interpolation library for Arduino. |
||||
* Author: Jose Gama 2015 |
||||
|
||||
* |
||||
* This library is free software; you can redistribute it |
||||
* and/or modify it under the terms of the GNU Lesser |
||||
* General Public License as published by the Free Software |
||||
* Foundation; either version 3 of the License, or (at |
||||
* your option) any later version. |
||||
* |
||||
* This library is distributed in the hope that it will |
||||
* be useful, but WITHOUT ANY WARRANTY; without even the |
||||
* implied warranty of MERCHANTABILITY or FITNESS FOR A |
||||
* PARTICULAR PURPOSE. See the GNU Lesser General Public |
||||
* License for more details. |
||||
* |
||||
* You should have received a copy of the GNU Lesser |
||||
* General Public License along with this library; if not, |
||||
* write to the Free Software Foundation, Inc., |
||||
* 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
||||
* |
||||
*/ |
||||
|
||||
/*
|
||||
* From: https://github.com/tuxcell/interpolationArduino
|
||||
* replaced all doubles by float (wirtz@parasitstudio.de) |
||||
*/ |
||||
|
||||
#ifndef interpolation_h |
||||
#define interpolation_h |
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100 |
||||
#include <Arduino.h> |
||||
#else |
||||
#include <WProgram.h> |
||||
#endif |
||||
|
||||
class interpolation |
||||
{ |
||||
public: |
||||
// constructor
|
||||
interpolation( void ); |
||||
interpolation( float x[], float y[], int lenXY); |
||||
interpolation( float x[], float y[], int lenXY, float valInterp); |
||||
|
||||
void valueI( float valInterp ); |
||||
void valuelenXY( int lenXY ); |
||||
void valueX( float x[]); |
||||
void valueY( float y[]); |
||||
void valueXM( float XM[]); |
||||
void valueZ( float Z[]); |
||||
float LinearInterpolate(); |
||||
float CosineInterpolate(); |
||||
float CubicInterpolate(); |
||||
float LagrangeInterpolate(); |
||||
float QuadraticInterpolate(); |
||||
float AkimaInterpolate(); |
||||
|
||||
private: |
||||
float* _x; |
||||
float* _y; |
||||
float* _XM; |
||||
float* _Z; |
||||
int _lenXY; |
||||
float _valInterp; |
||||
|
||||
float LinearInterp( float x[], float y[], int n, float p ); |
||||
float CosineInterp( float x[], float y[], int n, float p ); |
||||
float CubicInterp( float x[], float y[], int n, float p ); |
||||
float LagrangeInterp( float x[], float y[], int n, float p ); |
||||
float QuadraticInterp( float x[], float y[], int n, float p ); |
||||
float AkimaInterp( float x[], float y[], float XM[], float Z[], int n, float p ); |
||||
|
||||
}; |
||||
|
||||
#endif |
||||
@ -1,124 +0,0 @@ |
||||
/*
|
||||
From: https://raw.githubusercontent.com/kerinin/arduino-splines
|
||||
*/ |
||||
|
||||
#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,45 +0,0 @@ |
||||
/*
|
||||
From: https://github.com/kerinin/arduino-splines
|
||||
|
||||
Library for 1-d splines |
||||
Copyright Ryan Michael |
||||
Licensed under the LGPLv3 |
||||
*/ |
||||
|
||||
#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