/*---------------------------------------------------------------------------*
  Project: Matrix vector Library
  File:    quat.c

  Copyright 1998-2001 Nintendo.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Log: quat.c,v $
  Revision 1.3  2007/01/11 00:45:26  aka
  Removed win32.h.

  Revision 1.2  2006/02/20 04:25:42  mitu
  Changed include path from dolphin/ to revolution/.

  Revision 1.1.1.1  2005/05/12 02:15:49  yasuh-to
  Ported from dolphin source tree.


    9    2002/10/15 16:01 Hirose
    Added one more const to C_QUATMakeClosest( ).
    Fixed implementation also.

    8    2002/08/20 16:50 Hirose
    Workaround for divided-by-zero exceptions.

    7    2002/04/11 13:11 Hirose
    const type specifier support. (by Hiratsu@IRD)

    6     2002/01/04 6:03p Hirose
    Fixed wrong implementation of QUATMtx.

    5    2001/09/19 4:00p Hirose
    Workaround for the WIN32 build.

    4     2001/09/12 10:40a Hirose
    Removed unnecessary possibilities of zero division exception.

    3    2001/08/27 3:55p Hirose
    Added some Paired-single assembler functions.

    2     2001/08/20 10:52a Hirose
    Fixed a function name bug. Used temporary variable in C_QUATMultiply.

    1     2001/08/03 1:20a Hirose
    Initial check in.

  $NoKeywords: $

 *---------------------------------------------------------------------------*/

#include <math.h>
#include <revolution/mtx.h>
#include "mtxAssert.h"

#ifdef WIN32
#define atan2f              (f32)atan2
#endif

#define QUAT_EPSILON        0.00001F

