Cubic插值代码

一维Cubic插值代码

#include <chrono>
#include <iostream>
#include <cstring>
#include <math.h>
double cubicCalc(double xs[4], double ys[4], double xInput)
{
    // 旧方法
    // return p[1] + 0.5 * x*(p[2] - p[0] + x * (2.0*p[0] - 5.0*p[1] + 4.0*p[2] - p[3] + x * (3.0*(p[1] - p[2]) + p[3] - p[0])));

    // 初始化
    double* H,*A,*B,*C,*D,*Z,*F,*X,*Y;
    double dbOutY;
    X = new double[4];   
    Y = new double[4];   

    A = new double[4];   
    B = new double[4];   
    C = new double[4];   
    D = new double[4];   
    H = new double[4];   
    memcpy(X, xs, 4*sizeof(double));
    memcpy(Y, ys, 4*sizeof(double));

    int M = 4;
    int N = 3;
    int i,P,L;   
    // 生成Spline
    for (i=1;i<=N;i++)   
    {   
        H[i-1]=X[i]-X[i-1];   
    }   

    L=N-1;   
    for(i=1;i<=L;i++)   
    {   
        A[i]=H[i-1]/(H[i-1]+H[i]);   
        B[i]=3*((1-A[i])*(Y[i]-Y[i-1])/H[i-1]+A[i]*(Y[i+1]-Y[i])/H[i]);   
    }   
    A[0]=1;   
    A[N]=0;   
    B[0]=3*(Y[1]-Y[0])/H[0];   
    B[N]=3*(Y[N]-Y[N-1])/H[N-1];   

    for(i=0;i<=N;i++)   
    {   
        D[i]=2;   
    }   

    for(i=0;i<=N;i++)   
    {   
        C[i]=1-A[i];   
    }   

    P=N;   
    for(i=1;i<=P;i++)   
    {   

        if (  fabs(D[i]) <= 0.000001 )                                  
        {   
            return false;   
            //    MessageBox(0,"�޽�","��ʾ,MB_OK);   
            //break;   
        }   
        A[i-1]=A[i-1]/D[i-1];   
        B[i-1]=B[i-1]/D[i-1];   
        D[i]=A[i-1]*(-C[i])+D[i];   
        B[i]=-C[i]*B[i-1]+B[i];   
    }   
    B[P]=B[P]/D[P];   
    for(i=1;i<=P;i++)   
    {   
        B[P-i]=B[P-i]-A[P-i]*B[P-i+1];   
    }   
    // 生成Spline结束
    // 获得Y坐标
    double E,E1,K,K1,H1;   
    int j ;    
    if(xInput<X[0])   
    {   
        j = 0;   

    }   
    else if (xInput > X[N])   
    {   
        j = N-1;   
    }   
    else  
    {   
        for (j=1;j<=N;j++)   
        {   
            if(xInput<=X[j])   
            {   
                j=j-1;   

                break;   
            }   
        }   

    }   

    //////////////////////////////////////////////////////////////////////////   
    E=X[j+1]-xInput;   
    E1=E*E;   
    K=xInput-X[j];   
    K1=K*K;   
    H1=H[j]*H[j];   

    dbOutY=(3*E1-2*E1*E/H[j])*Y[j]+(3*K1-2*K1*K/H[j])*Y[j+1];   
    dbOutY=dbOutY+(H[j]*E1-E1*E)*B[j]-(H[j]*K1-K1*K)*B[j+1];   
    dbOutY=dbOutY/H1;   
    return dbOutY;
}

二维插值

double bicubicInterpolate(double* xs, double* ys, double p[4][4], double x, double y)
{
    double arr[4];
    arr[0] = cubicCalc(xs, p[0], y);
    arr[1] = cubicCalc(xs, p[1], y);
    arr[2] = cubicCalc(xs, p[2], y);
    arr[3] = cubicCalc(xs, p[3], y);
    return cubicCalc(ys, arr, x);
}

• 上述代码中的xs是描述二维矩阵中取样点的X坐标的集合（从小到大）
• ys是上述矩阵中取样点的y坐标的集合
• 二位插值的思路实际上就是先在一维上完成插值然后利用一维插值的结果向量再进行正交方向上的一维插值