曲线平滑-B样条曲线

3D空间曲线三次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;
}