过控制点的三次NURBS曲线及Eigen库实现

原理详见《曲线曲面的NURBS 造型技术与数控仿真》第三章部分。这里主要用Eigen库将其实现。

主要函数如下：

1、delta函数

double d(const MatrixXd u, const int i, const int k) {
	return u(0, i + k) - u(0, i);
}

2、计算M矩阵函数

MatrixXd calCuM(const MatrixXd u, const int i) {
	Matrix<double, 4, 4> m;
	m.setZero();
	m(0, 0) = pow(d(u, i + 3, 1), 2) / (d(u, i + 2, 2) * d(u, i + 1, 3));
	m(0, 2) = pow(d(u, i + 2, 1), 2) / (d(u, i + 2, 2) * d(u, i + 2, 3));
	m(1, 2) = 3 * d(u, i + 3, 1) * d(u, i + 2, 1) / (d(u, i + 2, 2) * d(u, i + 2, 3));
	m(2, 2) = 3 * pow(d(u, i + 3, 1), 2) / (d(u, i + 2, 2) * d(u, i + 2, 3));
	m(3, 3) = pow(d(u, i + 3, 1), 2) / (d(u, i + 3, 2) * d(u, i + 3, 3));
	m(0, 1) = 1 - m(0, 0) - m(0, 2);
	m(1, 0) = -3.0 * m(0, 0);
	m(1, 1) = 3.0 * m(0, 0) - m(1, 2);
	m(2, 0) = 3.0 * m(0, 0);
	m(2, 1) = -3.0 * m(0, 0) - m(2, 2);
	m(3, 0) = -m(0, 0);
	m(3, 2) = -(m(2, 2) / 3.0 + m(3, 3) + pow(d(u, i + 3, 1), 2) / (d(u, i + 3, 2) * d(u, i + 2, 3)));
	m(3, 1) = m(0, 0) - m(3, 2) - m(3, 3);
	return m;
}

3、计算NURBS样条曲线函数（未考虑权重，可自行添加）

