3次样条曲线差值函数c++实现

/*
	mousePoint		为输入的拟合点
	linePoint		为计算后拟合曲线上的点，只要以这些点来画线就可以绘制出曲线
	controlPoint	为计算后得到的理论控制点数组
*/

void B3Line(vector<Point>& mousePoint, vector<Point>& linePoint, vector<Point>& controlPoint)
{

	const int nPointCount = 5;				//	在每一段曲线上要拟合出的点数
	const int nMaxmousePtCount = 300;			//	能够处理的鼠标点数为这个变理值减1
	const int nMaxSiShu = nMaxmousePtCount + 5;

	double d[nMaxSiShu] = { 0 };
	double sishu[nMaxSiShu][nMaxSiShu] = { 0 };	//	用来保存方和程系数和对应行的右值

	double x[nMaxSiShu] = { 0 };			//	用来保存鼠标点的x坐标
	double y[nMaxSiShu] = { 0 };			//	用来保存鼠标点的y坐标

	double q0x = 0.0, q0y = 0.0, qnx = 0.0, qny = 0.0;


	int nCtrlPtCount = mousePoint.size();	    //	实际控制点数
	if (nCtrlPtCount == 0)
	{
		return;
	}

	nCtrlPtCount++;

	q0x = mousePoint[0].x;
	q0y = mousePoint[0].y;
	qnx = mousePoint.back().x;
	qny = mousePoint.back().y;

	int nPt = nCtrlPtCount + 2;					//	理论控制点数

	if (nCtrlPtCount > nMaxmousePtCount) return;
	if (nCtrlPtCount < 3) return;				//	只在已有两个鼠标点的情况下才进行计算



	Point pt = mousePoint[0];
	x[0] = pt.x;
	y[0] = pt.y;
	for (int k = 1; k < nCtrlPtCount; k++)
	{
		Point pt = mousePoint[k - 1];
		x[k] = pt.x;
		y[k] = pt.y;

	}


	//	初始化矩阵
	sishu[0][0] = -0.5; sishu[0][1] = 0.0;		sishu[0][2] = 3.0 / 2;
	sishu[1][0] = 0.0;	sishu[1][1] = 4.0 / 6;	sishu[1][2] = 4.0 / 6;

	for (int i = 2; i <= nPt - 1 - 2; i++)
	{

		sishu[i][i - 1] = 1.0 / 6;
		sishu[i][i] = 4.0 / 6;
		sishu[i][i + 1] = 1.0 / 6;
	}

	sishu[nPt - 2][nPt - 2 - 1] = 2.0 / 9;
	sishu[nPt - 2][nPt - 2] = 4.0 / 6;
	sishu[nPt - 2][nPt - 2 + 1] = 0;
	sishu[nPt - 1][nPt - 2 - 1] = -0.5;
	sishu[nPt - 1][nPt - 2] = 0.0;
	sishu[nPt - 1][nPt - 2 + 1] = 3.0 / 2;

	sishu[0][nPt] = q0x;					sishu[0][nPt + 1] = q0y;
	sishu[1][nPt] = x[0] + 1.0 / 3 * q0x;	sishu[1][nPt + 1] = y[0] + 1.0 / 3 * q0y;

	for (int i = 2; i <= nPt - 1 - 2; i++)
	{
		sishu[i][nPt] = x[i - 1];
		sishu[i][nPt + 1] = y[i - 1];
	}


	sishu[nPt - 2][nPt] = x[nPt - 3] - 1.0 / 9 * qnx;  sishu[nPt - 2][nPt + 1] = y[nPt - 3] - 1.0 / 9 * qny;
	sishu[nPt - 1][nPt] = qnx;						  sishu[nPt - 1][nPt + 1] = qny;
	//	完成矩阵的初始化

	//	进行方程组求解
	for (int i = 0; i < nPt; i++)
	{
		//	找出列主元
		int k = i;
		double max = fabs(sishu[i][k]);
		for (int j = i; j < nPt; j++)
		{
			if (max < fabs(sishu[j][i]))
			{
				k = j;
			}
		}

		if (k != i)
		{
			//	进行含主元行的交换, 第k行和第i行交换
			for (int n = 0; n <= nPt + 1; n++)
			{
				double t = sishu[i][n];
				sishu[i][n] = sishu[k][n];
				sishu[k][n] = t;
			}

		}

		//	将主元化为1

		double temp = sishu[i][i];

		for (k = i; k <= nPt + 1; k++)
		{
			sishu[i][k] = sishu[i][k] / temp;
		}

		for (int nn = 0; nn < nPt; nn++)
		{
			if (nn != i)
			{
				double tt = -1.0 * sishu[nn][i];
				for (int mm = 0; mm <= nPt + 1; mm++)
				{
					sishu[nn][mm] += sishu[i][mm] * tt;
				}
			}
		}

	}
	//	完成方程组的求解

	//	取出解得的值, 理论控制点
	controlPoint.clear();
	for (int i = 0; i < nPt; i++)
	{
		x[i] = sishu[i][nPt];
		y[i] = sishu[i][nPt + 1];
		Point pt;
		pt.x = x[i];
		pt.y = y[i];
		controlPoint.push_back(pt);
	}


	//	跟据计算出来的理论控制点，进行三次B样条计算
	linePoint.clear();
	for (int i = 2; i <= nPt - 3; i++)
	{
		for (int j = 0; j <= nPointCount; j++)
		{
			double t = 1.0 * j / nPointCount;
			double t2 = t * t;
			double t3 = t * t * t;

			double xPt = 1.0 / 6 * (1 - 3 * t + 3 * t2 - t3) * x[i - 1] + 1.0 / 6 * (4 - 6 * t2 + 3 * t3) * x[i] + 1.0 / 6 * (1 + 3 * t + 3 * t2 - 3 * t3) * x[i + 1] + 1.0 / 6 * t3 * x[i + 2];
			double yPt = 1.0 / 6 * (1 - 3 * t + 3 * t2 - t3) * y[i - 1] + 1.0 / 6 * (4 - 6 * t2 + 3 * t3) * y[i] + 1.0 / 6 * (1 + 3 * t + 3 * t2 - 3 * t3) * y[i + 1] + 1.0 / 6 * t3 * y[i + 2];

			Point pt;
			pt.x = xPt;
			pt.y = yPt;
			linePoint.push_back(pt);
		}
	}
}