BP三阶拟合

之前整理BP三样条拟合空间曲线
若要过首位点，则添加两个点Po=2P1-P2,Pn+1=2Pn-Pn-1
代码如下：

void BPthreeorder(CPosition *&pt, int &Num, int *InsertNum)
{
	if (pt == NULL || InsertNum == NULL) return;

	int InsertNumSum = 0;                               //  计算需要插入的点总数
	for (int i = 0; i<Num - 1; i++)  InsertNumSum += InsertNum[i];

	Points3D *temp = new Points3D[Num + 2];
	for (int i = 0; i<Num; i++)
		temp[i + 1] = pt[i];

	temp[0].x = 2 * temp[1].x - temp[2].x;                  //  将折线延长线上两点加入作为首点和尾点
	temp[0].y = 2 * temp[1].y - temp[2].y;
	temp[0].z = 2 * temp[1].z - temp[2].z;

	temp[Num + 1].x = 2 * temp[Num].x - temp[Num - 1].x;
	temp[Num + 1].y = 2 * temp[Num].y - temp[Num - 1].y;
	temp[Num + 1].z = 2 * temp[Num].z - temp[Num - 1].z;
	Points3D NodePt1, NodePt2, NodePt3, NodePt4;
	double t;

	delete[]pt;                                      //  点数由原来的Num个增加到Num+InsertNumSum个，删除旧的存储空间，开辟新的存储空间

	pt = new Points3D[Num + InsertNumSum];
	int totalnum = 0;
	for (int i = 0; i<Num - 1; i++)                          //  每条线段均匀插入点
	{
		NodePt1 = temp[i]; NodePt2 = temp[i + 1]; NodePt3 = temp[i + 2]; NodePt4 = temp[i + 3];
		double dt = 1.0 / (InsertNum[i] + 1);

		for (int j = 0; j<InsertNum[i] + 1; j++)
		{
			t = dt*j;
			pt[totalnum].x = F03(t)*NodePt1.x + F13(t)*NodePt2.x + F23(t)*NodePt3.x + F33(t)*NodePt4.x;
			pt[totalnum].y = F03(t)*NodePt1.y + F13(t)*NodePt2.y + F23(t)*NodePt3.y + F33(t)*NodePt4.y;
			pt[totalnum].z = F03(t)*NodePt1.z + F13(t)*NodePt2.z + F23(t)*NodePt3.z + F33(t)*NodePt4.z;
			totalnum++;
		}

		if (i == Num - 2){              //  最后一个尾点
			t = 1;
			pt[totalnum].x = F03(t)*NodePt1.x + F13(t)*NodePt2.x + F23(t)*NodePt3.x + F33(t)*NodePt4.x;
			pt[totalnum].y = F03(t)*NodePt1.y + F13(t)*NodePt2.y + F23(t)*NodePt3.y + F33(t)*NodePt4.y;
			pt[totalnum].z = F03(t)*NodePt1.z + F13(t)*NodePt2.z + F23(t)*NodePt3.z + F33(t)*NodePt4.z;
			totalnum++;
		}
	}
	delete[]temp;
	Num = Num + InsertNumSum;
}

定义结构体如下

```cpp
struct Points3D
{
	double  x;
	double  y;
	double  z;
	Points3D(double _x, double _y, double _z) { x = _x; y = _y; z = _z; }
	Points3D() {};
	bool operator==(const Points3D & pt) { return (x == pt.x && y == pt.y && z == pt.z); }
};