三维空间任意一点绕任意轴线旋转C++

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

三维空间任意一点绕任意轴线旋转的C++实现，公式推导以及matlab实现请参照该链接：
https://blog.csdn.net/maple_2014/article/details/104443928

C++实现

//三维空间任意一点绕任意轴线旋转
//n为轴线法向量（且为单位向量），pn为轴线上一点，p为输入点，angle为旋转的角度（弧度）,p_out为输出点
void LineRotation(Eigen::Vector3f& n, Eigen::Vector3f& pn, Eigen::Vector4f& p, float angle, Eigen::Vector4f& p_out)
{
	//方法一：按过程推导
	//在XYZ坐标系下：
	//轴线n直线方程：x = x0 + n.x * t / y = y0 + n.y * t / z = z0 + n.z * t ——公式一;
	//p点旋转到p_out点的圆弧轨迹所在的平面方程：n.x * (x - p.x) + n.y * (y - p.y) + n.z * (z - p.z) = 0 ——公式二
	//由公式一和公式二可得：t0 = n.x * (p.x - pn.x) + n.y * (p.y - pn.y) + n.z * (p.z - pn.z)
	float t0 = n(0) * (p(0) - pn(0)) + n(1) * (p(1) - pn(1)) + n(2) * (p(2) - pn(2));//表示轴线与圆弧平面交点处的t（直线方程）

	//圆弧圆心O
	float xc = pn(0) + n(0) * t0;
	float yc = pn(1) + n(1) * t0;
	float zc = pn(2) + n(2) * t0;

	//圆弧半径
	float r = sqrt(pow((p(0) - xc), 2) + pow((p(1) - yc), 2) + pow((p(2) - zc), 2));

	//输入的任意点P,圆心到P点的向量：OP
	Eigen::Vector3f OP;
	OP << (p(0) - xc) / r, (p(1) - yc) / r, (p(2) - zc) / r;

	//建立坐标系X'Y'Z',根据右手法则计算Y'，其中X' = OP，Z' = n
	Eigen::Vector3f Y1 = n.cross(OP);

	//因此坐标系X'Y'Z'与坐标系XYZ的转转矩阵为
	Eigen::Matrix3f R;
	R << OP(0), Y1(0), n(0),
		 OP(1), Y1(1), n(1),
		 OP(2), Y1(2), n(2);

	//P1(也即P旋转后的点)在坐标系X'Y'Z'下的坐标为P_temp，在XYZ坐标系下坐标为P1
	float x_temp = r * cos(angle);
	float y_temp = r * sin(angle);
	float z_temp = 0;

	//利用齐次变换，将P1在坐标系X'Y'Z'的坐标变换到XYZ的坐标
	//P1 = T * P_temp;
	//其中T = [R t 0 1]——4x4矩阵
	p_out(0) = R(0, 0) * x_temp + R(0, 1) * y_temp + R(0, 2) * z_temp + xc;
	p_out(1) = R(1, 0) * x_temp + R(1, 1) * y_temp + R(1, 2) * z_temp + yc;
	p_out(2) = R(2, 0) * x_temp + R(2, 1) * y_temp + R(2, 2) * z_temp + zc;
	p_out(3) = 1;
	//std::cout << "p_out1 = " << p_out.matrix().transpose() << std::endl;

	方法二：直接按结论
	//float K = 1 - cos(angle);
	//float M = n(0) * pn(0) + n(1) * pn(1) + n(2) * pn(2);

	//Eigen::Matrix4f T;
	//T << pow(n(0), 2)* K + cos(angle), n(0) * n(1) * K - n(2) * sin(angle), n(0) * n(2) * K + n(1) * sin(angle), (pn(0) - n(0) * M) * K + (n(2) * pn(1) - n(1) * pn(2)) * sin(angle),
	//	n(0) * n(1) * K + n(2) * sin(angle), pow(n(1), 2) * K + cos(angle), n(1) * n(2) * K - n(0) * sin(angle), (pn(1) - n(1) * M) * K + (n(0) * pn(2) - n(2) * pn(0)) * sin(angle),
	//	n(0) * n(2) * K - n(1) * sin(angle), n(1) * n(2) * K + n(0) * sin(angle), pow(n(2), 2) * K + cos(angle), (pn(2) - n(2) * M) * K + (n(1) * pn(0) - n(0) * pn(1)) * sin(angle),
	//	0, 0, 0, 1;

	//p_out = T * p;
	//std::cout << "p_out2 = " << p_out.matrix().transpose() << std::endl;
}