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