matrix to quaternion

matrix_to_quaternion.c

/* quaternion from matrix */
/* from Id Software, From Quaternion to Matrix and Back, 2005 */

float ReciprocalSqrt(float x)
{ 
    long i; 
    float y, r; 
    y = x * 0.5f; 
    i = *(long*)&x; 
    i = 0x5f3759df - (i >> 1); 
    r = *(float*)&i; 
    r = r * (1.5f - r * r * y); 
    return r; 
} 

void ConvertJointMatsToJointQuats(JointQuat *jointQuats, const JointMat *jointMats, const int numJoints)
{
    for(int i = 0; i < numJoints; i++)
    { 
        float *q = &jointQuats[i].q; 
        const float *m = jointMats[i].mat; 
        if(m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0f)
        { 
            float t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
            float s = ReciprocalSqrt(t) * 0.5f; 
            q[3] = s * t; 
            q[2] = (m[0 * 4 + 1] - m[1 * 4 + 0]) * s; 
            q[1] = (m[2 * 4 + 0] - m[0 * 4 + 2]) * s; 
            q[0] = (m[1 * 4 + 2] - m[2 * 4 + 1]) * s; 
        }
        else if(m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2])
        { 
            float t = + m[0 * 4 + 0] - m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f; 
            float s = ReciprocalSqrt(t) * 0.5f; 
            q[0] = s * t; 
            q[1] = (m[0 * 4 + 1] + m[1 * 4 + 0] ) * s; 
            q[2] = (m[2 * 4 + 0] + m[0 * 4 + 2] ) * s; 
            q[3] = (m[1 * 4 + 2] - m[2 * 4 + 1] ) * s; 
        }
        else if(m[1 * 4 + 1] > m[2 * 4 + 2])
        { 
            float t = - m[0 * 4 + 0] + m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f; 
            float s = ReciprocalSqrt(t) * 0.5f; 
            q[1] = s * t; 
            q[0] = (m[0 * 4 + 1] + m[1 * 4 + 0]) * s; 
            q[3] = (m[2 * 4 + 0] - m[0 * 4 + 2]) * s; 
            q[2] = (m[1 * 4 + 2] + m[2 * 4 + 1]) * s; 
        }
        else
        { 
            float t = - m[0 * 4 + 0] - m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
            float s = ReciprocalSqrt(t) * 0.5f; 
            q[2] = s * t; 
            q[3] = (m[0 * 4 + 1] - m[1 * 4 + 0]) * s; 
            q[0] = (m[2 * 4 + 0] + m[0 * 4 + 2]) * s; 
            q[1] = (m[1 * 4 + 2] + m[2 * 4 + 1]) * s; 
        } 
        q[4] = m[0 * 4 + 3]; 
        q[5] = m[1 * 4 + 3]; 
        q[6] = m[2 * 4 + 3]; 
        q[7] = 0.0f; 
    }
}