/*---------------------------------------------------------------------------*
  Name:         QUATAdd

  Description:  Returns the sum of two quaternions.

  Arguments:    p: 	first quaternion
                q: 	second quaternion
                r: 	resultant quaternion p+q
                
  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATAdd( const Quaternion *p, const Quaternion *q, Quaternion *r )
{
    ASSERTMSG( ( p != 0 ), QUAT_ADD_1 );
    ASSERTMSG( ( q != 0 ), QUAT_ADD_2 );
    ASSERTMSG( ( r != 0 ), QUAT_ADD_3 );

    r->x = p->x + q->x;
    r->y = p->y + q->y;
    r->z = p->z + q->z;
    r->w = p->w + q->w;
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATAdd
(
    const register Quaternion * p,
    const register Quaternion * q,
          register Quaternion * r
)
{
    register f32    pxy, qxy, rxy, pzw, qzw, rzw;
    
    asm
    {
        psq_l     pxy,  0(p), 0, 0
        psq_l     qxy,  0(q), 0, 0
            ps_add  rxy, pxy, qxy
        psq_st    rxy,  0(r), 0, 0

        psq_l     pzw,  8(p), 0, 0
        psq_l     qzw,  8(q), 0, 0
            ps_add  rzw, pzw, qzw
        psq_st    rzw,  8(r), 0, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATSubtract

  Description:  Returns the difference of two quaternions p-q.

  Arguments:    p: 	left quaternion
                q: 	right quaternion
                r: 	resultant quaternion difference p-q

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATSubtract( const Quaternion *p, const Quaternion *q, Quaternion *r )
{
    ASSERTMSG( ( p != 0 ), QUAT_SUBTRACT_1 );
    ASSERTMSG( ( q != 0 ), QUAT_SUBTRACT_2 );
    ASSERTMSG( ( r != 0 ), QUAT_SUBTRACT_3 );

    r->x = p->x - q->x;
    r->y = p->y - q->y;
    r->z = p->z - q->z;
    r->w = p->w - q->w;
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATSubtract
(
    const register Quaternion * p,
    const register Quaternion * q,
          register Quaternion * r
)
{
    register f32    pxy, qxy, rxy, pzw, qzw, rzw;
    
    asm
    {
        psq_l     pxy,  0(p), 0, 0
        psq_l     qxy,  0(q), 0, 0
            ps_sub  rxy, pxy, qxy
        psq_st    rxy,  0(r), 0, 0

        psq_l     pzw,  8(p), 0, 0
        psq_l     qzw,  8(q), 0, 0
            ps_sub  rzw, pzw, qzw
        psq_st    rzw,  8(r), 0, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATMultiply

  Description:  Returns the product of two quaternions. The order of 
                multiplication is important. (p*q != q*p)

  Arguments:    p: 	left quaternion
                q: 	right quaternion
                pq: 	resultant quaternion product p*q

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATMultiply( const Quaternion *p, const Quaternion *q, Quaternion *pq )
{
    Quaternion *r;
    Quaternion  pqTmp;

    ASSERTMSG( ( p  != 0 ), QUAT_MULTIPLY_1 );
    ASSERTMSG( ( q  != 0 ), QUAT_MULTIPLY_2 );
    ASSERTMSG( ( pq != 0 ), QUAT_MULTIPLY_3 );

    if ( p == pq || q == pq )
    {
        r = &pqTmp;
    }
    else
    {
        r = pq;
    }

    r->w = p->w*q->w - p->x*q->x - p->y*q->y - p->z*q->z;
    r->x = p->w*q->x + p->x*q->w + p->y*q->z - p->z*q->y;
    r->y = p->w*q->y + p->y*q->w + p->z*q->x - p->x*q->z;
    r->z = p->w*q->z + p->z*q->w + p->x*q->y - p->y*q->x;
    
    if ( r == &pqTmp )
    {
        *pq = pqTmp;
    }
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATMultiply
(
    const register Quaternion *p,
    const register Quaternion *q,
          register Quaternion *pq
)
{
    register f32    pxy, pzw, qxy, qzw;
    register f32    pnxy, pnzw, pnxny, pnznw;
    register f32    rxy, rzw, sxy, szw;

    asm
    {
        // [px][py] : Load
        psq_l       pxy, 0(p), 0, 0
        // [pz][pw] : Load
        psq_l       pzw, 8(p), 0, 0
        
        // [qx][qy] : Load
        psq_l       qxy, 0(q), 0, 0
        // [-px][-py]
        ps_neg      pnxny, pxy
        // [qz][qw] : Load
        psq_l       qzw, 8(q), 0, 0
        // [-pz][-pw]
        ps_neg      pnznw, pzw
        
        // [-px][py]
        ps_merge01  pnxy, pnxny, pxy
        
        // [pz*qx][pw*qx]
        ps_muls0    rxy, pzw, qxy
        // [-px*qx][-py*qx]
        ps_muls0    rzw, pnxny, qxy
        
        // [-pz][pw]
        ps_merge01  pnzw, pnznw, pzw

        // [-px*qy][py*qy]
        ps_muls1    szw, pnxy, qxy
        // [pz*qx-px*qz][pw*qx+py*qz]
        ps_madds0   rxy, pnxy, qzw, rxy
        // [-pz*qy][pw*qy]
        ps_muls1    sxy, pnzw, qxy
        // [-px*qx-pz*qz][-py*qx+pw*qz]
        ps_madds0   rzw, pnzw, qzw, rzw
        // [-px*qy-pz*qw][py*qy-pw*qw]
        ps_madds1   szw, pnznw, qzw, szw
        // [pw*qx+py*qz][pz*qx-px*qz]
        ps_merge10  rxy, rxy, rxy
        // [-pz*qy+px*qw][pw*qy+py*qw]
        ps_madds1   sxy, pxy, qzw, sxy
        // [-py*qx+pw*qz][-px*qx-pz*qz]
        ps_merge10  rzw, rzw, rzw
        
        // [pw*qx+py*qz-pz*qy+px*qw][pz*qx-px*qz+pw*qy+py*qw] : [pqx][pqy]
        ps_add      rxy, rxy, sxy
        // [pqx][pqy] : Store
        psq_st      rxy, 0(pq), 0, 0
        // [-py*qx+pw*qz+px*qy+pz*qw][-px*qx-pz*qz-py*qy+pw*qw] : [pqz][pqw]
        ps_sub      rzw, rzw, szw
        // [pqz][pqw] : Store
        psq_st      rzw, 8(pq), 0, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATScale

  Description:  Scales a quaternion.

  Arguments:    q: 	quaternion 
                r: 	resultant scaled quaternion
                scale: 	float to scale by

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATScale( const Quaternion *q, Quaternion *r, f32 scale )
{
    ASSERTMSG( ( q  != 0 ), QUAT_SCALE_1 );
    ASSERTMSG( ( r  != 0 ), QUAT_SCALE_2 );

    r->x = q->x * scale;
    r->y = q->y * scale;
    r->z = q->z * scale;
    r->w = q->w * scale;
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATScale
(
    const register Quaternion *q,
          register Quaternion *r,
          register f32         scale
)
{
    register f32    rxy, rzw;

    asm
    {
        psq_l       rxy, 0(q), 0, 0
        psq_l       rzw, 8(q), 0, 0
        ps_muls0    rxy, rxy, scale
        psq_st      rxy, 0(r), 0, 0
        ps_muls0    rzw, rzw, scale
        psq_st      rzw, 8(r), 0, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATDotProduct

  Description:  Returns the dot product of the two quaternions.

  Arguments:    p: 	first quaternion
                q: 	second quaternion

  Returns:      Dot product
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
f32 C_QUATDotProduct( const Quaternion *p, const Quaternion *q ) 
{
    ASSERTMSG( ( p  != 0 ), QUAT_DOTPRODUCT_1 );
    ASSERTMSG( ( q  != 0 ), QUAT_DOTPRODUCT_2 );

    return (q->x*p->x + q->y*p->y + q->z*p->z + q->w*p->w);
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
f32 PSQUATDotProduct( const register Quaternion *p, const register Quaternion *q ) 
{
    register f32    pxy, pzw, qxy, qzw, dp;

    asm
    {
        psq_l       pxy, 0(p), 0, 0
        psq_l       qxy, 0(q), 0, 0
        ps_mul      dp, pxy, qxy
        
        psq_l       pzw, 8(p), 0, 0
        psq_l       qzw, 8(q), 0, 0
        ps_madd     dp, pzw, qzw, dp
        
        ps_sum0     dp, dp, dp, dp
    }

    return dp;
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATNormalize

  Description:  Normalizes a quaternion

  Arguments:    src: 	the source quaternion
                unit: 	resulting unit quaternion

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATNormalize( const Quaternion *src, Quaternion *unit )
{
    f32 mag;

    ASSERTMSG( ( src  != 0 ), QUAT_NORMALIZE_1 );
    ASSERTMSG( ( unit != 0 ), QUAT_NORMALIZE_2 );

    mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);

    if ( mag >= QUAT_EPSILON )
    {
        mag = 1.0F / sqrtf(mag);
    
        unit->x = src->x * mag;
        unit->y = src->y * mag;
        unit->z = src->z * mag;
        unit->w = src->w * mag;
    }
    else
    {
        unit->x = unit->y = unit->z = unit->w = 0.0F;    
    }
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATNormalize( const register Quaternion *src, register Quaternion *unit )
{
    register f32    sxy, szw;
    register f32    mag, rsqmag, diff, c_zero;
    register f32    nwork0, nwork1;
    register f32    epsilon = QUAT_EPSILON;
    register f32    c_half  = 0.5F;
    register f32    c_three = 3.0F;

    asm
    {
        psq_l       sxy, 0(src), 0, 0

        // mag = [x*x][y*y]
        ps_mul      mag, sxy, sxy
        
        psq_l       szw, 8(src), 0, 0

        // c_zero = [0.0F]
        ps_sub      c_zero, epsilon, epsilon
        // mag = [x*x+z*z][y*y+w*w]
        ps_madd     mag, szw, szw, mag
        // mag = [x*x+y*y+z*z+w*w][N/A]
        ps_sum0     mag, mag, mag, mag
        
        // rsqmag = 1.0F / sqrtf(mag) : estimation
        frsqrte     rsqmag, mag
        // diff = mag - epsilon
        ps_sub      diff, mag, epsilon
        // Newton-Rapson refinement (x1) : E' = (E/2)(3 - X * E * E)
        fmul        nwork0, rsqmag, rsqmag
        fmul        nwork1, rsqmag, c_half
        fnmsub      nwork0, nwork0, mag, c_three
        fmul        rsqmag, nwork0, nwork1
        
        // rsqmag = ( mag >= epsilon ) ? rsqmag : 0
        ps_sel      rsqmag, diff, rsqmag, c_zero
        // sxy = [x*rsqmag][y*rsqmag]
        ps_muls0    sxy, sxy, rsqmag
        // szw = [z*rsqmag][w*rsqmag]
        ps_muls0    szw, szw, rsqmag

        psq_st      sxy, 0(unit), 0, 0
        psq_st      szw, 8(unit), 0, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATInverse

  Description:  Returns the inverse of the quaternion.

  Arguments:    src: 	the source quaternion
                inv: 	resulting inverse quaternion

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATInverse( const Quaternion *src, Quaternion *inv )
{
    f32 mag, norminv;

    ASSERTMSG( ( src != 0 ), QUAT_INVERSE_1 );
    ASSERTMSG( ( inv != 0 ), QUAT_INVERSE_2 );

    mag = ( src->x*src->x + src->y*src->y + src->z*src->z + src->w*src->w );

    if ( mag == 0.0f )
    {
        mag = 1.0f;
    }

    // [Inverse] = [Conjugate] / [Magnitude]
    norminv = 1.0f / mag;
    inv->x = -src->x * norminv;
    inv->y = -src->y * norminv;
    inv->z = -src->z * norminv;
    inv->w =  src->w * norminv;
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATInverse( const register Quaternion *src, register Quaternion *inv )
{
    register f32    sxy, szw, izz, iww;
    register f32    mag, nmag, norminv, nninv, nwork0, c_two, c_zero;
    register f32    c_one = 1.0F;

    asm
    {
        // load xy
        psq_l       sxy, 0(src), 0, 0

        // mag = [x*x][y*y]
        ps_mul      mag, sxy, sxy
        // c_zero = [0.0F]
        ps_sub      c_zero, c_one, c_one

        // load zw
        psq_l       szw, 8(src), 0, 0
        
        // mag = [x*x+z*z][y*y+w*w]
        ps_madd     mag, szw, szw, mag
        // c_two = [2.0F]
        ps_add      c_two, c_one, c_one
        // mag = [x*x+y*y+z*z+w*w][N/A]
        ps_sum0     mag, mag, mag, mag
        
        // zero check
        fcmpu       cr0, mag, c_zero
        beq-        __zero
   
        // norminv = 1.0F / mag
        fres        norminv, mag
        // nmag = -mag
        ps_neg      nmag, mag
        // Newton-Rapson refinment (x1) : E' = 2E-X*E*E
        ps_nmsub    nwork0, mag, norminv, c_two
        ps_mul      norminv, norminv, nwork0
        b           __mulnorm

    __zero:
        fmr         norminv, c_one
        
    __mulnorm:
        // nninv = [ -norminv ]
        ps_neg      nninv, norminv
        
        // iww = [ w*norminv ][ N/A ]
        ps_muls1    iww, norminv, szw
        // sxy = [ -x*norminv ][ -y*norminv ]
        ps_muls0    sxy, sxy, nninv

        // store w
        psq_st      iww, 12(inv), 1, 0
        
        // izz = [ -z*norminv ][ N/A ]
        ps_muls0    izz, szw, nninv
        
        // store xy
        psq_st      sxy, 0(inv), 0, 0
        // store z
        psq_st      izz, 8(inv), 1, 0
    }
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATDivide

  Description:  Returns the ratio of two quaternions.  Creates a result 
                r = p/q such that q*r=p (order of multiplication is important).

  Arguments:    p: 	left quaternion
                q: 	right quaternion
                r: 	resultant ratio quaterion p/q

  Returns:      none
 *---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*
    C version
 *---------------------------------------------------------------------------*/
