c++ - 旋转矩阵到四元数(和返回)有什么问题？

Question

我复制了一个代码，用于将 3D 旋转矩阵转换为四元数并返回。 jMonkey 中使用了相同的代码(我只是将它重写到我的 C++ 类中)。但是，它无法正常工作(至少不像我预期的那样。)

例如我做了这个测试:

matrix (a,b,c):  
a : 0.707107 0.000000 0.707107 
b : 0.000000 -1.000000 0.000000 
c : -0.707107 0.000000 0.707107 

>>> ortonormality:  
a.a b.b c.c  1.000000 1.000000 1.000000  
a.b a.c b.c  0.000000 0.000000 0.000000 

>>> matrix -> quat 
quat: 0.000000 0.594604 0.000000 0.594604  norm(quat) 0.707107  

>>> quat -> matrix  
matrix (a,b,c):
a: 0.000000 0.000000 1.000000 
b: 0.000000 1.000000 0.000000 
c: -1.000000 0.000000 0.000000 

我认为问题出在 matrix -> quat 因为我之前使用过quat -> matrix 过程，而且它工作正常.此外，奇怪的是，由正交矩阵构成的四元数不是幺正的。

matrix -> quat 过程

inline void fromMatrix( TYPE m00, TYPE m01, TYPE m02,    TYPE m10, TYPE m11, TYPE m12,        TYPE m20, TYPE m21, TYPE m22) {
    // Use the Graphics Gems code, from 
    // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
    TYPE t = m00 + m11 + m22;
    // we protect the division by s by ensuring that s>=1
    if (t >= 0) { // by w
        TYPE s = sqrt(t + 1);
        w = 0.5 * s;
        s = 0.5 / s;                 
        x = (m21 - m12) * s;
        y = (m02 - m20) * s;
        z = (m10 - m01) * s;
    } else if ((m00 > m11) && (m00 > m22)) { // by x
        TYPE s = sqrt(1 + m00 - m11 - m22); 
        x = s * 0.5; 
        s = 0.5 / s;
        y = (m10 + m01) * s;
        z = (m02 + m20) * s;
        w = (m21 - m12) * s;
    } else if (m11 > m22) { // by y
        TYPE s = sqrt(1 + m11 - m00 - m22); 
        y = s * 0.5; 
        s = 0.5 / s;
        x = (m10 + m01) * s;
        z = (m21 + m12) * s;
        w = (m02 - m20) * s;
    } else { // by z
        TYPE s = sqrt(1 + m22 - m00 - m11); 
        z = s * 0.5; 
        s = 0.5 / s;
        x = (m02 + m20) * s;
        y = (m21 + m12) * s;
        w = (m10 - m01) * s;
    }
}

quat -> matrix 过程

inline void toMatrix( MAT& result) const {
        TYPE r2 = w*w + x*x + y*y + z*z;
        //TYPE s  = (r2 > 0) ? 2d / r2 : 0;
        TYPE s  = 2 / r2;
        // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
        // will be used 2-4 times each.
        TYPE xs = x * s;  TYPE ys = y * s;  TYPE zs = z * s;
        TYPE xx = x * xs; TYPE xy = x * ys; TYPE xz = x * zs;
        TYPE xw = w * xs; TYPE yy = y * ys; TYPE yz = y * zs;
        TYPE yw = w * ys; TYPE zz = z * zs; TYPE zw = w * zs;
        // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
        result.xx = 1 - (yy + zz);
        result.xy =     (xy - zw);
        result.xz =     (xz + yw);
        result.yx =     (xy + zw);
        result.yy = 1 - (xx + zz);
        result.yz =     (yz - xw);
        result.zx =     (xz - yw);
        result.zy =     (yz + xw);
        result.zz = 1 - (xx + yy);
};

抱歉 TYPE, VEC, MAT, QUAT 它是类 templtes 的一部分...应该替换为 double, Vec3d, Mat3d, Quat3d 或 float 、Vec3f、Mat3f、Quat3f。

编辑:

我还检查了我是否直接使用 jMonkey 获得相同的行为(以防我在 Java 到 C++ 的转换中出现错误)。我这样做 - 使用此代码:

    Matrix3f Min  = new Matrix3f( 0.707107f, 0.000000f, 0.707107f, 0.000000f, -1.000000f, 0.000000f, -0.707107f,  0.000000f,  0.707107f  );
    Matrix3f Mout = new Matrix3f( );
    Quaternion q = new Quaternion();
    q.fromRotationMatrix(Min);
    System.out.println( q.getX()+" "+q.getY()+" "+q.getZ()+" "+q.getW() );
    q.toRotationMatrix(Mout);
    System.out.println( Mout.get(0,0) +" "+Mout.get(0,1)+" "+Mout.get(0,2) );
    System.out.println( Mout.get(1,0) +" "+Mout.get(1,1)+" "+Mout.get(1,2) );
    System.out.println( Mout.get(2,0) +" "+Mout.get(2,1)+" "+Mout.get(2,2) );

Answer 1

你的矩阵:

matrix (a,b,c):  
a : 0.707107 0.000000 0.707107 
b : 0.000000 -1.000000 0.000000 
c : -0.707107 0.000000 0.707107

是正交的，但它不是旋转矩阵。旋转矩阵的行列式为 1；您的矩阵具有行列式 -1，因此是 improper rotation .

我认为您的代码可能是正确的，问题出在您的数据中。尝试使用真实的旋转矩阵。