qmatrix() can falter in some cases.

I feed qmatrix() a diverse range of orthonormal matrices. The resulting quaternion is nearly always fine. I sanity check it by rotating (1,0,0) vector with the quaternion to see if I get the original x axis in the matrix.

This goes well in nearly all cases, but not all cases.

Here is an orthonormal matrix that fails the test.

I compare it with an alternate matrix-to-quaternion function, which does not fail the test.

I think the issue stems from a +0 vs -0 component in the quaternion.

// bug.c
// Tested on linux with:
// $ clang -DCONFIG_MATH3D_FLOAT -g -std=c99 bug.c math3d.c -lm
// $ ./a.out

#include <math.h>
#include <stdio.h>

#define _GNU_SOURCE
#include <fenv.h>

#include "math3d.h"

static void qrot( float* __restrict__ vout, const float* __restrict__ v, const float* q )
{
        float cp0[3];
        vcrs( cp0, q, v );
        cp0[0] += q[3]*v[0];
        cp0[1] += q[3]*v[1];
        cp0[2] += q[3]*v[2];
        float cp1[3];
        vcrs( cp1, q, cp0 );
        vout[0] = v[0] + 2.0f * cp1[0];
        vout[1] = v[1] + 2.0f * cp1[1];
        vout[2] = v[2] + 2.0f * cp1[2];
}

#define MAX(A,B) ( A>B ? A : B )
static void qmatrix_alternate(float * __restrict__ q, const float * __restrict__ M)
{
        const float m00 = M[4*0+0];
        const float m01 = M[4*1+0];
        const float m02 = M[4*2+0];
        const float m10 = M[4*0+1];
        const float m11 = M[4*1+1];
        const float m12 = M[4*2+1];
        const float m20 = M[4*0+2];
        const float m21 = M[4*1+2];
        const float m22 = M[4*2+2];

        q[3] = sqrtf( MAX( 0, 1 + m00 + m11 + m22 ) ) / 2;
        q[0] = sqrtf( MAX( 0, 1 + m00 - m11 - m22 ) ) / 2;
        q[1] = sqrtf( MAX( 0, 1 - m00 + m11 - m22 ) ) / 2;
        q[2] = sqrtf( MAX( 0, 1 - m00 - m11 + m22 ) ) / 2;
        q[0] = copysignf( q[0], m21 - m12 );
        q[1] = copysignf( q[1], m02 - m20 );
        q[2] = copysignf( q[2], m10 - m01 );
}

int main()
{
    feenableexcept( FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW );

    float m[16] =
    {
        // xaxis
        -0.901274, -0.000000, 0.433249, 0,
        // yaxis
        0.000000, -1.000000, -0.000000, 0,
        // zaxis
        0.433249, -0.000000, 0.901274, 0,
        // p
        0,0,1,0
    };

    const float x[4] = {1,0,0,0};
    float tx[4];

    float q0[4] = {-1,-1,-1,-1};
    qmatrix_alternate( q0, m );
    qrot( tx, x, q0 );
    fprintf( stdout, "q with alternate: %f %f %f %f\n", q0[0], q0[1], q0[2], q0[3] );
    fprintf( stdout, "tx with alternate: %f %f %f\n", tx[0], tx[1], tx[2] );

    float q1[4] = {-1,-1,-1,-1};
    qmatrix( q1, m );
    qrot( tx, x, q1 );
    fprintf( stdout, "q with original: %f %f %f %f\n", q1[0], q1[1], q1[2], q1[3] );
    fprintf( stdout, "tx with original: %f %f %f\n", tx[0], tx[1], tx[2] );
}

rlk / util3d

qmatrix() can falter in some cases. #1