三维坐标绕任意轴旋转计算方法


struct XYZ
{
	double x;
	double y;
	double z;
};

void Normalise(XYZ* s)
{
	double s2 = sqrt(s->x*s->x + s->y*s->y + s->z*s->z);
	s->x = s->x / s2;
	s->y = s->y / s2;
	s->z = s->z / s2;
}

XYZ RotatePointAboutLine(XYZ p, double theta, XYZ p1, XYZ p2)
{
	XYZ u, q1, q2;
	double d;

	/* Step 1 */
	q1.x = p.x - p1.x;
	q1.y = p.y - p1.y;
	q1.z = p.z - p1.z;

	u.x = p2.x - p1.x;
	u.y = p2.y - p1.y;
	u.z = p2.z - p1.z;
	Normalise(&u);
	d = sqrt(u.y*u.y + u.z*u.z);

	/* Step 2 */
	if (d != 0) {
		q2.x = q1.x;
		q2.y = q1.y * u.z / d - q1.z * u.y / d;
		q2.z = q1.y * u.y / d + q1.z * u.z / d;
	}
	else {
		q2 = q1;
	}

	/* Step 3 */
	q1.x = q2.x * d - q2.z * u.x;
	q1.y = q2.y;
	q1.z = q2.x * u.x + q2.z * d;

	/* Step 4 */
	q2.x = q1.x * cos(theta) - q1.y * sin(theta);
	q2.y = q1.x * sin(theta) + q1.y * cos(theta);
	q2.z = q1.z;

	/* Inverse of step 3 */
	q1.x = q2.x * d + q2.z * u.x;
	q1.y = q2.y;
	q1.z = -q2.x * u.x + q2.z * d;

	/* Inverse of step 2 */
	if (d != 0) {
		q2.x = q1.x;
		q2.y = q1.y * u.z / d + q1.z * u.y / d;
		q2.z = -q1.y * u.y / d + q1.z * u.z / d;
	}
	else {
		q2 = q1;
	}

	/* Inverse of step 1 */
	q1.x = q2.x + p1.x;
	q1.y = q2.y + p1.y;
	q1.z = q2.z + p1.z;
	return(q1);
}
	XYZ line_n = { 6.0,-2.0,0.0 }, line_m = { 12.0,8.0,0.0 };
	XYZ p1 = { 3.0,5.0,0.0 }, p2 = { 10.0,6.0,0.0 }, p3 = { 1.0,1.0,0.0 }, p4 = { 3.0,5.0,0.0 };
	p1 = RotatePointAboutLine(p1, 60.0/180.0*3.14159265, line_n, line_m);

p1结果:

P1      P2          P3         P4
5.6471 | 10.2941 | 3.5000  | 5.6471 
3.4118 | 5.8235  | -0.5000 | 3.4118 
5.3468 | 0.5941  | 5.0498  | 5.3468

    原文作者:novanova2009
    原文地址: https://blog.csdn.net/novanova2009/article/details/122387673
    本文转自网络文章,转载此文章仅为分享知识,如有侵权,请联系博主进行删除。
点赞