void C_QUATDivide( const Quaternion *p, const Quaternion *q, Quaternion *r )
{
    Quaternion qtmp;

    ASSERTMSG( ( p != 0 ), QUAT_DIVIDE_1 );
    ASSERTMSG( ( q != 0 ), QUAT_DIVIDE_2 );
    ASSERTMSG( ( r != 0 ), QUAT_DIVIDE_3 );

    C_QUATInverse(q, &qtmp);
    C_QUATMultiply(&qtmp, p, r);
}

/*---------------------------------------------------------------------------*
    Paired-Single assembler version
 *---------------------------------------------------------------------------*
            Note that this performs NO error checking.
 *---------------------------------------------------------------------------*/
#ifdef GEKKO
void PSQUATDivide( const Quaternion *p, const Quaternion *q, Quaternion *r )
{
    Quaternion qtmp;

    PSQUATInverse(q, &qtmp);
    PSQUATMultiply(&qtmp, p, r);
}
#endif

/*---------------------------------------------------------------------------*
  Name:         QUATExp

  Description:  Exponentiate quaternion (where q.w == 0).

  Arguments:    q: 	pure quaternion
                r: 	resultant exponentiate quaternion (an unit quaternion)

  Returns:      none
 *---------------------------------------------------------------------------*/
void C_QUATExp( const Quaternion *q, Quaternion *r ) 
{
    f32 theta, scale;

    ASSERTMSG( ( q != 0 ), QUAT_EXP_1 );
    ASSERTMSG( ( r != 0 ), QUAT_EXP_2 );

    // pure quaternion check
    ASSERTMSG( ( q->w == 0.0F ), QUAT_EXP_3 );

    theta = sqrtf( q->x*q->x + q->y*q->y + q->z*q->z );
    scale = 1.0F;

    if ( theta > QUAT_EPSILON )
        scale = (f32)sinf(theta)/theta;
    
    r->x = scale * q->x;
    r->y = scale * q->y;
    r->z = scale * q->z;
    r->w = (f32)cosf(theta);
}


