矩阵转四元素的方法-已验证

cxmsmgr

于 2018-04-23 11:51:56 发布

阅读量1.1k

点赞数

CC 4.0 BY-SA版权

分类专栏：算法坐标转换四元素

本文链接：https://blog.youkuaiyun.com/jjgg171314/article/details/80048503

//矩阵转四元素的方法
// float mat[3][4] = { 00 01 02
//                     10 11 12
//                     20 21 22
//                    }
Quaternion convert3x3MatrixToQuaternion2(float mat[3][3])
{
    float t,S,X,Y,Z,W;
    
    t = mat[0][0] + mat[1][1] + mat[2][2] + 1.0f;
    if (t > 0.0001f) {
        S = sqrtf(t) * 2.0f;
        X = (mat[1][2] - mat[2][1]) / S;
        Y = (mat[2][0] - mat[0][2]) / S;
        Z = (mat[0][1] - mat[1][0]) / S;
        W = 0.25f * S;
    }else {
        
        if ( mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2] )  {
            S  = sqrt( 1.0 + mat[0][0] - mat[1][1] - mat[2][2] ) * 2;
            X = 0.25 * S;
            Y = (mat[0][1] + mat[1][0] ) / S;
            Z = (mat[2][0] + mat[0][2] ) / S;
            W = (mat[1][2] - mat[2][1] ) / S;
        } else if ( mat[1][1] > mat[2][2] ) {
            S  = sqrt( 1.0 + mat[1][1] - mat[0][0] - mat[2][2] ) * 2;