欧拉角和四元数之间的转换

绝地灬酷狼 2021-11-29 19:20 815阅读 0赞

#  欧拉角和四元数之间的转换 #

![20190722172943253.png][]

// 四元数转Euler
    // q0 q1 q2 q3
    // w x y z
    void quaternion_2_euler(float quat[4], float angle[3])
    {
        angle[0] = atan2(2.0 * (quat[3] * quat[2] + quat[0] * quat[1]), 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2]));
        angle[1] = asin(2.0 * (quat[2] * quat[0] - quat[3] * quat[1]));
        angle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), -1.0 + 2.0 * (quat[0] * quat[0] + quat[1] * quat[1]));
        //angle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), 1.0 - 2.0 * (quat[2] * quat[2] + quat[3] * quat[3]));
    }

![20190722173147668.png][]

// Euler转四元数
    // q0 q1 q2 q3
    // w x y z
    void euler_2_quaternion(float angle[3], float quat[4])
    {
        double cosPhi_2 = cos(double(angle[0]) / 2.0);
        double sinPhi_2 = sin(double(angle[0]) / 2.0);
        double cosTheta_2 = cos(double(angle[1] ) / 2.0);
        double sinTheta_2 = sin(double(angle[1] ) / 2.0);
        double cosPsi_2 = cos(double(angle[2]) / 2.0);
        double sinPsi_2 = sin(double(angle[2]) / 2.0);
    
        quat[0] = float(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2);
    
        quat[1] = float(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2);
    
        quat[2] = float(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2);
    
        quat[3] = float(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
    
    }

[20190722172943253.png]: /images/20211129/075ef63d4ac64b369b9e1be2eb83455a.png
[20190722173147668.png]: /images/20211129/502a8036a627497f847e1f5d19f39d64.png