三次样条插值特点与实现 (引用了一点别人代码，但做了改动！)

用了点别人代码，但做了一些改动，因为看不懂那人的代码：

引用部分原文: http://haoxiang.org/2011/06/cubic-spline-interpolation-curve-fitting/

一. 主要特点

　　1.)  一阶导数是二次曲线（光滑的..） 

     2.) 二阶导数是线性的

     3.) 也就是说，三次样条插值，是把N个离散数据，变成N-1个段，每一段的一次，二次都是连续的！

     4.) 计算量大点...

二. 实现

#define MAX_LEN_3 (100)

/
void CCubicSpline::derivative(const double *pXi,const double *pYi, int len, double *pCoefs, double Ml, double Mr)
{

double d[MAX_COUNT] = {0.0}; //右值

double h[MAX_COUNT] = {0.0};
double u[MAX_COUNT] = {0.0};
double lam[MAX_COUNT] = {0.0};

//一阶导数
for (int i = 0; i<len-1; i++)
{
h[i] = pXi[i+1] - pXi[i];
}

//计算矩阵值
for (int i = 1; i<len-1; i++)
{
u[i] = h[i-1]/(h[i] + h[i - 1]);
lam[i] = h[i] /(h[i] + h[i - 1]);
}

double m[MAX_COUNT*MAX_COUNT] = {0.0};

double a[MAX_COUNT] = {0.0};
double b[MAX_COUNT] = {0.0};
double c[MAX_COUNT] = {0.0};

//填冲..
for (int i = 0; i<len; i++)
{
for (int j = 0; j<len; j++)
{
m[i*len + j] = 0.0;
}

if (i == 0)
{
m[i*len + 0] = 2;
m[i*len + 1] = 1;

b[0] = 2;
c[0] = 1;
}
else if (i == (len - 1))
{
m[i*len + len - 2] = 1;
m[i*len + len - 1] = 2;

a[len -1] = 1;
b[len -1] = 2;
}
else
{
m[i*len + i-1] = u[i];
m[i*len + i ] = 2;
m[i*len + i+1] = lam[i];

a[i] = u[i];
b[i] = 2;
c[i] = lam[i];
}
}

// 计算方程右面
double g[MAX_COUNT] = {0.0};

// double Ml = 1.0; //左 指定的系数
// double Mr = 0.6868; //右 指定的系数
g[0] =( 6 * ( (pYi[1] - pYi[0])/ h[0] - Ml) )/h[0];
g[len - 1] = 6 * ( Mr - (pYi[len - 1] - pYi[len - 2])/h[len - 2] )/h[len - 2];

// g[0] =0.0;
// g[len - 1] = 0.0;

for (int i = 1; i < len - 1; i++)
{
double tmp1 = ((pYi[i+1] - pYi[i])/h[i]);
double tmp2 = ((pYi[i] - pYi[i-1])/ h[i-1]);
g[i] =( tmp1 - tmp2 ) / (h[i-1] + h[i]) * 6;
}

printf("\n====================\n");
for (int i=0; i<len; i++)
{
for (int j=0; j<len; j++)
{
printf(" %7.3f ", m[i*len + j]);
}
printf(" * M%d : %7.3f \n", i, g[i]);
}

int count = len - 2;

printf("\n=========\n");
for (int j=0; j<count; j++)
{
printf(" %7.3f %7.3f %7.3f * M%d : %7.3f \n", a[j], b[j], c[j], j, g[j]);
}

//b[1] = a[1]; c[1]
//
//
// 解方程 --> 追赶法
double p[MAX_COUNT] = {0.0};
p[0] = c[0]/b[0];
for (int i = 1; i<len - 1;i++)
{
p[i] = c[i]/(b[i] - a[i]*p[i-1]);
}

double y[MAX_COUNT] = {0.0};
y[0] = g[0]/b[0];
for (int i = 1; i<len;i++)
{
y[i] = (g[i] - a[i]*y[i-1])/(b[i] - a[i]*p[i-1]);
}

double M[MAX_COUNT] = {0.0};

M[len -1] = y[len -1];
for (int i=len-2; i>=0; i--)
{
M[i] = y[i] - p[i]*M[i+1];
}

printf("\n P:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", p[i]);
}

printf("\n Y:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", y[i]);
}

printf("\n M:");
for (int i=0; i<len; i++)
{
printf(" %9.5f ", M[i]);
}
printf("\n");

//代入三次样表的表达式

for (int i=0; i<len-1; i++)
{
pCoefs[i*4 + 0] = M[i+0] / (6 * h[i]);
pCoefs[i*4 + 1] = (pYi[i+0] - M[i+0] * h[i] * h[i] / 6) / h[i];
pCoefs[i*4 + 2] = M[i+1] / (6 * h[i]);
pCoefs[i*4 + 3] = (pYi[i+1] - M[i+1] * h[i] * h[i] / 6) / h[i];
}

}

//////////////////////////////

