四元数转欧拉角

四元数转欧拉角_在线测试工具
在线测试C程序工具

#include <stdio.h>
#include <cmath>

//四元数
struct Quaternion
{
    double w, x, y, z;
};

//欧拉角
struct EulerAngles
{
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q)
{
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // 如果超出范围，使用90度
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

int main(void)
{
    Quaternion wxyz = {
        //输入四元数数据
        .w = -0.529,
        .x = 0.002,
        .y = 0.805,
        .z = -0.268,
    };

    EulerAngles out = ToEulerAngles(wxyz); //转换成欧拉角

    printf("r=%f p=%f y=%f ",
           out.roll / M_PI * 180.0,
           out.pitch / M_PI * 180.0,
           out.yaw / M_PI * 180.0);

    return 0;
}

测试结果