Quaternion.from_matrix #32

Closed
mbj2011 opened this Issue Apr 5, 2014 · 3 comments

Projects

None yet

2 participants

@mbj2011
mbj2011 commented Apr 5, 2014

Produced weird results, so I redid it using http://www.flipcode.com/documents/matrfaq.html#Q55 as a reference and now it seems to work. fingers crossed

    /// Creates a quaternion from a 3x3 matrix.
    /// Params:
    ///  matrix = 3x3 matrix (rotation)
    /// Returns: A quaternion representing the rotation (3x3 matrix)
    static Quaternion from_matrix(Matrix!(qt, 3, 3) matrix) {
        Quaternion ret;

        auto mat = matrix.matrix;
        qt trace = mat[0][0] + mat[1][1] + mat[2][2] + 1.0;

        if(trace > 0) {
            real s = 0.5 / sqrt(trace);

            ret.w = to!qt(0.25 / s);
            ret.x = to!qt((mat[2][1] - mat[1][2]) * s);
            ret.y = to!qt((mat[0][2] - mat[2][0]) * s);
            ret.z = to!qt((mat[1][0] - mat[0][1]) * s);
        } else if((mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2])) {
            real s = 2.0 * sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]);

            ret.w = to!qt((mat[2][1] + mat[1][2]) / s);
            ret.x = to!qt(0.5 / s);
            ret.y = to!qt((mat[0][1] + mat[1][0]) / s);
            ret.z = to!qt((mat[0][2] + mat[2][0]) / s);
        } else if(mat[1][1] > mat[2][2]) {
            real s = 2.0 * sqrt(1 + mat[1][1] - mat[0][0] - mat[2][2]);

            ret.w = to!qt((mat[0][2] + mat[2][0]) / s);
            ret.x = to!qt((mat[0][1] + mat[1][0]) / s);
            ret.y = to!qt(0.5 / s);
            ret.z = to!qt((mat[1][2] + mat[2][1]) / s);
        } else {
            real s = 2.0 * sqrt(1 + mat[2][2] - mat[0][0] - mat[1][1]);

            ret.w = to!qt((mat[1][0] + mat[0][1]) / s);
            ret.x = to!qt((mat[0][2] + mat[2][0]) / s);
            ret.y = to!qt((mat[1][2] + mat[2][1]) / s);
            ret.z = to!qt(0.5f / s);
        }

        return ret;
    }

I hope the code compiles as I did it in a text editor using the separate function I got working in my project =)

@Dav1dde
Owner
Dav1dde commented Apr 5, 2014

Thanks for your continuing bug reports and improvments! I will look into this as soon as possible, but on the first glance, this looks solid.

@Dav1dde Dav1dde added a commit that referenced this issue Apr 9, 2014
@Dav1dde "merge" #32 903445b
@Dav1dde
Owner
Dav1dde commented Apr 9, 2014

I still don't have much, time, I checked if it compiles and just put it in. Thanks for the code, if it doesn't work, please reopen it.

@Dav1dde Dav1dde closed this Apr 9, 2014
@mbj2011
mbj2011 commented Apr 21, 2014

I can't seem to reopen the issue, I hope it's alright that I just post a comment.

The posted function seems to have problems as well, I'm so sorry for inconveniencing you with more broken code :(

I've been working with the following:

    /// Creates a quaternion from a 3x3 matrix.
    /// Params:
    ///  matrix = 3x3 matrix (rotation)
    /// Returns: A quaternion representing the rotation (3x3 matrix)
    static Quaternion from_matrix(Matrix!(qt, 3, 3) matrix) {
        Quaternion ret;

        auto mat = matrix.matrix;
        qt trace = mat[0][0] + mat[1][1] + mat[2][2];

        if(trace > 0) {
            qt s = 0.5 / sqrt(trace + 1.0f);

            ret.w = to!qt(0.25 / s);
            ret.x = to!qt((mat[2][1] - mat[1][2]) * s);
            ret.y = to!qt((mat[0][2] - mat[2][0]) * s);
            ret.z = to!qt((mat[1][0] - mat[0][1]) * s);
        } else if((mat[0][0] > mat[1][1]) && (mat[0][0] > mat[2][2])) {
            qt s = 2.0 * sqrt(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);

            ret.w = to!qt((mat[2][1] - mat[1][2]) / s);
            ret.x = to!qt(0.25f * s);
            ret.y = to!qt((mat[0][1] + mat[1][0]) / s);
            ret.z = to!qt((mat[0][2] + mat[2][0]) / s);
        } else if(mat[1][1] > mat[2][2]) {
            qt s = 2.0 * sqrt(1 + mat[1][1] - mat[0][0] - mat[2][2]);

            ret.w = to!qt((mat[0][2] - mat[2][0]) / s);
            ret.x = to!qt((mat[0][1] + mat[1][0]) / s);
            ret.y = to!qt(0.25f * s);
            ret.z = to!qt((mat[1][2] + mat[2][1]) / s);
        } else {
            qt s = 2.0 * sqrt(1 + mat[2][2] - mat[0][0] - mat[1][1]);

            ret.w = to!qt((mat[1][0] - mat[0][1]) / s);
            ret.x = to!qt((mat[0][2] + mat[2][0]) / s);
            ret.y = to!qt((mat[1][2] + mat[2][1]) / s);
            ret.z = to!qt(0.25f * s);
        }

        return ret;
    }

without any trouble for a while now.

I'm fairly certain that it works since the output of loads of tests have magnitude 1.

Reference:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

@Dav1dde Dav1dde reopened this Apr 21, 2014
@Dav1dde Dav1dde closed this Sep 20, 2014
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment