Matrix 转 四元数 已验证

方法一：

float qw,qx,qy,qz;

float tr = pose[0] + pose[5] + pose[10];
            
            if (tr > 0) {
                float S = sqrt(tr+1.0) * 2; // S=4*qw
                qw = 0.25 * S;
                qx = (pose[9] - pose[6]) / S;
                qy = (pose[2] - pose[8]) / S;
                qz = (pose[4] - pose[1]) / S;
            } else if ((pose[0] > pose[5])&(pose[0] > pose[10])) {
                float S = sqrt(1.0 + pose[0] - pose[5] - pose[10]) * 2; // S=4*qx
                qw = (pose[9] - pose[6]) / S;
                qx = 0.25 * S;
                qy = (pose[1] + pose[4]) / S;
                qz = (pose[2] + pose[8]) / S;
            } else if (pose[5] > pose[10]) {
                float S = sqrt(1.0 + pose[5] - pose[0] - pose[10]) * 2; // S=4*qy
                qw = (pose[2] - pose[8]) / S;
                qx = (pose[1] + pose[4]) / S;
                qy = 0.25 * S;
                qz = (pose[6] + pose[9]) / S;
            } else { 
                float S = sqrt(1.0 + pose[10] - pose[0] - pose[5]) * 2; // S=4*qz
                qw = (pose[4] - pose[1]) / S;
                qx = (pose[2] + pose[8]) / S;
                qy = (pose[6] + pose[9]) / S;
                qz = 0.25 * S;
            }

方法二：

    if (m[0] > m[5] && m[0] > m[10])
    {
        float v84 = sqrt(m[0] + 1.0 - m[5] - m[10]) * 2;
        q[0] = v84 * 0.25f;
        q[1] = (m[1] + m[4]) / v84;
        q[2] = (m[2] + m[8]) / v84;
        q[3] = (m[6] - m[9]) / v84;
    }
    else if(m[5] > m[10])
    {
        float v55 = sqrt(m[5] + 1.0 - m[0] - m[10]) * 2;
        q[0] = (m[1] + m[4]) / v55;
        q[1] = v55 * 0.25f;
        q[2] = (m[6] + m[9]) / v55;
        q[3] = (m[8] - m[2]) / v55;
    }
    else
    {
        float v25 = sqrt(m[10] + 1.0 - m[0] - m[5]) * 2;
        q[0] = (m[8] + m[2]) / v25;
        q[1] = (m[6] + m[9]) / v25;
        q[2] = v25 * 0.25f;
        q[3] = (m[1] - m[4]) / v25;
    }