C++实现matlab中的interp1和interp2插值

头文件

interpfun.h

#ifndef INTERPFUN_H
#define INTERPFUN_H
#include"math.h"
#include<stdio.h>
#include <string.h> 
#ifndef SafeDeleteVec
#define SafeDeleteVec(X) { if((X)) delete (X); (X)=NULL;}
#endif
class Interp
{
public:


char *strMethod1;
char *strMethod2;
char *strMethod3;






public:
Interp();
~Interp();
/*****以下两个函数为插值函数**/
void interp_onePoint(float *x,float *y,int n,float t,float *fval,char *method);//返回单个点的值
void interp_multiPoint(float *x,float *y,int n,float *t,float *fval,int m,char *method);//返回多个点的值,点的个数为m
float interp2_onePoint(float *x,float *y,float *z,int m,int n,float a,float b,char *method);//matlab interp2 实现 2D面数据插值
void interp2_multiPoint(float *x,float *y,float *z,int m,int n,float *a,float *b,int asize,int bsize,float *fval,char *method);//matlab interp2 实现 2D面向量插值


private:
//拉格朗日线性插值
float lgr(float *x,float *y,int n,float t);
//拉格朗日一元三点线性插值
float lg3(float *x,float *y,int n,float t);
//平滑插值 三次多项式插值
void spl(float *x,float *y,int n,int k,float t,float *s);

int findIndex(float *x,int t,int n);

};

实现：

interpfun.cpp

#include"interpfun.h"
Interp::Interp()
{
strMethod1 ="lgr";
strMethod2 = "lg3";
strMethod3 = "spl";
}


Interp::~Interp()
{

}




float Interp::lgr(float *x,float *y,int n,float t)
{
int i,j,k,m;
float z,s;
z= 0.0f;
if(n<1)
return(z);
if(n==1)
{
z=y[0];
return(z);
}


if(n==2)
{
z=(y[0]*(t-x[1]) - y[1]*(t-x[0]))/(x[0]-x[1]);
return(z);


}
if(n>1 && t>x[n-1]){
z= y[n-1];
return(z);
}
if(n>1 && t<x[0]){
z= y[0];
return(z);
}




i =0;
while((x[i] <t) && (i<n)) {
i=i+1;
}


k = i-4;
if(k<0) 
k=0;
m = i+3;
if(m>n-1) 
m = n-1;
for( i = k;i<=m;i++)
{
s=1.0;
for( j =k;j<=m;j++){
if(j!=i) 
s= s*(t-x[j])/(x[i]-x[j]);
}
z = z+s*y[i];
}
return(z);


}


//拉格朗日一元三点插值
float Interp::lg3(float *x,float *y,int n,float t)
{
int i,j,k,m;
float z,s;
z= 0.0;
if(n<1) return (z);
if(n ==1) {z= y[0];return(z);}
if(n==2)
{
z = (y[0]*(t-x[1]) - y[1]*(t - x[0]))/(x[0] - x[1]);
return(z);
}
if(n>0&&t<x[0]){
z = y[0];
return(z);
}
if(n>0 && t>x[n-1]){
z = y[n-1];
return(z);
}




if(t<=x[1]) 
{
k =0;
m=2;
}
else if(t>=x[n-2])
{
k = n-3;
m = n-1;
}
else 
{
k =1;
m =n;
while ((m-k) !=1)
{
i = (k+m)/2;
if(t<x[i-1])
m =i;
else
k =i;
}
k = k-1;
m = m-1;
if(fabs(t-x[k]) <fabs(t- x[m]))
k = k-1;
else
m = m+1;
}
z = 0.0;
for(i =k;i<=m;i++)
{
s= 1.0;
for(j = k;j<=m;j++)
{
if(j!=i)
s = s*(t-x[j])/(x[i] - x[j]);
}
z = z+ s*y[i];




}
return (z);




}


