曲线平滑-B样条曲线

struct D_DOT3D //D_DOT3D示例，未完全实现
{
double x,y,z;
}

double GetThreeBSplineValue(double p0, double p1, double p2, double p3, double t)
{
double A0 = (p0 + 4*p1 + p2) / 6;
double A1 = - (p0 - p2)/2;
double A2 = (p0 - 2*p1 + p2) / 2;
double A3 = - (p0 - 3*p1 + 3*p2 - p3)/6;
return A0 + A1 * t + A2 * t*t + A3 * t*t*t;
}

D_DOT3D GetThreeBSplineValue(D_DOT3D p0, D_DOT3D p1, D_DOT3D p2, D_DOT3D p3 , double t)
{
D_DOT3D dot;
dot.x = GetThreeBSplineValue(p0.x, p1.x, p2.x, p3.x, t);
dot.y = GetThreeBSplineValue(p0.y, p1.y, p2.y, p3.y, t);
dot.z = GetThreeBSplineValue(p0.z, p1.z, p2.z, p3.z, t);

return dot;
}

long ThreeBSplineCurve( vector<D_DOT3D> &line )
{
if (line.size()<4)
return 0;

vector<D_DOT3D> vout;
for (int i=0; i<line.size()-3; ++i)
{
D_DOT3D p0(line[i]);
D_DOT3D p1(line[i+1]);
D_DOT3D p2(line[i+2]);
D_DOT3D p3(line[i+3]);
if(0 == i)
{
D_DOT3D dot1 = GetThreeBSplineValue(p0, p1, p2, p3, 0.0);
vout.push_back(dot1);
}

D_DOT3D dot2 = GetThreeBSplineValue(p0, p1, p2, p3, 1/3.0);
D_DOT3D dot3 = GetThreeBSplineValue(p0, p1, p2, p3, 2/3.0);
D_DOT3D dot4 = GetThreeBSplineValue(p0, p1, p2, p3, 1.0);

vout.push_back(dot2);
vout.push_back(dot3);
vout.push_back(dot4);
}

line = vout;

return 1;
}