0

假设通过从四元数到 Vecmath 类的矩阵的任意转换应该保持与向量/点的相同转换,我错了吗?

这是我的测试脚本。它只是循环打印旋转的矢量,测试结果并将四元数转换为矩阵,反之亦然。

public class Test {

    public static Vector3d rotateVector( Quat4d q, Vector3d v )
    {
        Quat4d w = new Quat4d();
        w.x = v.x;
        w.y = v.y;
        w.z = v.z;
        w.w = 0;

        Quat4d qTmp = new Quat4d();

        // q * w * q^-1

        qTmp.mul( q, w );
        qTmp.mulInverse( q );

        return new Vector3d( qTmp.x, qTmp.y, qTmp.z );
    }

    public static Vector3d rotateVector( Matrix4d m, Vector3d v )
    {
        Point3d vp = new Point3d( v );

        m.transform( vp );

        return new Vector3d( vp );
    }

    public static final double EPS = 0.00000001;

    public static void main( String[] args )
    {
        Quat4d q = new Quat4d();
        q.set( new AxisAngle4d( new Vector3d( 1, 0, -1 ), Math.PI ) ); // same as rotation x 90°, y 90°, z -90° in static frame

        Matrix4d m = new Matrix4d();

        Vector3d v = new Vector3d( 1, 2, 3 );

        Vector3d vResult = new Vector3d( -3, -2, -1 );

        String rotMethod = "";

        for( int i = 0; i < 4; i++ )
        {
            rotMethod += "q";
            Vector3d vq = rotateVector( q, v );
            System.out.println( rotMethod + ": " + vq + " ==> " + vResult.epsilonEquals( vq, EPS ) );

            m.set( q );

            rotMethod += "m";
            Vector3d vm = rotateVector( m, v );
            System.out.println( rotMethod + ": " + vm + " ==> " + vResult.epsilonEquals( vm, EPS ) );

            q.set( m );
        }
    }
}

这是输出。第一个四元数和第一个矩阵都很好。但随后它们逐渐偏离并收敛到一个有点镜像的点。

q: (-3.0, -2.0000000000000004, -1.0) ==> true
qm: (-2.9999999999999987, -1.9999999999999993, -0.9999999999999989) ==> true
qmq: (1.4421707565920456, 0.2675892211829013, 3.4421707565920463) ==> false
qmqm: (1.0000000000000002, 1.9999999999999998, 3.0) ==> false
qmqmq: (1.0, 1.9999999999999996, 3.0) ==> false
qmqmqm: (1.0000000000000002, 1.9999999999999998, 3.0) ==> false
qmqmqmq: (1.0, 1.9999999999999996, 3.0) ==> false
qmqmqmqm: (1.0000000000000002, 1.9999999999999998, 3.0) ==> false

提前感谢您的帮助和时间。


编辑:解决方法

我可以在网络上找到的每个矩阵到四元数的转换都与我的测试脚本(如 vecmath 库)不稳定。

测试了以下实现:

使用Matrix -> Euler -> Quaternion的解决方法提供了预期的结果。我融合了Christoph Gohlke的两种转换的实现。

public static void setQuatFromMatrix( Quat4d quat, Matrix4d mat )
{
    double az, ay, ax;
    double ai, aj, ak;
    double si, sj, sk;
    double ci, cj, ck;
    double cy, cc, cs, sc, ss;

    cy = Math.sqrt( mat.m00 * mat.m00 + mat.m10 * mat.m10 );

    if( cy > MathUtil.EPS )
    {
        ax = Math.atan2( mat.m21, mat.m22 );
        ay = Math.atan2( -mat.m20, cy );
        az = Math.atan2( mat.m10, mat.m00 );
    }
    else
    {
        ax = Math.atan2( -mat.m12, mat.m11 );
        ay = Math.atan2( -mat.m20, cy );
        az = 0.0;
    }

    ai = ax / 2.0;
    aj = ay / 2.0;
    ak = az / 2.0;

    ci = Math.cos( ai );
    si = Math.sin( ai );
    cj = Math.cos( aj );
    sj = Math.sin( aj );
    ck = Math.cos( ak );
    sk = Math.sin( ak );
    cc = ci * ck;
    cs = ci * sk;
    sc = si * ck;
    ss = si * sk;

    quat.x = cj * sc - sj * cs;
    quat.y = cj * ss + sj * cc;
    quat.z = cj * cs - sj * sc;
    quat.w = cj * cc + sj * ss;
}

我没有优化它,因为我不知道它是如何工作的,但它对我有用。这是输出:

q: (-3.0, -2.0000000000000004, -1.0) ==> true
qm: (-2.9999999999999987, -1.9999999999999993, -0.9999999999999989) ==> true
qmq: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000009) ==> true
qmqm: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000007) ==> true
qmqmq: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000009) ==> true
qmqmqm: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000007) ==> true
qmqmqmq: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000009) ==> true
qmqmqmqm: (-2.9999999999999996, -2.0000000000000004, -1.0000000000000007) ==> true
4

0 回答 0