/*****************************************************************************
 * $Id: gfx_math_matrix.hh,v 1.3 2006/08/01 23:14:02 devel Exp $
 * @file
 * @author	Jim E. Brooks  http://www.jimbrooks.org
 * @brief	Matrix math.
 * @remarks	This should be include by gfx_math.hh only.
 * @verbatim
 *------------------------------------------------------------------------------
 * A gfx matrix has 12 elements.  Xw,Yw,Zw,Ow were eliminated to minimize space.
 *
 * The amount and the ordering of matrix elements are abstracted.
 * The idea is for matrix code to independent of the exact layout of a matrix
 * (in case, eg, a specific gfxsys requires a 16-element row-major matrix).
 * 
 * Use the typedef Matrix.  It can be treated as a float array, while abstracting
 * the amount of elements.
 *     void Rotate( Matrix& m );    // expands to "float m[MATRIX_ELEMS]&"
 *                                  // C++ will accept it but discards the redundant "&"
 *
 * (rare) When computing the index of a matrix element using a row,col tuple,
 * don't hard-code the index computations as that would have implict dependencies
 * on a particular matrix layout.  Instead, use the MIX() macros.
 *     m[i*4+0]       // wrong
 *     m[ MIX(i,0) ]  // right (use macro that hides layout of matrix)
 * An example of a similar mistake is calling Distance( fpx[3], fpx[3]) as
 * Distance( pos, m[Ox] ) which wrongly assumes Ox,Oy,Oz are contiguous elements.
 *------------------------------------------------------------------------------
 * @endverbatim
 *//*
 * LEGAL:	COPYRIGHT (C) 2004 JIM E. BROOKS
 *          THIS SOURCE CODE IS RELEASED UNDER THE TERMS
 *          OF THE GNU GENERAL PUBLIC LICENSE (GPL).
 *****************************************************************************/

#ifndef GFX_MATH_MATRIX_HH
#define GFX_MATH_MATRIX_HH 1

// 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    12
//#define MATRIX_ELEMS    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 ); return m[i]; }
        const FP& operator[]( uint i ) const { ASSERT( i < MATRIX_ELEMS ); return m[i]; }
        FP m[MATRIX_ELEMS];
    };
    typedef MatrixTemplate<fp>  Matrix;
    typedef MatrixTemplate<fpx> MatrixExt;
#else
    typedef fp  Matrix[MATRIX_ELEMS];       // Matrix    : single-precision matrix
    typedef fpx MatrixExt[MATRIX_ELEMS];    // 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
#define MATRIX_ROW_MAJOR	0
#endif

/// Matrix element offsets.
#if MATRIX_ELEMS == 12
    #if MATRIX_ROW_MAJOR
    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
#elif MATRIX_ELEMS == 16
    #if MATRIX_ROW_MAJOR
    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
#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 == 12
    #if MATRIX_ROW_MAJOR
    #	define MIX( I, J )		( (I)*4 + (J) )
    #else
    #	define MIX( I, J )		( (J)*3 + (I) )
    #endif
    #if MATRIX_ROW_MAJOR
    #	define MIXR( I, J )		( (J)*4 + (I) )
    #else
    #	define MIXR( I, J )		( (I)*3 + (J) )
    #endif
#elif MATRIX_ELEMS == 16
    #if MATRIX_ROW_MAJOR
    #	define MIX( I, J )		( (I)*4 + (J) )
    #else
    #	define MIX( I, J )		( (J)*4 + (I) )
    #endif
    #if MATRIX_ROW_MAJOR
    #	define MIXR( I, J )		( (J)*4 + (I) )
    #else
    #	define MIXR( I, J )		( (I)*4 + (J) )
    #endif
#endif

// It served its purpose.
#undef MATRIX_ELEMS

/*****************************************************************************
 * 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
			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( 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( 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
	MULTIPLY_MATRIX_ELEM3( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM3( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM3( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM4( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
	MULTIPLY_MATRIX_ELEM3( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM3( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM3( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM4( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
	MULTIPLY_MATRIX_ELEM3( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM3( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM3( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM4( Oz, Ox,Zx, Oy,Zy, Oz,Zz /*, Ow,Oz */ , Oz ); 
#else
	MULTIPLY_MATRIX_ELEM3( Xx, Xx,Xx, Yx,Xy, Zx,Xz /*, Xw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM3( Yx, Xx,Yx, Yx,Yy, Zx,Yz /*, Xw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM3( Zx, Xx,Zx, Yx,Zy, Zx,Zz /*, Xw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM3( Xy, Xy,Xx, Yy,Xy, Zy,Xz /*, Yw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM3( Yy, Xy,Yx, Yy,Yy, Zy,Yz /*, Yw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM3( Zy, Xy,Zx, Yy,Zy, Zy,Zz /*, Yw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM3( Xz, Xz,Xx, Yz,Xy, Zz,Xz /*, Zw,Ox */      ); 
	MULTIPLY_MATRIX_ELEM3( Yz, Xz,Yx, Yz,Yy, Zz,Yz /*, Zw,Oy */      ); 
	MULTIPLY_MATRIX_ELEM3( Zz, Xz,Zx, Yz,Zy, Zz,Zz /*, Zw,Oz */      ); 
	MULTIPLY_MATRIX_ELEM4( Ox, Ox,Xx, Oy,Xy, Oz,Xz /*, Ow,Ox */ , Ox ); 
	MULTIPLY_MATRIX_ELEM4( Oy, Ox,Yx, Oy,Yy, Oz,Yz /*, Ow,Oy */ , Oy ); 
	MULTIPLY_MATRIX_ELEM4( 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];
}

/*****************************************************************************
 * 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.
 *****************************************************************************/
template<typename FP, typename MATRIX>
void
RotateMatrixFixed( uint axis, FP degree, MATRIX& m /*OUT*/ )
{
	const FP rad = deg2rad( degree );
	const FP s = sin( rad );
	const FP c = cos( rad );
	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
}

/*****************************************************************************
 * 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.
 *****************************************************************************/
template<typename FP, typename MATRIX>
void
RotateMatrixLocal( uint axis, FP degree, MATRIX& m /*OUT*/ )
{
	// Identity (1:1) matrix r.
	MATRIX r;
	IdentityMatrix( r );

	// Rotate matrix r.
	RotateMatrixFixed( axis, degree, 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];
}

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

#endif // GFX_MATH_MATRIX_HH