/*---------------------------------------------------------------------------*
  Name:         QUATLogN

  Description:  Returns the natural logarithm of a UNIT quaternion

  Arguments:    q: 	unit quaternion
                r: 	resultant logarithm quaternion (an pure quaternion)

  Returns:      none
 *---------------------------------------------------------------------------*/
void C_QUATLogN( const Quaternion *q, Quaternion *r )
{
    f32 theta,scale;

    ASSERTMSG( ( q != 0 ), QUAT_LOGN_1 );
    ASSERTMSG( ( r != 0 ), QUAT_LOGN_2 );

    scale = q->x*q->x + q->y*q->y + q->z*q->z;

    // unit quaternion check
#ifdef _DEBUG
    {
        f32 mag;
        mag = scale + q->z*q->z;
        if ( mag < 1.0F - QUAT_EPSILON || mag > 1.0F + QUAT_EPSILON )
        {
            ASSERT( QUAT_LOGN_3 );
        }
    }
#endif

    scale = sqrtf(scale);
    theta = atan2f( scale, q->w );

    if ( scale > 0.0F )
        scale = theta/scale;

    r->x = scale*q->x;
    r->y = scale*q->y;
    r->z = scale*q->z;
    r->w = 0.0F;

}


/*---------------------------------------------------------------------------*
  Name:         QUATMakeClosest

  Description:  Modify q so it is on the same side of the hypersphere as qto

  Arguments:    q: 	quaternion
                qto: 	quaternion to be close to
                r: 	resultant modified quaternion

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATMakeClosest( const Quaternion *q, const Quaternion *qto, Quaternion *r )
{
    f32 dot;

    ASSERTMSG( ( q   != 0 ), QUAT_MAKECLOSEST_1 );
    ASSERTMSG( ( qto != 0 ), QUAT_MAKECLOSEST_2 );
    ASSERTMSG( ( r   != 0 ), QUAT_MAKECLOSEST_3 );
    
    dot =  q->x*qto->x + q->y*qto->y + q->z*qto->z + q->w*qto->w;
    
    if ( dot < 0.0f ) 
    {
        r->x = -q->x;
        r->y = -q->y;
        r->z = -q->z;
        r->w = -q->w;
    }
    else
    {
        *r = *q;
    }
}


/*---------------------------------------------------------------------------*
  Name:         QUATRotAxisRad

  Description:  Returns rotation quaternion about arbitrary axis.

  Arguments:    r: 	resultant rotation quaternion
                axis: 	rotation axis
                rad: 	rotation angle in radians

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATRotAxisRad( Quaternion *r, const Vec *axis, f32 rad )
{
    f32 half, sh, ch;
    Vec nAxis;

    ASSERTMSG( ( r    != 0 ), QUAT_ROTAXISRAD_1 );
    ASSERTMSG( ( axis != 0 ), QUAT_ROTAXISRAD_2 );

    VECNormalize(axis, &nAxis);
    
    half = rad * 0.50F;
    sh   = sinf(half);
    ch   = cosf(half);
    
    r->x = sh * nAxis.x;
    r->y = sh * nAxis.y;
    r->z = sh * nAxis.z;
    r->w = ch;
}


/*---------------------------------------------------------------------------*
  Name:         QUATMtx

  Description:  Converts a matrix to a unit quaternion.

  Arguments:    r: 	result quaternion
                m: 	the matrix

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATMtx( Quaternion *r, const Mtx m )
{
    f32 tr,s;
    s32 i,j,k;
    s32 nxt[3] = {1,2,0};
    f32 q[3];

    ASSERTMSG( ( r != 0 ), QUAT_MTX_1 );
    ASSERTMSG( ( m != 0 ), QUAT_MTX_2 );

    tr = m[0][0] + m[1][1] + m[2][2];
    if( tr > 0.0f )
    {
        s = (f32)sqrtf(tr + 1.0f);
        r->w = s * 0.5f;
        s = 0.5f / s;
        r->x = (m[2][1] - m[1][2]) * s;
        r->y = (m[0][2] - m[2][0]) * s;
        r->z = (m[1][0] - m[0][1]) * s;
    }
    else 
    {
        i = 0;
        if (m[1][1] > m[0][0]) i = 1;
        if (m[2][2] > m[i][i]) i = 2;
        j = nxt[i];
        k = nxt[j];
        s = (f32)sqrtf( (m[i][i] - (m[j][j] + m[k][k])) + 1.0f );
        q[i] = s * 0.5f;
        
        if (s!=0.0f)
            s = 0.5f / s;
        
        r->w = (m[k][j] - m[j][k]) * s;
        q[j] = (m[i][j] + m[j][i]) * s;
        q[k] = (m[i][k] + m[k][i]) * s;

        r->x = q[0];
        r->y = q[1];
        r->z = q[2];
    }
}


/*---------------------------------------------------------------------------*
  Name:         QUATLerp

  Description:  Linear interpolation of two quaternions.

  Arguments:    p: 	first quaternion
                q: 	second quaternion
                r: 	resultant interpolated quaternion
                t: 	interpolation parameter

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATLerp( const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t )
{
    ASSERTMSG( ( p != 0 ), QUAT_LERP_1 );
    ASSERTMSG( ( q != 0 ), QUAT_LERP_2 );
    ASSERTMSG( ( r != 0 ), QUAT_LERP_3 );

    r->x = t * ( q->x - p->x ) + p->x;
    r->y = t * ( q->y - p->y ) + p->y;
    r->z = t * ( q->z - p->z ) + p->z;
    r->w = t * ( q->w - p->w ) + p->w;
}


/*---------------------------------------------------------------------------*
  Name:         QUATSlerp

  Description:  Spherical linear interpolation of two quaternions.

  Arguments:    p: 	first quaternion
                q: 	second quaternion
                r: 	resultant interpolated quaternion
                t: 	interpolation parameter

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATSlerp( const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t )
{
    f32 theta, sin_th, cos_th, tp, tq;
    
    ASSERTMSG( ( p != 0 ), QUAT_SLERP_1 );
    ASSERTMSG( ( q != 0 ), QUAT_SLERP_2 );
    ASSERTMSG( ( r != 0 ), QUAT_SLERP_3 );

    cos_th = p->x * q->x + p->y * q->y + p->z * q->z + p->w * q->w;
    tq     = 1.0F;
    
    if ( cos_th < 0.0F )
    {
        cos_th = -cos_th;
        tq     = -tq;
    }

    if ( cos_th <= 1.0F - QUAT_EPSILON )
    {
        theta  = acosf(cos_th);
        sin_th = sinf(theta);
        tp     = sinf((1.0F - t) * theta) / sin_th;
        tq    *= sinf( t * theta ) / sin_th;
    }
    else
    {
        // cos(theta) is close to 1.0F -> linear
        tp = 1.0F - t;
        tq = tq * t;
    }

    r->x = tp * p->x + tq * q->x;
    r->y = tp * p->y + tq * q->y;
    r->z = tp * p->z + tq * q->z;
    r->w = tp * p->w + tq * q->w;

}


/*---------------------------------------------------------------------------*
  Name:         QUATSquad

  Description:  Spherical cubic quadrangle interpolation of two quaternions
                with derrived inner-quadrangle quaternions.

                This will be used with the function QUATCompA().

  Arguments:    p: 	first quaternion
                a: 	derrived inner-quadrangle quaternion
                b: 	derrived inner-quadrangle quaternion
                q: 	second quaternion
                r: 	resultant interpolated quaternion
                t: 	interpolation parameter

  Returns:      NONE
 *---------------------------------------------------------------------------*/