//平滑插值 三次多项式插值
void Interp::spl(float *x,float *y,int n,int k,float t,float *s)
{
int kk, m,lc;


float u[5], p,q;

s[4] = 0.0f;
s[0] = 0.0f;
s[1] = 0.0f;
s[2] = 0.0f;
s[3] = 0.0f;





if(n<1)
return;
if(n == 1)
{
s[0] = y[0];
s[4] = y[0];
return;
}
if(n == 2)
{
s[0] = y[0];
s[1] = (y[1] - y[0])/(x[1] - x[0]);
if(k<0)
s[4] = (y[0]*(t - x[1]) - y[1]*(t - x[0]))/(x[0] - x[1]);
return;
}


if(k<0 && n>0 && t<x[0] )
{


s[4] = y[0];
return;
}


if(k<0 && n>0 && t>x[n-1] )
{


s[4] = y[n-1];
return;
}






if(k<0)
{


if(t<=x[1]) 
kk =0;
else if(t>=x[n-1])
kk = n-2;
else
{


kk=1;
m = n;
while (((kk -m) !=1) && ((kk - m) != -1))
{
lc=(kk +m)/2;
if(t<x[lc-1]) 
m = lc;
else
kk =lc;


}
kk = kk -1;
}
}
else
kk = k;


if( kk>n-1)
kk = n-2;


u[2] = (y[kk +1] - y[kk])/(x[kk +1] - x[kk]);
if (n == 3)
{
if(kk == 0)
{
u[3] = (y[2] - y[1])/(x[2]-x[1]);
u[4] = 2.0*u[3] - u[2];
u[1] = 2.0*u[2] - u[3];
u[0] = 2.0*u[1] - u[2];
}
else
{
u[1] = (y[1] - y[0])/(x[1] - x[0]);
u[0] = 2.0*u[1] - u[2];
u[3] = 2.0*u[2] - u[1];
u[4] = 2.0*u[3] - u[2];
}


}
else
{
if(kk <=1)
{
u[3] = (y[kk+2] - y[kk +1])/(x[kk+2]-x[kk+1]);
if(kk ==1)
{
u[1] = (y[1] -y[0])/(x[1] - x[0]);
u[0] = 2.0*u[1] - u[2];
if(n == 4)
u[4] = 2.0*u[3] - u[2];
else
u[4] = (y[4]-y[3])/(x[4] - x[3]);
}
else
{
u[1] = 2.0*u[2] - u[3];
u[0] = 2.0*u[1] - u[2];
u[4] = (y[3]-y[2])/(x[3] - x[2]);
}


}
else if(kk >=(n-3))
{
u[1] = (y[kk]-y[kk-1])/(x[kk] - x[kk-1]);
if(kk == (n-3))
{
u[3] = (y[n-1] - y[n-2])/(x[n-1] - x[n-2]);
u[4] = 2.0*u[3] - u[2];
if( n== 4)
u[0] = 2.0*u[1] - u[2];
else 
u[0] = (y[kk-1] - y[kk-2])/(x[kk-1] - x[kk-2]);


}
else
{
u[3] = 2.0*u[2] - u[1];
u[4] = 2.0*u[3] - u[2];
u[0] = (y[kk -1] - y[kk-2])/(x[kk-1] - x[kk -2]);
}


}
else
{
u[1] = (y[kk] - y[kk -1])/(x[kk] - x[kk-1]);
u[0] = (y[kk-1] - y[kk -2])/(x[kk-1] - x[kk-2]);
u[3] = (y[kk+2] - y[kk +1])/(x[kk+2] - x[kk +1]);
u[4] = (y[kk+3] - y[kk +2])/(x[kk+3] - x[kk +2]);
}


}


s[0] = fabs(u[3] - u[2]);
s[1] = fabs(u[0] - u[1]);
if( (s[0] + 1.0 == 1.0) && (s[1] + 1.0 == 1.0))
p = (u[1] + u[2])/2.0;
else
p = (s[0]*u[1] + s[1]*u[2])/(s[0] + s[1]);
s[0] = fabs(u[3] - u[4]);
s[1] = fabs(u[2] - u[1]);
if((s[0] + 1.0 == 1.0) && (s[1] + 1.0 == 1.0))
q = (u[2]+u[3])/2.0;
else 
q= (s[0]*u[2] + s[1]*u[3])/(s[0] + s[1]);


s[0] = y[kk];
s[1] = p;
s[3] = x[kk +1] - x[kk];
s[2] = (3.0*u[2] - 2.0*p - q)/s[3];
s[3] = (p + q - 2.0*u[2])/(s[3]*s[3]);
if (k<0)
{
p = t-x[kk];
s[4] = s[0] + s[1]*p + s[2]*p*p + s[3]*p*p*p;

}
return;


}


