c - C中的四元数到欧拉角和欧拉角到四元数

我一直在编写此代码来检查欧拉角和四元数，但是它运行不正确（或者我不理解旋转角度）：

#include <stdio.h>
#include <math.h>

#define DR2D (180 / M_PI)
#define DD2R (M_PI / 180)

int main(int argc, char** argv)
{
    float x, y, z;

    x = 0 * DD2R;
    y = 0 * DD2R;
    z = 180 * DD2R;

    printf("x=%f y=%f z=%f\n", x, y, z);

    float sx = sin(x / 2);
    float sy = sin(y / 2);
    float sz = sin(z / 2);
    float cx = cos(x / 2);
    float cy = cos(y / 2);
    float cz = cos(z / 2);

    float qx, qy, qz, qw;

    printf("sx = %f sy = %f sz = %f cx = %f cy = %f cz = %f\n", sx, sy, sz, cx, cy, cy);

    qx = cx*cy*sz + sx*sy*cz;
    qy = sx*cy*cz + cx*sy*sz;
    qz = cx*sy*cz - sx*cy*sz;
    qw = cx*cy*cz - sx*sy*sz;

    printf("Quaternion -> (%f, %f, %f, %f)\n", qx, qy , qz , qw);

    //------------------------------------------------------------------
    float sqw = qw*qw;
    float sqx = qx*qx;
    float sqy = qy*qy;
    float sqz = qz*qz;
    float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
    float test = qx*qy + qz*qw;

    if (test > 0.499*unit) { // singularity at north pole
        x = 2 * atan2(qx,qw);
        y = M_PI/2;
        z = 0;
    }
    else if (test < -0.499*unit) { // singularity at south pole
        x = -2 * atan2(qx,qw);
        y = -M_PI/2;
        z = 0;
    }
    else {
        x = atan2(2*qy*qw-2*qx*qz , sqx - sqy - sqz + sqw);
        y = asin(2*test/unit);
        z = atan2(2*qx*qw-2*qy*qz , -sqx + sqy - sqz + sqw);
    }

    printf("recover euler x=%.2f y=%.2f z=%.2f\n",
        x * DR2D, y * DR2D, z * DR2D);
}

因为输出很奇怪：

例如：x180ºy90ºz90º

x=3.141593 y=1.570796 z=1.570796
sx = 1.000000 sy = 0.707107 sz = 0.707107 cx = -0.000000 cy = 0.707107 cz = 0.707107
Quaternion -> (0.500000, 0.500000, -0.500000, -0.500000)
reconversion euler x=270.00 y=90.00 z=0.00

例如x90ºy90ºz90º

x=1.570796 y=1.570796 z=1.570796
sx = 0.707107 sy = 0.707107 sz = 0.707107 cx = 0.707107 cy = 0.707107 cz = 0.707107
Quaternion -> (0.707107, 0.707107, 0.000000, 0.000000)
recover euler x=180.00 y=90.00 z=0.00

最佳答案

您使用的算法的域仅位于[0，pi / 2）区间，即第一象限。或者，因为您希望输入以度为单位，所以请在0（零）（包括0）和90度（不包括在内）之间。