c++写一个函数 用来将坐标从一个3D坐标系转换到另一个3D坐标系

在C++中，我们可以创建一个函数来处理3D坐标的变换。例如，如果我们要从笛卡尔坐标系统（XYZ）转换到极坐标系统（rθφ），通常涉及到球面坐标到笛卡尔坐标的转换公式。假设我们已经有了欧拉角或其他适当的旋转矩阵来完成这种转换。这里是一个简单的函数示例：

#include <cmath>

// 定义3D点结构体
struct Vector3 {
    double x, y, z;
};

// 函数原型，接受原始3D点和两个坐标系之间的转换矩阵（假设是4x4的旋转矩阵）
Vector3 convertCoordinates(Vector3 original, Matrix4x4 fromCartesian, Matrix4x4 toCartesian) {
    // 将点添加到矩阵的最后一行，然后做矩阵乘法
    Vector4 pointWithOrigin = {original.x, original.y, original.z, 1};
    Vector4 transformed = fromCartesian * pointWithOrigin;

    // 取出转换后的三维坐标
    double newX = transformed.x / transformed.w;   // 注意除以w防止数值溢出
    double newY = transformed.y / transformed.w;
    double newZ = transformed.z / transformed.w;

    return {newX, newY, newZ};
}