void C_QUATSquad( const Quaternion *p, const Quaternion *a, const Quaternion *b,
                  const Quaternion *q, Quaternion *r, f32 t )
{
    Quaternion pq, ab;
    f32 t2;

    ASSERTMSG( ( p != 0 ), QUAT_SQUAD_1 );
    ASSERTMSG( ( a != 0 ), QUAT_SQUAD_2 );
    ASSERTMSG( ( b != 0 ), QUAT_SQUAD_3 );
    ASSERTMSG( ( q != 0 ), QUAT_SQUAD_4 );
    ASSERTMSG( ( r != 0 ), QUAT_SQUAD_5 );

    t2 = 2 * t * ( 1.0F - t );
    C_QUATSlerp(p, q, &pq, t);
    C_QUATSlerp(a, b, &ab, t);
    C_QUATSlerp(&pq, &ab, r, t2);
}


/*---------------------------------------------------------------------------*
  Name:         QUATCompA

  Description:  Compute a, the term used in Boehm-type interpolation
                a[n] = q[n]* qexp(-(1/4)*( ln(qinv(q[n])*q[n+1]) +
                                           ln( qinv(q[n])*q[n-1] )))

  Arguments:    qprev: 	previous quaternion
                q: 	current quaternion
                qnext: 	next quaternion
                a: 	result quaternion A

  Returns:      none
/*---------------------------------------------------------------------------*/
void C_QUATCompA( const Quaternion *qprev, const Quaternion *q, const Quaternion *qnext, Quaternion *a ) 
{
    Quaternion qm, qp, lqm, lqp, qpqm, exq;

    ASSERTMSG( ( qprev != 0 ), QUAT_COMPA_1 );
    ASSERTMSG( ( q     != 0 ), QUAT_COMPA_2 );
    ASSERTMSG( ( qnext != 0 ), QUAT_COMPA_3 );
    ASSERTMSG( ( a     != 0 ), QUAT_COMPA_4 );

    C_QUATDivide(qprev, q, &qm);
    C_QUATLogN(&qm, &lqm);
    C_QUATDivide(qnext, q, &qp);
    C_QUATLogN(&qp, &lqp);
    
    C_QUATAdd(&lqp, &lqm, &qpqm);
    C_QUATScale(&qpqm, &qpqm, -0.25F);
    
    C_QUATExp(&qpqm, &exq);
    C_QUATMultiply(q, &exq, a);
}


/*===========================================================================*/