クォータニオンはC

Question

私はオイラー角と四元数をチェックするため、このコードを書いてきた

をクォータニオンするが、それはcorrecly実行されていない（または多分私は回転を理解していない）：

#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); 
} 

出力は非常に奇妙であるので：

たとえば：X 180°Y 90°Z 90°

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 

あるいは例えばX 90°Y 90°Z 90°

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 

Answer 1

使用するアルゴリズムには、区間[0、pi/2]にのみ属するドメインがあり、最初の象限になります。または、入力を0度から90度までの度数で入力したいからです。