float Interp::interp2_onePoint(float *x,float *y,float *z,int m,int n,float a,float b,char *method)
{


//find a，b的位置
int tempi,tempj;
float w1,w2,w;
float tempx[2] = {0,0};
float tempz[2] = {0,0};
/*if(a>x[m-1] || b>y[n-1] || a<x[0] || b<y[0])
{
w = 0.0f;


}
else
{*/






tempi = findIndex(x,a,m);
tempj = findIndex(y,b,n);
if(tempi<0)
tempi =0;
if(tempi>m-2)
tempi = m-2;
if(tempj<0)
tempj = 0;
if(tempj>n-2)
tempj = n-2;


tempx[0] = x[tempi];
tempx[1] = x[tempi+1];
tempz[0] = z[tempj*m + tempi];
tempz[1] = z[tempj*m + tempi + 1];


interp_onePoint(tempx,tempz,2,a,&w1,method);
//update tempz
tempz[0] = z[(tempj+1)*m + tempi];
tempz[1] = z[(tempj+1)*m + tempi + 1];
interp_onePoint(tempx,tempz,2,a,&w2,method);


//update tempx and tempz
tempx[0] = y[tempj];
tempx[1] = y[tempj+1];


tempz[0] = w1;
tempz[1] = w2;


interp_onePoint(tempx,tempz,2,b,&w,method);
//}
return(w);
}




void Interp::interp2_multiPoint(float *x,float *y,float *z,int m,int n,float *a,float *b,int asize,int bsize,float *fval,char *method)
{
//grid nets
for(int i=0;i<asize;i++)
{
for(int j = 0;j<bsize;j++)
*(fval+j*asize+i) = interp2_onePoint(x,y,z,m,n,a[i],b[j],method);
}

}


/*
void Interp::interp2_multiPoint(float *x,float *y,float *z,int m,int n,float *gridx,float *gridy,int asize,int bsize,float *fval,char *method)
{
//grid nets
for(int i=0;i<asize;i++)
{
for(int j = 0;j<bsize;j++)
*(fval+j*asize+i) = interp2_onePoint(x,y,z,m,n,gridx[j*asize + i],gridx[j*asize + i],method);
}

}*/


int Interp::findIndex(float *x,int t,int n)
{
int kk,m,lc;
/*if(t<x[0])
return( 0);
if(t>=x[n-1])
return(n-1);*/


if(t<=x[1]) 
kk =0;
else if(t>=x[n-1])
kk = n-2;
else
{


kk=1;
m = n;
while (((kk -m) !=1) && ((kk - m) != -1))
{
lc=(kk +m)/2;
if(t<x[lc-1]) 
m = lc;
else
kk =lc;


}
kk = kk -1;
}


return(kk);


}


void Interp::interp_onePoint(float *x,float *y,int n,float t,float *fval,char *method)
{

if(nullptr == method)
*fval = lgr(x,y,n,t);
else if(_strcmpi(method,strMethod1) == 0)
*fval = lgr(x,y,n,t);
else if(_strcmpi(method,strMethod2) == 0)
*fval = lg3(x,y,n,t);
else if(_strcmpi(method,strMethod3) == 0)
{
static float s[5];
spl(x,y,n,-1,t,s);
*fval = s[4];
}
else
*fval = lgr(x,y,n,t);
}


void Interp::interp_multiPoint(float *x,float *y,int n,float *t,float *fval,int m,char *method)
{
if(t == NULL)
return;
float tempVal = 0.0;;
for(int k = 0;k<m;k++){

interp_onePoint(x,y,n,t[k],&tempVal,method);
fval[k] = tempVal;
}

}

测试：

#include""

int main()

{

 float t ,z;
static float x1[10] = {0.10,0.15,0.25,0.4,0.5,0.57,0.7,0.85,0.93,1.00};
static float y1[10] = {0.904837,0.860708,0.778801,0.670320,0.606531,
0.565525,0.496585,0.427415,0.394554,0.367879};


char *method = "spl";
float t[3] = {0.63,0.71,0.84};
float multval[3] = {0};
float val;
interp.interp_onePoint(x1,y1,10,1.5,&val,method);
cout<<"t=10,"<<"z = "<<val<<endl;

getchar();

interp.interp_multiPoint(x1,y1,10,t,multval,3,method);


cout<<"t[0] = "<<t[0]<<","<<"val[0]="<<multval[0]<<endl;
cout<<"t[1] = "<<t[1]<<","<<"val[1]="<<multval[1]<<endl;
cout<<"t[2] = "<<t[2]<<","<<"val[2]="<<multval[2]<<endl;
getchar();

 return 0;

}