用c++实现三维点的旋转

假设我们要将一个三维点 $(x,y,z)$ 绕着单位向量 $(u,v,w)$ 旋转 $\theta$ 角度，那么可以使用以下代码实现：

#include <cmath>
#include <iostream>

using namespace std;

struct Point3D {
    double x, y, z;
    Point3D(double x = 0, double y = 0, double z = 0): x(x), y(y), z(z) {}
};

// 计算向量长度
double length(double x, double y, double z) {
    return sqrt(x * x + y * y + z * z);
}

// 将向量归一化
void normalize(double& x, double& y, double& z) {
    double len = length(x, y, z);
    if (len > 0) {
        x /= len;
        y /= len;
        z /= len;
    }
}

// 将三维点绕着指定的向量旋转
void rotate(Point3D& p, double u, double v, double w, double theta) {
    // 将向量归一化
    normalize(u, v, w);

    // 计算旋转矩阵
    double angle = theta * M_PI / 180;  // 角度转弧度
    double cos_theta = cos(angle);
    double sin_theta = sin(angle);
    double rotation_matrix[3][3] = {
        {cos_theta + (1 - cos_theta) * u * u, (1 - cos_theta) * u * v - sin_theta * w, (1 - cos_theta) * u * w + sin_theta * v},
        {(1 - cos_theta) * v * u + sin_theta * w, cos_theta + (1 - cos_theta) * v * v, (1 - cos_theta) * v * w - sin_theta * u},
        {(1 - cos_theta) * w * u - sin_theta * v, (1 - cos_theta) * w * v + sin_theta * u, cos_theta + (1 - cos_theta) * w * w}
    };

    // 计算旋转后的坐标
    double x = p.x * rotation_matrix[0][0] + p.y * rotation_matrix[0][1] + p.z * rotation_matrix[0][2];
    double y = p.x * rotation_matrix[1][0] + p.y * rotation_matrix[1][1] + p.z * rotation_matrix[1][2];
    double z = p.x * rotation_matrix[2][0] + p.y * rotation_matrix[2][1] + p.z * rotation_matrix[2][2];

    // 更新点的坐标
    p.x = x;
    p.y = y;
    p.z = z;
}

int main() {
    Point3D p(1, 0, 0);
    double u = 0, v = 0, w = 1;
    double theta = 90;
    rotate(p, u, v, w, theta);
    cout << "(" << p.x << ", " << p.y << ", " << p.z << ")" << endl;  // 输出旋转后的坐标
    return 0;
}

以上代码中，`normalize` 函数用于将向量归一化，`rotate` 函数用于计算点的旋转，其中旋转矩阵的计算使用了 Rodrigues 公式。在 `main` 函数中，我们定义了一个三维点 $(1,0,0)$，绕着 $z$ 轴旋转 $90$ 度，最终输出旋转后的坐标。