void CCubicSpline::derivative(const double *pXi,const double *pYi, int len, double *pCoefs)
{

int count = len - 2;

double d[MAX_COUNT] = {0.0}; //右值

double h[MAX_COUNT] = {0.0};
double u[MAX_COUNT] = {0.0};
double lam[MAX_COUNT] = {0.0};

//一阶导数
for (int i = 0; i<len-1; i++)
{
h[i] = pXi[i+1] - pXi[i];
}

//计算矩阵值
for (int i = 1; i<len-1; i++)
{
u[i] = h[i-1]/(h[i] + h[i - 1]);
lam[i] = h[i] /(h[i] + h[i - 1]);
}

double m[MAX_COUNT*MAX_COUNT] = {0.0};

double a[MAX_COUNT] = {0.0};
double b[MAX_COUNT] = {0.0};
double c[MAX_COUNT] = {0.0};

//填冲..
for (int i = 0; i<len; i++)
{
for (int j = 0; j<len; j++)
{
m[i*len + j] = 0.0;
}

if (i == 0)
{
m[i*len + 0] = 2;
m[i*len + 1] = 1;

}
else if (i == (len - 1))
{
m[i*len + len - 2] = 1;
m[i*len + len - 1] = 2;

}
else
{
m[i*len + i-1] = u[i];
m[i*len + i ] = 2;
m[i*len + i+1] = lam[i];

a[i-1] = u[i];
b[i-1] = 2;
c[i-1] = lam[i];
}
}

a[0] = 0.0;
c[count - 1] = 0.0;

// 计算方程右面
double g[MAX_COUNT] = {0.0};

for (int i = 1; i < len - 1; i++)
{
double tmp1 = ((pYi[i+1] - pYi[i])/h[i]);
double tmp2 = ((pYi[i] - pYi[i-1])/ h[i-1]);
g[i-1] =( tmp1 - tmp2 ) / (h[i-1] + h[i]) * 6;
}

printf("\n====================\n");
for (int i=0; i<len; i++)
{
for (int j=0; j<len; j++)
{
printf(" %7.3f ", m[i*len + j]);
}
printf(" * M%d : %7.3f \n", i, g[i]);
}

printf("\n=========\n");
for (int j=0; j<count; j++)
{
printf(" %7.3f %7.3f %7.3f * M%d : %7.3f \n", a[j], b[j], c[j], j, g[j]);
}

//b[1] = a[1]; c[1]
//
//
// 解方程 --> 追赶法
double p[MAX_COUNT] = {0.0};
p[0] = c[0]/b[0];
for (int i = 1; i<count - 1;i++)
{
p[i] = c[i]/(b[i] - a[i]*p[i-1]);
}

double y[MAX_COUNT] = {0.0};
y[0] = g[0]/b[0];
for (int i = 1; i<count;i++)
{
y[i] = (g[i] - a[i]*y[i-1])/(b[i] - a[i]*p[i-1]);
}

double M[MAX_COUNT] = {0.0};

M[count -1] = y[count -1];
for (int i=count-1; i>=0; i--)
{
M[i] = y[i] - p[i]*M[i+1];
}

for (int i=count - 1; i>= 0; i--)
{
M[i+1] = M[i];
}
M[0] = 0.0;

printf("\n P:");
for (int i=0; i<count; i++)
{
printf(" %9.5f ", p[i]);
}

printf("\n Y:");
for (int i=0; i<count; i++)
{
printf(" %9.5f ", y[i]);
}

printf("\n M:");
for (int i=0; i<len-1; i++)
{
printf(" %9.5f ", M[i]);
}
printf("\n");

//代入三次样条的表达式

// for(i = 0;i <= pLine->point_num - 2;i++)
// {
// pLine->a3[i] = M[i] / (6 * H[i]);
// pLine->a1[i] = (pLine->y[i] - M[i] * H[i] * H[i] / 6) / H[i];
// pLine->b3[i] = M[i+1] / (6 * H[i]);
// pLine->b1[i] = (pLine->y[i+1] - M[i+1] * H[i] * H[i] / 6) /H[i];
// }

for (int i=0; i<len-1; i++)
{
pCoefs[i*4 + 0] = M[i+0] / (6 * h[i]);
pCoefs[i*4 + 1] = (pYi[i+0] - M[i+0] * h[i] * h[i] / 6) / h[i];
pCoefs[i*4 + 2] = M[i+1] / (6 * h[i]);
pCoefs[i*4 + 3] = (pYi[i+1] - M[i+1] * h[i] * h[i] / 6) / h[i];
}

}

double CCubicSpline::Spline3(double startX, double endX, int index, double curX)
{
double Coefs0 = m_pCoefs[index * 4 + 0];
double Coefs1 = m_pCoefs[index * 4 + 1];
double Coefs2 = m_pCoefs[index * 4 + 2];
double Coefs3 = m_pCoefs[index * 4 + 3];

double outY0 = Coefs0 * (endX - curX) * (endX - curX) * (endX - curX);
double outY1 = Coefs1 * (endX - curX);
double outY2 = Coefs2 * (curX - startX) * (curX - startX) * (curX - startX);
double outY3 = Coefs3 * (curX - startX);

return (outY0 + outY1 + outY2 + outY3);
}

void CCubicSpline::PushPoint(CPoint point)
{
vPoint.push_back(point);

// 计算系数

double xi[MAX_COUNT] = { 0.0};
double yi[MAX_COUNT] = { 0.0};
int count = vPoint.size();

for (int i=0; i<count; i++)
{
CPoint point = vPoint[i];
xi[i] = point.x;
yi[i] = point.y;
}


if (count < 4)
{
return;
}
derivative(xi,yi, count, m_pCoefs);

}

if (vPoint.size() > 3)
{
int startX = vPoint[0].x;
int EndX = 0;
pDC->MoveTo(vPoint[0].x,vPoint[0].y);
for (int i=1; i<vPoint.size(); i++)
{
EndX = vPoint[i].x;

for (int curX=startX; curX<EndX; )
{
double curY = this->Spline3(startX, EndX, i -1 , curX);
//pDC->Ellipse(curX-1,curY-1,curX+1,curY+1);

pDC->LineTo(curX, curY);

if (startX < EndX) curX++;
if (startX > EndX) curX--;

}
startX = EndX;

}

}

for (int i=0; i<vPoint.size(); i++)
{
CPoint point = vPoint[i];

pDC->Ellipse(point.x-3,point.y-3,point.x+3,point.y+3);
}

// 差不多全在这儿了！  运行还不错...