bool calcuNURBS(const MatrixXd &mat, MatrixXd &vs, MatrixXd &ve, const int N, MatrixXd &outMat) {
	MatrixXd U, temp, tempDiff, dU, A, E, tempPnt, ctrlPnt, tempU, d, M, D, T;
	int row = int(mat.rows()), col = int(mat.cols()), stepSize = 3, outIdx=0;
	double tempSum = 0.0, dt=0.0;

	U.setZero(1, row + stepSize + 3);
	dU.setZero(1, row + stepSize + 3);
	A.setZero(row, row);
	E.setZero(row, col);
	temp.setZero(1, row - 1);

	for (int i = 1; i <= row - 1; i++) {
		tempDiff.setZero(1, col);
		tempDiff = mat.block(i, 0, 1, col) - mat.block(i - 1, 0, 1, col);
		temp(0, i - 1) = tempDiff.norm();
	}

	tempSum = temp.sum();

	U.block(0, row + stepSize - 1, 1, stepSize + 1).setOnes();
	for (int i = stepSize + 1; i <= row + stepSize - 2; i++) {
		U(0, i) = U(0, i - 1) + temp(0, i - stepSize - 1) / tempSum;
	}

	if (vs.norm() == 0) {
		vs = mat.block(1, 0, 1, col) - mat.block(0, 0, 1, col);
	}
	vs.normalize();

	if (ve.norm() == 0) {
		ve = mat.block(row-1, 0, 1, col) - mat.block(row-2, 0, 1, col);
	}
	ve.normalize();

	for (int i = stepSize + 1; i <= row + stepSize - 1; i++) {
		dU(0, i - 1) = U(0, i) - U(0, i - 1);
	}

	A(0, 0) = 1;
	A(row - 1, row - 1) = 1;
	E.block(0, 0, 1, col) = mat.block(0, 0, 1, col) + dU(0, 3) / 3 * vs;
	E.block(row - 1, 0, 1, col) = mat.block(row - 1, 0, 1, col) - dU(0, row + 1) / 3 * ve;

	for (int i = 2; i <= row - 1; i++) {
		A(i - 1, i - 2) = dU(0, i + 2) * dU(0, i + 2) / dU.block(0, i, 1, 3).sum();
		A(i - 1, i - 1) = dU(0, i + 2) * (dU(0, i) + dU(0, i + 1)) / dU.block(0, i, 1, 3).sum() + \
			dU(0, i + 1) * (dU(0, i + 2) + dU(0, i + 3)) / dU.block(0, i + 1, 1, 3).sum();
		A(i - 1, i) = dU(0, i + 1) * dU(0, i + 1) / dU.block(0, i + 1, 1, 3).sum();
		E.block(i - 1, 0, 1, col) = (dU(0, i + 1) + dU(0, i + 2)) * mat.block(i - 1, 0, 1, col);
	}

	tempPnt.setZero();
	tempPnt = A.inverse() * E;
	ctrlPnt.setZero(tempPnt.rows() + 2, tempPnt.cols());
	int ctrlPntRow = int(ctrlPnt.rows());
	ctrlPnt.block(0, 0, 1, col) = mat.block(0, 0, 1, col);
	ctrlPnt.block(1, 0, ctrlPntRow - 2, col) = tempPnt;
	ctrlPnt.block(ctrlPntRow - 1, 0, 1, col) = mat.block(row - 1, 0, 1, col);
	
	//cout << "theory point is: " << endl;
	//cout << ctrlPnt << endl;

	T.resize(1, 4);
	outMat.resize(N, col);
	outMat.setZero();
	dt = (ctrlPntRow - 3) / double(N);
	
	for (int i = 0; i < ctrlPntRow - 3; i++) {
		M = calCuM(U, i);
		//cout << i+1 << "th M is: " << endl;
		//cout << M << endl;
		D = ctrlPnt.block(i, 0, 4, col);
		//cout << i + 1 << "th D is: " << endl;
		//cout << D << endl;
		for (double t = 0.0; t < 1.0; t = t + dt) {
			for (int k = 0; k <= 3; k++) {
				T(0, k) = pow(t, k);
			}
			outMat.block(outIdx, 0, 1, col) = T * M * D;
			outIdx++;
			if (outIdx >= N-1) {
				break;
			}
		}
	}
	outMat.block(outIdx, 0, 1, col) = ctrlPnt.block(ctrlPntRow - 1, 0, 1, col);  // 最后一点
	return true;
}

4、调用示例

int main()
{
	string srcPath = "./srcdata.txt";
	string filePath = "./data.txt";
	Matrix<double, 6, 3> srcMat;
	MatrixXd outMat, vs, ve;
	vs.setZero(); ve.setZero();
    srcMat << 0, 0, 0, 1, 2, 2, 4, 2, 1, 5, 6, 3, 6, 2, -1, 7, 5, 0;
	calcuNURBS(srcMat, vs, ve, 1000, outMat);

	writeDataIntoFile(srcMat, srcPath);
	writeDataIntoFile(outMat, filePath);

	return 0;
}

5、matlab绘图

clc
clear 
close all

srcPnt = textread('srcdata.txt');
nurbsCurve = textread('data.txt');

figure
subplot(221)
plot(srcPnt(:, 1), srcPnt(:, 2), 'bls'); hold on; 
plot(nurbsCurve(:, 1), nurbsCurve(:, 2), 'g-.'); hold on; 
grid on; axis equal
title('x0y');
subplot(222)
plot(srcPnt(:, 1), srcPnt(:, 3), 'bls'); hold on; 
plot(nurbsCurve(:, 1), nurbsCurve(:, 3), 'g-.'); hold on; 
grid on; axis equal
title('x0z');
subplot(223)
plot(srcPnt(:, 2), srcPnt(:, 3), 'bls'); hold on; 
plot(nurbsCurve(:, 2), nurbsCurve(:, 3), 'g-.'); hold on; 
grid on; axis equal
title('y0z');
subplot(224)
plot3(srcPnt(:, 1), srcPnt(:, 2), srcPnt(:, 3), 'bls'); hold on; 
plot3(nurbsCurve(:, 1), nurbsCurve(:, 2), nurbsCurve(:, 3), 'g-.'); hold on; 
grid on; axis equal
title('xyz');  

绘制图形如下：