/*****************************************************************************
 * @file
 * @author  Jim E. Brooks  http://www.jimbrooks.org
 * @brief   Proven math/matrix code written in 2004..2006 (gfx_2006).
 *
 * This old code was hacked in order to compile.
 *
 *//*
 * LEGAL:   COPYRIGHT (C) 2004 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE VERSION 2 (GPL 2).
 *****************************************************************************/

#ifndef MATH_MATRIX_2006_HH
#define MATH_MATRIX_2006_HH

namespace gfx_2006 {

using math::Vector2;
using math::Vector3;
using math::LocalVertex;
using math::WorldVertex;
using math::EyeVertex;
using math::NormalVertex;

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////  M A C R O S  ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
///////////////////////////////  T R I G   /////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

// fpx is used in trig functions since they're used in matrix functions
// which need to maintain precision.

// 180' = PI
// 360' = 2 * PI
const fpx PI  = 3.14159265358979323846;
const fpx PI2 = 6.28318530717958647692;   // pi*2

#if SPEED
typedef fp Radian;  // aren't distinct C++ types (useless for overloading functions)
typedef fp Degree;
#else
typedef fpx Radian;
typedef fpx Degree;
#endif

const Radian RADIAN_180 = math::PI;   // 180' in radians (half circle)
const Radian RADIAN_360 = math::PI2;  // 360' in radians (full circle)

// Compare degrees/radians while ignoring sign.
#if 0  // [2007]
#define IF_DEG_GT( DEG1, DEG2 ) (ABS(DEG1) > ABS(DEG2))
#define IF_DEG_LT( DEG1, DEG2 ) (ABS(DEG1) < ABS(DEG2))
#define IF_RAD_GT( RAD1, RAD2 ) (ABS(RAD1) > ABS(RAD2))
#define IF_RAD_LT( RAD1, RAD2 ) (ABS(RAD1) < ABS(RAD2))
#endif

/*****************************************************************************
 * sincos().
 *****************************************************************************/
template<typename FP>
void
SinCos( Radian rad, FP& si, FP& co )
{
    // This x87 asm code in this C++ template returning args by reference
    // seems perilous but was tested by gfx/tests/math.cc.
#if defined(__GNUC__) && defined(CPU_X86)
    asm("fsincos" : "=t" (co), "=u" (si) : "0" (rad));
#elif defined(__GNUC__) && _GLIBCXX_HAVE_SINCOS
    sincos( rad, &si, &co );  // GNU extension
#else
    si = sin( rad );
    co = cos( rad );
#endif
}

/*****************************************************************************
 * Truncate a degree.
 *****************************************************************************/
INLINE Degree
TruncateDeg( Degree deg )
{
    if ( EX(deg < 360.0) )
        return deg;
    else
        return deg - 360.0;
}

/*****************************************************************************
 * Convert a (360) degree into a radian.
 * 360' = 2 * PI
 *****************************************************************************/
INTERN const fpx degradRatio = (1.0 / 360.0) * (math::PI2);
#define DEG2RAD_2006( DEG ) ((DEG) * (degradRatio))
INLINE Radian
deg2rad( Degree deg )
{
    return DEG2RAD_2006( deg );
    //return deg / 360.0 * (2.0 * math::PI); // x87 FDIV is slower than FMUL
}

/*****************************************************************************
 * Convert a radian into a (360) degree.
 * 360' = 2 * PI
 *****************************************************************************/
INTERN const fpx raddegRatio = (1.0 / math::PI2) * 360.0;
#define RAD2DEG_2006( RAD ) ((RAD) * (raddegRatio))
INLINE Degree
rad2deg( Radian rad )
{
    return RAD2DEG_2006( rad );
    //return rad / (2.0 * math::PI) * 360.0;    // x87 FDIV is slower than FMUL
}

/*****************************************************************************
 * 360' sin().
 *****************************************************************************/
INLINE fpx
sin_deg( Degree deg )
{
    return sin( deg2rad( deg ) );
}

/*****************************************************************************
 * 360' cos().
 *****************************************************************************/
INLINE fpx
cos_deg( Degree deg )
{
    return cos( deg2rad( deg ) );
}

////////////////////////////////////////////////////////////////////////////////
/////////////////////////////  M A T R I X   ///////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

// A gfx matrix has 12 elements to minimize space (W elements aren't needed).
// A 16 element matrix remains a compile option.
#define MATRIX_ELEMS_2006    12
//#define MATRIX_ELEMS_2006    16

/// Matrix type.
#if DEBUG == 2
    // For debugging, this class does bounds-checking.
    template<typename FP>
    struct MatrixTemplate
    {
        FP&       operator[]( uint i )       { ASSERT( i < MATRIX_ELEMS_2006 ); return m[i]; }
        const FP& operator[]( uint i ) const { ASSERT( i < MATRIX_ELEMS_2006 ); return m[i]; }
        FP m[MATRIX_ELEMS_2006];
    };
    typedef MatrixTemplate<fp>  Matrix;
    typedef MatrixTemplate<fpx> MatrixExt;
#else
    typedef fp  Matrix[MATRIX_ELEMS_2006];       // Matrix    : single-precision matrix
    typedef fpx MatrixExt[MATRIX_ELEMS_2006];    // MatrixExt : extended-precision matrix
#endif

/// Row-major or column-major.
/// If row-major, to go to row i, multiply i by amount of columns.
#ifndef MATRIX_ROW_MAJOR_2006
#define MATRIX_ROW_MAJOR_2006   0
#endif

/// Matrix element offsets.
#if MATRIX_ELEMS_2006 == 12
    #if MATRIX_ROW_MAJOR_2006
    enum { Xx, Xy, Xz, Ox,
           Yx, Yy, Yz, Oy,
           Zx, Zy, Zz, Oz };
    #else
    enum { Xx, Yx, Zx,
           Xy, Yy, Zy,
           Xz, Yz, Zz,
           Ox, Oy, Oz };
    #endif // MATRIX_ROW_MAJOR_2006
#elif MATRIX_ELEMS_2006 == 16
    #if MATRIX_ROW_MAJOR_2006
    enum { Xx, Xy, Xz, Ox,
           Yx, Yy, Yz, Oy,
           Zx, Zy, Zz, Oz,
           Xw, Yw, Zw, Ow };    // W elements are strongly deprecated
    #else
    enum { Xx, Yx, Zx, Xw,
           Xy, Yy, Zy, Yw,
           Xz, Yz, Zz, Zw,
           Ox, Oy, Oz, Ow };
    #endif // MATRIX_ROW_MAJOR_2006
#endif

/// Macros to access matrix elements (depends on row/column major order).
/// These macros should be used instead of writing hardcoded index computations.
#if MATRIX_ELEMS_2006 == 12
    #if MATRIX_ROW_MAJOR_2006
    #   define MIX( I, J )      ( (I)*4 + (J) )
    #else
    #   define MIX( I, J )      ( (J)*3 + (I) )
    #endif
    #if MATRIX_ROW_MAJOR_2006
    #   define MIXR( I, J )     ( (J)*4 + (I) )
    #else
    #   define MIXR( I, J )     ( (I)*3 + (J) )
    #endif
#elif MATRIX_ELEMS_2006 == 16
    #if MATRIX_ROW_MAJOR_2006
    #   define MIX( I, J )      ( (I)*4 + (J) )
    #else
    #   define MIX( I, J )      ( (J)*4 + (I) )
    #endif
    #if MATRIX_ROW_MAJOR_2006
    #   define MIXR( I, J )     ( (J)*4 + (I) )
    #else
    #   define MIXR( I, J )     ( (I)*4 + (J) )
    #endif
#endif

// It served its purpose.
#undef MATRIX_ELEMS_2006

/*****************************************************************************
 * Load matrix with identity mapping.
 *****************************************************************************/
template<typename MATRIX>
void
IdentityMatrix( MATRIX& m /*OUT*/ )
{
    m[Xx] = 1.0; m[Xy] = 0.0; m[Xz] = 0.0;
    m[Yx] = 0.0; m[Yy] = 1.0; m[Yz] = 0.0;
    m[Zx] = 0.0; m[Zy] = 0.0; m[Zz] = 1.0;
    m[Ox] = 0.0; m[Oy] = 0.0; m[Oz] = 0.0;
}

/*****************************************************************************
 * Copy matrix.
 *****************************************************************************/
template<typename MATRIX_DEST, typename MATRIX_SRC>
void
CopyMatrix( MATRIX_DEST& dest, const MATRIX_SRC& src )
{
    dest[Xx] = src[Xx]; dest[Xy] = src[Xy]; dest[Xz] = src[Xz];
    dest[Yx] = src[Yx]; dest[Yy] = src[Yy]; dest[Yz] = src[Yz];
    dest[Zx] = src[Zx]; dest[Zy] = src[Zy]; dest[Zz] = src[Zz];
    dest[Ox] = src[Ox]; dest[Oy] = src[Oy]; dest[Oz] = src[Oz];
}

/*****************************************************************************
 * Multiply two matrixs.
 *
 * @verbatim
 *
 * Matrix multiplication can be used to combine matrix transformations.
 *
 * For example, the two transformations of a local vertex to an eye vertex:
 * vl * wm[] = vw      // local --> world
 * vw * em[] = ve      // world --> eye
 *
 * Is mathematically equivalent to:
 * vl * wm[] * em[] = ve
 *
 * Combining the two matrixs into one using multiplication:
 * wm[] * em[] = wem[]
 *
 * Allows a faster single vertex transformation:
 * vl * wem[] = ve
 *
 * @endverbatim
 *****************************************************************************/
#if 0
// Reference function:
template<typename FPP,typename FPM,typename FPN>
void
MultiplyMatrix( FPP p[16] /*OUT*/, const FPM m[16], const FPN n[16] )
{
    for ( uint i = 0; i < 4; ++i )
    for ( uint j = 0; j < 4; ++j )
    {
        FPP e = 0.0;
        for ( uint k = 0; k < 4; ++k )
        {
#if MATRIX_ROW_MAJOR_2006
            e += m[k*4+j] * n[i*4+k];
#else
            e += m[i*4+k] * n[k*4+j];
#endif
        }
        p[i*4+j] = e;
    }
}
#endif // 0

// Actual optimized function:
template<typename MATRIX_P, typename MATRIX_M, typename MATRIX_N>
void
MultiplyMatrix( MATRIX_P& p /*OUT*/, const MATRIX_M& m, const MATRIX_N& n )
{
    // Optimizations:
    // Calculating Xw,Yw,Zw,Ow (0,0,0,1) is bypassed as they never change.
    // Xw,Yw,Zw are always 0 so multiplying one factor is bypassed.
    // Ow is always 1 so Oxyz coefficients are added directly without multiplying it by 1.
    // Inefficiencies:
    // Some factors are re-multiplied.

#define MULTIPLY_MATRIX_ELEM3_2006( i, a0,a1, b0,b1, c0,c1 )            \
    p[i] =   m[a0] * n[a1]                                      \
           + m[b0] * n[b1]                                      \
           + m[c0] * n[c1];

#define MULTIPLY_MATRIX_ELEM4_2006( i, a0,a1, b0,b1, c0,c1, oi )        \
    p[i] =   m[a0] * n[a1]                                      \
           + m[b0] * n[b1]                                      \
           + m[c0] * n[c1]                                      \
           +         n[oi];

#if MATRIX_ROW_MAJOR_2006
    MULTIPLY_MATRIX_ELEM3_2006( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM4_2006( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM4_2006( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM4_2006( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz ); 
#else
    MULTIPLY_MATRIX_ELEM3_2006( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
    MULTIPLY_MATRIX_ELEM3_2006( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
    MULTIPLY_MATRIX_ELEM4_2006( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
    MULTIPLY_MATRIX_ELEM4_2006( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
    MULTIPLY_MATRIX_ELEM4_2006( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz ); 
#endif
}

/*****************************************************************************
 * Transpose a matrix.
 * Reverses the mapping between two coordinate systems.
 *****************************************************************************/
template<typename MATRIX_DEST, typename MATRIX_SRC>
void
TransposeMatrix( MATRIX_DEST& dest, const MATRIX_SRC& src )
{
    // Transpose rotation sub-matrix.
    // X = X'
    dest[Xx] = src[Xx];
    dest[Xy] = src[Yx];
    dest[Xz] = src[Zx];
    // Y = Y'
    dest[Yx] = src[Xy];
    dest[Yy] = src[Yy];
    dest[Yz] = src[Zy];
    // Z = Z'
    dest[Zx] = src[Xz];
    dest[Zy] = src[Yz];
    dest[Zz] = src[Zz];

    // Negate origin sub-matrix.
    dest[Ox] = -src[Ox];
    dest[Oy] = -src[Oy];
    dest[Oz] = -src[Oz];
}

/*****************************************************************************
 * Rotate coordinates.
 *****************************************************************************/
template<typename FP, typename MATRIX>
FP
RotX( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Xx] + y * m[Xy] + z * m[Xz];
}
template<typename FP, typename MATRIX>
FP
RotY( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Yx] + y * m[Yy] + z * m[Yz];
}
template<typename FP, typename MATRIX>
FP
RotZ( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Zx] + y * m[Zy] + z * m[Zz];
}

/*****************************************************************************
 * Rotate/translate coordinates.
 *****************************************************************************/
template<typename FP, typename MATRIX>
FP
RotTransX( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
}
template<typename FP, typename MATRIX>
FP
RotTransY( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
}
template<typename FP, typename MATRIX>
FP
RotTransZ( FP x, FP y, FP z, const MATRIX& m )
{
    return x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
}

/*****************************************************************************
 * Rotate/translate vertexs.
 *****************************************************************************/
template<typename FP, typename MATRIX>
void
RotTrans( FP& x2, FP& y2, FP& z2, const FP x, const FP y, const FP z, const MATRIX& m )
{
    x2 = x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
    y2 = x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
    z2 = x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
}
template<typename FP, typename MATRIX>
void
RotTrans( WorldVertex& v2, const FP x, const FP y, const FP z, const MATRIX& m )
{
    v2.x = x * m[Xx] + y * m[Xy] + z * m[Xz] + m[Ox];
    v2.y = x * m[Yx] + y * m[Yy] + z * m[Yz] + m[Oy];
    v2.z = x * m[Zx] + y * m[Zy] + z * m[Zz] + m[Oz];
}
template<typename MATRIX>
void
RotTrans( WorldVertex& v2, const LocalVertex& v, const MATRIX& m )
{
    v2.x = v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz] + m[Ox];
    v2.y = v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz] + m[Oy];
    v2.z = v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] + m[Oz];
}
template<typename MATRIX>
void
RotTrans( EyeVertex& v2, const LocalVertex& v, const MATRIX& m )
{
    v2.x = v.x * m[Xx] + v.y * m[Xy] + v.z * m[Xz] + m[Ox];
    v2.y = v.x * m[Yx] + v.y * m[Yy] + v.z * m[Yz] + m[Oy];
    v2.z = v.x * m[Zx] + v.y * m[Zy] + v.z * m[Zz] + m[Oz];
}

/*****************************************************************************
 * Transform a 3D vertex thru one coordinate system.
 * More specialized variants are at eng_class_math.hh.
 *****************************************************************************/
template<typename MATRIX>
void
TransformVertex( EyeVertex& v2, const WorldVertex& v, const MATRIX& m )
{
    fp x = v.x + m[Ox];
    fp y = v.y + m[Oy];
    fp z = v.z + m[Oz];
    v2.x = x * m[Xx] + y * m[Xy] + z * m[Xz];
    v2.y = x * m[Yx] + y * m[Yy] + z * m[Yz];
    v2.z = x * m[Zx] + y * m[Zy] + z * m[Zz];
}

/*****************************************************************************
 * Calling this variant requires specifying template arg types:
 * TransformVertex<EyeVertex,WorldVertex,Matrix>();
 *****************************************************************************/
template<typename VERTEX_OUT, typename VERTEX_IN, typename MATRIX>
VERTEX_OUT
TransformVertex( const VERTEX_IN& v, const MATRIX& m )
{
    fp x = v.x + m[Ox];
    fp y = v.y + m[Oy];
    fp z = v.z + m[Oz];  // uses C++ return-value optimization
    return VERTEX_OUT( x * m[Xx] + y * m[Xy] + z * m[Xz],   // = x
                       x * m[Yx] + y * m[Yy] + z * m[Yz],   // = y
                       x * m[Zx] + y * m[Zy] + z * m[Zz] ); // = z
}

/*****************************************************************************
 * Translate a matrix by row.
 * For translating the eye/viewpoint.
 *****************************************************************************/
template<typename FPM, typename MATRIX>
void
TranslateMatrixFixed( uint axis, FPM inc, MATRIX& m /*OUT*/ )
{
    m[Ox] += inc * m[ MIX(axis,0) ];
    m[Oy] += inc * m[ MIX(axis,1) ];
    m[Oz] += inc * m[ MIX(axis,2) ];
}

/*****************************************************************************
 * Translate a matrix relative to itself (local translation).
 * For translating an object.
 *****************************************************************************/
template<typename FPM, typename MATRIX>
void
TranslateMatrixLocal( uint axis, FPM inc, MATRIX& m /*OUT*/ )
{
    m[Ox] += inc * m[ MIXR(axis,0) ];
    m[Oy] += inc * m[ MIXR(axis,1) ];
    m[Oz] += inc * m[ MIXR(axis,2) ];
}

/*****************************************************************************
 * Rotate a matrix around a fixed axis.
 * This function is suited to rotating the viewpoint/eye.
 * The word "fixed" should be clear by looking at the math.
 * Eg, the value of the X coord won't be changed by a rotation around the X axis.
 *****************************************************************************/
// Radian/Degree are just typedefs (not distinct types).
template<typename MATRIX>
void
RotateMatrixFixedRadian( uint axis, Radian rad, MATRIX& m /*OUT*/ )
{
#if SPEED
    fp s, c; SinCos( rad, s, c );
#else
    const fpx s = sin( rad );
    const fpx c = cos( rad );
#endif
    MATRIX n;
    CopyMatrix( n, m ); // dest,src
    switch ( axis )
    {
        // X : Pitch
        case AXIS_X:
        n[Yx] = m[Yx]*c - m[Zx]*s;      // Y = Ycos - Zsin
        n[Yy] = m[Yy]*c - m[Zy]*s;
        n[Yz] = m[Yz]*c - m[Zz]*s;

        n[Zx] = m[Yx]*s + m[Zx]*c;      // Z = Ysin + Zcos
        n[Zy] = m[Yy]*s + m[Zy]*c;
        n[Zz] = m[Yz]*s + m[Zz]*c;
        break;

        // Y : Yaw
        case AXIS_Y:
        n[Xx] = m[Xx]*c - m[Zx]*s;      // X = Xcos - Zsin
        n[Xy] = m[Xy]*c - m[Zy]*s;
        n[Xz] = m[Xz]*c - m[Zz]*s;

        n[Zx] = m[Xx]*s + m[Zx]*c;      // Z = Xsin + Zcos
        n[Zy] = m[Xy]*s + m[Zy]*c;
        n[Zz] = m[Xz]*s + m[Zz]*c;
        break;

        // Z : Roll
        case AXIS_Z:
        n[Xx] = m[Xx]*c - m[Yx]*s;      // X = Xcos - Ysin
        n[Xy] = m[Xy]*c - m[Yy]*s;
        n[Xz] = m[Xz]*c - m[Yz]*s;

        n[Yx] = m[Xx]*s + m[Yx]*c;      // Y = Xsin + Ycos
        n[Yy] = m[Xy]*s + m[Yy]*c;
        n[Yz] = m[Xz]*s + m[Yz]*c;
        break;

        default: ASSERT(0); return;
    }

    CopyMatrix( m, n ); // dest,src
}
template<typename MATRIX>
void
RotateMatrixFixedDegree( uint axis, Degree deg, MATRIX& m /*OUT*/ )
{
    RotateMatrixFixedRadian( axis, deg2rad(deg), m );
}

/*****************************************************************************
 * Rotate a local coordinate system around its own axis.
 * This function is suited to rotating an independent object.
 * The rotation is relative to local coodinate system (not the fixed/eye system).
 * The trick is to load an identity-mapped matrix and rotate it, then transform
 * it thru the given matrix (which defines the local coordinate system)
 * as though it was the coords (1.0, 1.0, 1.0).  These coords of course define
 * the X,Y,Z axises.  The transformation is effectively a local rotation.
 *****************************************************************************/
// Radian/Degree are just typedefs (not distinct types).
template<typename MATRIX>
void
RotateMatrixLocalRadian( uint axis, Radian rad, MATRIX& m /*OUT*/ )
{
    // Identity (1:1) matrix r.
    MATRIX r;
    IdentityMatrix( r );

    // Rotate matrix r.
    RotateMatrixFixedRadian( axis, rad, r );

    // Transform r thru m.
    // Ie, thru matrix m, pass r as if it were the rotated coords (1.0, 1.0, 1.0).
    MATRIX t;
    t[Xx] = RotTransX( m[Xx], m[Xy], m[Xz], r );
    t[Xy] = RotTransY( m[Xx], m[Xy], m[Xz], r );
    t[Xz] = RotTransZ( m[Xx], m[Xy], m[Xz], r );

    t[Yx] = RotTransX( m[Yx], m[Yy], m[Yz], r );
    t[Yy] = RotTransY( m[Yx], m[Yy], m[Yz], r );
    t[Yz] = RotTransZ( m[Yx], m[Yy], m[Yz], r );

    t[Zx] = RotTransX( m[Zx], m[Zy], m[Zz], r );
    t[Zy] = RotTransY( m[Zx], m[Zy], m[Zz], r );
    t[Zz] = RotTransZ( m[Zx], m[Zy], m[Zz], r );

    // m = r excluding origin elements.
    m[Xx] = t[Xx];
    m[Xy] = t[Xy];
    m[Xz] = t[Xz];

    m[Yx] = t[Yx];
    m[Yy] = t[Yy];
    m[Yz] = t[Yz];

    m[Zx] = t[Zx];
    m[Zy] = t[Zy];
    m[Zz] = t[Zz];
}
template<typename MATRIX>
void
RotateMatrixLocalDegree( uint axis, Degree deg, MATRIX& m /*OUT*/ )
{
    RotateMatrixLocalRadian( axis, deg2rad(deg), m );
}

/*****************************************************************************
 * Print matrix.
 *****************************************************************************/
template<typename MATRIX>
void
PrintMatrix( const MATRIX& m )
{
    cout.setf( ios::fixed, ios::floatfield );
    cout.precision( 6 );
    cout << "Xxyzw:  " << m[Xx] << "  " << m[Xy] << "  " << m[Xz] << std::endl
                << "Yxyzw:  " << m[Yx] << "  " << m[Yy] << "  " << m[Yz] << std::endl
                << "Zxyzw:  " << m[Zx] << "  " << m[Zy] << "  " << m[Zz] << std::endl
                << "Oxyzw:  " << m[Ox] << "  " << m[Oy] << "  " << m[Oz] << std::endl;
}
template<typename MATRIX>
void
PrintMatrixOrigin( const MATRIX& m )
{
    cout.setf( ios::fixed, ios::floatfield );
    cout.precision( 6 );
    cout << m[Ox] << ' ' << m[Oy] << ' ' << m[Oz] << std::endl;
}

////////////////////////////////////////////////////////////////////////////////
//////////////////////////////  V E C T O R   //////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * Distance between vector and implied zero origin.
 *****************************************************************************/
INLINE fp
Distance( const Vector2& v )
{
    return sqrtf( v.x * v.x
                + v.y * v.y );
}
INLINE fp
Distance( const Vector3& v )
{
    return sqrtf( v.x * v.x
                + v.y * v.y
                + v.z * v.z );
}

/*****************************************************************************
 * Distance between two vectors -- DistanceSquared() is faster than this.
 *****************************************************************************/
template<typename VECTOR>  // 2D/3D
fp
Distance( const VECTOR& v1, const VECTOR& v2 )
{
    VECTOR delta = v2 - v1;
    return gfx_2006::Distance( delta );
}

/*****************************************************************************
 * Calculate the distance^2 (squared) between two vectors.
 * DISTANCE RETURNED IS SQUARED (distance^2).
 * SQUARE ROOT OF DISTANCE IS SKIPPED FOR SPEED.
 * sqrt() and the underlying FPU instruction is very slow.
 *****************************************************************************/
INLINE fpx
DistanceSquared( const Vector3& v1, const Vector3& v2 )
{
    fpx xd = v2.x - v1.x;  // extra precision
    fpx yd = v2.y - v1.y;
    fpx zd = v2.z - v1.z;
    return xd*xd + yd*yd + zd*zd;
}

// Distance^2 between two matrixs.
// Don't pass different kinds of matrixs (eg Eye origin is negative but Dyna is positive).
template<typename MATRIX1, typename MATRIX2>
fpx
DistanceSquaredMatrix( const MATRIX1& m1, const MATRIX2& m2 )
{
    Vector3 v1( m1[Ox], m1[Oy], m1[Oz] );
    Vector3 v2( m2[Ox], m2[Oy], m2[Oz] );
    return gfx_2006::DistanceSquared( v1, v2 );
}

/*****************************************************************************
 * @return A normalized vector (unit-length 1.0) of the same vector.
 * Not to be confused with calculating the normal vector of two vectors (CrossProduct).
 *****************************************************************************/
INLINE Vector3
Normalize( const Vector3& v )
{
    fp dist = gfx_2006::Distance( v );
    return Vector3( v.x / dist,
                    v.y / dist,
                    v.z / dist );
}

/*****************************************************************************
 * Calculate a cross product of two vectors which yields a normal vector
 * that is perpendicular to the plane containing the two source vectors.
 * Note: The cross product won't be unit length.
 * c = a x b
 *****************************************************************************/
INLINE void
CrossProduct( NormalVertex& n, /*OUT*/
              const Vector3& v1, /* origin of normal vector */
              const Vector3& v2,
              const Vector3& v3 )
{
    fp a, b, c;
    fp d, e, f;

    a = v2.x - v1.x;
    b = v2.y - v1.y;
    c = v2.z - v1.z;

    d = v3.x - v1.x;
    e = v3.y - v1.y;
    f = v3.z - v1.z;

    n.x = b*f - c*e;
    n.y = c*d - a*f;
    n.z = a*e - b*d;
}

/*****************************************************************************
 * Calculate the dot product of two vectors which yields a scalar.
 * If the angle between the two vectors is between 0 and 90 degrees,
 * the dot product will be positive, else negative.
 *****************************************************************************/
INLINE fp
DotProduct( const Vector2& v1, const Vector2& v2 )
{
    return   v1.x * v2.x
           + v1.y * v2.y;
}
INLINE fp
DotProduct( const Vector3& v1, const Vector3& v2 )
{
    return   v1.x * v2.x
           + v1.y * v2.y
           + v1.z * v2.z;
}

/*****************************************************************************
 * Calculate angle (in radians) between two vectors sharing the same origin.
 *****************************************************************************/
template<typename VECTOR>  // 2D/3D template
Radian
Angle( const VECTOR& u, const VECTOR& v )
{
    // Dot product formula indirectly provides the angle.
    // angle = arccos( DotProduct(u,v) / Distance(u) * Distance(v) )
    fp uv = gfx_2006::Distance(u) * gfx_2006::Distance(v);
    if ( ABS(uv) < 0.1 ) return 0.0;  // prevent NaN and div by 0
    fp theta = DotProduct(u,v) / uv;
    theta = Clamp( theta, -1.0, 1.0 ); // acos() won't tolerate 1.00000001
DEBUG_CODE( errno = 0; )
    fp rad = acos( theta );
ASSERT(errno == 0);  // if acos() failed
    return rad;
}

/*****************************************************************************
 * Interpolate between two vectors.
 * @param   v1, v2
 * @param   frac
 *          Fraction in range {0,..,1.0}.
 *          If fraction is 0.0, v1 is returned.
 *          If fraction is 1.0, v2 is returned.
 *          If fraction is 0.5, midpoint is returned.
 * @return Interpolated 3D point.
 *****************************************************************************/
INLINE Vector3
Interpolate( const Vector3& v1, const Vector3& v2, const fp fraction )
{
ASSERT( fraction >= 0.0 && fraction <= 1.0 );
    return Vector3( v1 + ((v2-v1) * fraction) );
}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////  M I S C   M A T H   //////////////////////////////
////////////////////////////////////////////////////////////////////////////////

/*****************************************************************************
 * x^2
 *****************************************************************************/
template<typename T>
T
SQUARE( T x )
{
    return x * x;
}

/*****************************************************************************
 * Average.
 *****************************************************************************/
template<typename T>
T
AVG( T x, T y )
{
    return (x + y) / 2;
}

/*****************************************************************************
 * Force a number to be within a range.
 *****************************************************************************/
template<typename T>
T
Range( T val, T lo, T hi )
{
    if ( val < lo ) return lo;
    if ( val > hi ) return hi;
    return val;
}

/*****************************************************************************
 * Map a number from one range into another.
 *****************************************************************************/
INLINE fp
Remap( fp value, fp rangeOld, fp rangeNew )
{
    return value/rangeOld*rangeNew;
}

/*****************************************************************************
 * Truncate a float at an interval.
 * Eg, Truncate(24,10) returns 20.
 *****************************************************************************/
INLINE fp
Truncate( fp val, fp interval )
{
    float integral;                     // not fp as address is passed to modff()
    modff( val/interval, &integral );
    return integral*interval;
}

/*****************************************************************************
 * Normalize a 3D point (unit 1 distance) then return one normalized coordinate.
 * The return value typically is passed to asin().
 *****************************************************************************/
INLINE fp
NormalizeCoord( uint axis, const Vector3& v )
{
    fp dist = gfx_2006::Distance( v );
#if 0
    return v[axis] / dist;
#else  // 2007
    if ( axis == AXIS_X )
        return v.x / dist;
    else if ( axis == AXIS_Y )
        return v.y / dist;
    else if ( axis == AXIS_Z )
        return v.z / dist;
    else { ASSERT(0); }
#endif
}

} // namespace gfx_2006

#endif // MATH_MATRIX_2006_HH
