用C++写代码 四参数坐标转换

在C++中，如果你需要编写一个函数来进行四元数坐标之间的转换，比如从三维欧几里得坐标到旋转四元数，或者反过来，可以使用数学库如Eigen来处理这种复杂的数学运算。以下是使用Eigen库的一个简单示例，假设我们有一个名为`quaternion_from_euler`的函数将欧拉角转换为四元数，以及`euler_from_quaternion`用于反向操作：

#include <Eigen/Dense>

// 欧拉角转四元数
Eigen::Matrix4f quaternion_from_euler(float roll, float pitch, float yaw) {
    Eigen::AngleAxisf r(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf p(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf y(yaw, Eigen::Vector3f::UnitZ());

    return r * p * y;
}

// 四元数转欧拉角
std::tuple<float, float, float> euler_from_quaternion(const Eigen::Quaternionf& quat) {
    const double x = quat.x();
    const double y = quat.y();
    const double z = quat.z();
    const double w = quat.w();

    float yaw = std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
    float pitch = std::asin(std::max(-1.0, std::min(1.0, 2 * (w * y - x * z))));
    float roll = std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + z * z));

    return std::make_tuple(roll, pitch, yaw);
}

// 示例
int main() {
    // 转换欧拉角为四元数
    Eigen::Quaternionf quat = quaternion_from_euler(0.5, 0.3, 0.7);
    
    // 反向转换并打印结果
    auto eulers = euler_from_quaternion(quat);
    float new_roll, new_pitch, new_yaw;
    std::tie(new_roll, new_pitch, new_yaw) = eulers;
    std::cout << "Converted Euler angles: Roll=" << new_roll << ", Pitch=" << new_pitch << ", Yaw=" << new_yaw << std::endl;

    return 0;
}