将四元数转换成欧拉角的代码

下面是将四元数转换成欧拉角的代码，其中 `q` 是一个四元数，`roll`、`pitch` 和 `yaw` 分别是对应的欧拉角：

#include <cmath>

void quaternion_to_euler(double q[4], double& roll, double& pitch, double& yaw) {
    double sinr_cosp = 2.0 * (q[0] * q[1] + q[2] * q[3]);
    double cosr_cosp = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
    roll = std::atan2(sinr_cosp, cosr_cosp);

    double sinp = 2.0 * (q[0] * q[2] - q[3] * q[1]);
    if (std::abs(sinp) >= 1.0)
        pitch = std::copysign(M_PI / 2.0, sinp);
    else
        pitch = std::asin(sinp);

    double siny_cosp = 2.0 * (q[0] * q[3] + q[1] * q[2]);
    double cosy_cosp = 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]);
    yaw = std::atan2(siny_cosp, cosy_cosp);
}

这里使用了 C++ 标准库中的 `std::atan2`、`std::asin` 和 `std::copysign` 函数，需要在代码文件开头添加头文件 `#include <cmath>`。