三次样条插值原理推导与C语言实现（3）

公众号的文章没法同步到CSDN，就先截图搬运过来了，欢迎关注嵌入式与机电联合公众号，一起学习，加油加油！

三次样条插值原理推导与C语言实现（1）

三次样条插值原理推导与C语言实现（2）

三次样条插值原理推导与C语言实现（2）

​

#include "mex.h" 
#include <stdio.h>
#include <math.h>
/*
time：原始横坐标数据
pos:原始纵坐标数据
N：原始数据长度
InterData:插值数据的横坐标
InterNum:插值数据的个数
out：插值数据的纵坐标（输出）
InitVel：第一边界条件，初始端点的速度
EndVel：第一边界条件，最后一个端点的速度
*/

void MySpline(double time[],double pos[],int N,double InterData[],int InterNum,double out[],double InitVel,double EndVel)
{
        /*原始数据t,data*/
    double *t = time;
    double *data = pos;
    int n = N;
    int dim = n+1;//数组下标从1到n不要0
    double bl = InitVel;//初始点速度
    double bn = EndVel;//终止点速度
    //构造三对角矩阵方程lambda,mu,D,这三个矩阵需要hi=x1+1-x1 
    double h[dim],lambda[dim],mu[dim],D[dim];
    //构造追赶法A=LU，L U所需参数
    double l[dim],m[dim],u[dim];
    // 构造正解K矩阵，逆解M矩阵
    double K[dim],M[dim];
    //构造三次方程4个系数矩阵
    double a[dim],b[dim],c[dim],d[dim];
    
    //1计算hi=xi+1-x1参数
    for(int i=1;i<n;i++)//h[1]=t[1]- t[0]  h[5]=t[5]-t[4]
    {
        h[i]=t[i]- t[i-1];
    }
   
    /*2构造n-2个数组系数, 填充n-2个方程组系数*/
    for(int i=2;i<n;i++)//2.3.4.5....n-1
    {
      lambda[i]=h[i-1]/(h[i-1]+h[i]);
      mu[i]= h[i]/(h[i]+h[i-1]);
      D[i]=6/(h[i]+h[i-1])*( (data[i]-data[i-1])/h[i]-(data[i-1]-data[i-2])/h[i-1] );
    }

    /*3根据边界条件确定另外2个方程,以此确定三对角矩阵A*/
    D[1]=6/h[1]*((data[2-1]-data[1-1])/h[1]-bl );
    D[n]=6/h[n-1]*(bn-(data[n-1]-data[n-2])/h[n-1]);
    mu[1]=1;
    lambda[n]=1;

    /*4利用追赶法求解矩阵A*/
    /*1)将三对角矩阵A分解成L,U矩阵 l u m*/
    l[1]=2;
    u[1]=mu[1]/l[1];
    for(int i=2;i<=n;i++)
    {
        m[i]= lambda[i];
        l[i]= 2-m[i]*u[i-1];
        u[i]=mu[i]/l[i];
    }

    /*2)AM=D LUM=D LK=D(正解K) UM=K(逆解M)*/
    //正解K
    K[1] = D[1]/l[1];//LK=D
    for(int i=2;i<=n;i++)
    {
        K[i]=(D[i]-m[i]*K[i-1])/l[i];
    }

     //逆解M
    M[n]=K[n];
    for(int i=n-1;i>=1;i--)
    {
        M[i]= K[i]- u[i]*M[i+1];
    }

    /*5求解出M后，根据M计算三次方程的系数abcd*/
    // n个点一共n-1个方程
    for(int i=1;i<n;i++)
    {
         a[i]= data[i-1];
         b[i]=(data[i+1-1]-data[i-1])/h[i]-h[i]*(M[i]/3 + M[i+1]/6);
         c[i]= M[i]/2;
         d[i]= (M[i+1]-M[i])/(6*h[i]);
    }

    /*6利用解出的三次方程数abcd,解出各段三次函数的表达式*/
    //红意点插值
    //判断插入的点属于哪个三次方程中，即确定i
    int j =0;//用于输出数组的下标
    int inNUm =InterNum;
    double *xin=InterData;
    for(int i=0;i<inNUm;i++)//每一个点都进行判断并且计算
    {
        int index=0;//用于记录分段三次方程的下标
        //循环可断插入的值在哪一个区间
        for(int kk=1;kk<n; kk++)/*n个点一共n-1个方程*/
        {
            if((xin[i]>=t[kk-1]) && (xin[i]<=t[kk])) 
            {
                index = kk;
                break;//如果找到了插入值在哪一个区问，则记录kk，跳出循环
            }
        }
        if(index!=0)//说明已经找到插值点所属于的三次方程
        {
            out[j] = a[index]+b[index]*(xin[i]-t[index-1])+c[index]*(xin[i]-t[index-1])*(xin[i]-t[index-1])+d[index]*(xin[i]-t[index-1])*(xin[i]-t[index-1])*(xin[i]-t[index-1]);
        //    printf("y = %lf\r\n",y);
            j++;
        }
        else
        {
            printf("None!!!!\r\n");
        }

    
    }
}


void mexFunction(int nlhs,mxArray *plhs[], int nrhs,const mxArray *prhs[])  
{  
    /*1）定义输入变量用于接收matlab传入的值*/
    double *pInputData1;
    double *pInputData2;
    int N;
    double *InterData;
    int InNum;

    /*定义输出变量用于输出C模块计算的结果*/
    double *pOutputData;
    double InitVel;
    double EndVel;

    /*2）通过prhs接收matlab传入的值，通过plhs传递结果给matlab*/
    pInputData1 = mxGetPr(prhs[0]); //传入的是数组就用mxGetPr
    pInputData2 = mxGetPr(prhs[1]); 
    N = mxGetScalar(prhs[2]);//传入的是值就用mxGetScalar
    InterData = mxGetPr(prhs[3]);
    InNum = mxGetScalar(prhs[4]);
    InitVel = mxGetScalar(prhs[5]);
    EndVel = mxGetScalar(prhs[6]);

    /*3）创建输出指针*/
    plhs[0] = mxCreateDoubleMatrix(1,InNum,mxREAL);
    pOutputData = mxGetPr(plhs[0]);

    /*4）调用C函数*/
    MySpline(pInputData1,pInputData2,N,InterData,InNum,pOutputData,InitVel,EndVel);
}

%% 已知数据点
t = 0:pi/14:2*pi;
data = sin(t);
n = length(data);

%% 等间隔插值
xStep = pi/30;
tt = 0:xStep:2*pi;
Initvel = 0;%第一边界条件 初始速度设置为0
EndVel = 0;%第一边界条件，最后一点速度设置为0
Innum = length(tt);
out = MySpline(t,data,n,tt,Innum,Initvel,EndVel);%推导的三次样条插值
vql = interp1(t,data,tt,'cubic');%matlab自带的样条插值

%% 对比推导的插值与matlab自带的插值效果
for i=1:length(tt)
    hold on;
    plot(tt(i),out(i),'-ro');
    plot(tt(i),vql(i),':k+');
end

legend('自己推导的三次样条插值','matlab自带的三次样条插值');