标签：std 欧拉角 double pitch 四元 angles cosp 数转

四元数转欧拉角_在线测试工具
在线测试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;
}

测试结果

标签：std,欧拉角,double,pitch,四元,angles,cosp,数转
来源： https://blog.csdn.net/weixin_43808708/article/details/122208403

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。