/*=========================================================================
*
* matrix.c	- matrix functions
*
* Version			1.0
* Revision date
*
*=======================================================================*/
#include "matrix.h"

#include "util.h"
#include "vertex.h"


//--- Global statics --------------------------------------------------------

unsigned long MTH_g_a2048_fInvSquareRootTable[1024*2];


//--------------------------------------------------------------------



/****************************************************************************
*		Description:	get translation vector of a matrix 
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stTrs		: translation vector
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xGetTranslationMatrix ( MLT_tdstMatrix *p_stMatrix, MTH3D_tdstVector *p_stTrs )
{
  *p_stTrs = p_stMatrix->stTranslation ;
}

/****************************************************************************
*		Description:	get rotation matrix of a matrix
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stI		: first vector of the rotation matrix
*						p_stJ		: second vector of the rotation matrix
*						p_stK		: third vector of the rotation matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xGetRotationMatrix ( MLT_tdstMatrix *p_stMatrix ,
							  MTH3D_tdstVector *p_stI ,
							  MTH3D_tdstVector *p_stJ ,
							  MTH3D_tdstVector *p_stK )
{
  MLT_M_SetVertexXYZ ( p_stI ,
                       p_stMatrix->a9_xRotMatrix[0][0] ,
                       p_stMatrix->a9_xRotMatrix[1][0] ,
                       p_stMatrix->a9_xRotMatrix[2][0] ) ;

  MLT_M_SetVertexXYZ ( p_stJ ,
                       p_stMatrix->a9_xRotMatrix[0][1] ,
                       p_stMatrix->a9_xRotMatrix[1][1] ,
                       p_stMatrix->a9_xRotMatrix[2][1] ) ;

  MLT_M_SetVertexXYZ ( p_stK ,
                       p_stMatrix->a9_xRotMatrix[0][2] ,
                       p_stMatrix->a9_xRotMatrix[1][2] ,
                       p_stMatrix->a9_xRotMatrix[2][2] ) ;
}

/****************************************************************************
*		Description:	get scale matrix of a matrix
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stI		: first vector of the scale matrix
*						p_stJ		: second vector of the scale matrix
*						p_stK		: third vector of the scale matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xGetScaleMatrix ( MLT_tdstMatrix *p_stMatrix ,
						   MTH3D_tdstVector *p_stI ,
						   MTH3D_tdstVector *p_stJ ,
						   MTH3D_tdstVector *p_stK )
{
  MLT_M_SetVertexXYZ ( p_stI ,
                       p_stMatrix->a9_xScaleMatrix[0][0] ,
                       p_stMatrix->a9_xScaleMatrix[1][0] ,
                       p_stMatrix->a9_xScaleMatrix[2][0] ) ;

  MLT_M_SetVertexXYZ ( p_stJ ,
                       p_stMatrix->a9_xScaleMatrix[0][1] ,
                       p_stMatrix->a9_xScaleMatrix[1][1] ,
                       p_stMatrix->a9_xScaleMatrix[2][1] ) ;

  MLT_M_SetVertexXYZ ( p_stK ,
                       p_stMatrix->a9_xScaleMatrix[0][2] ,
                       p_stMatrix->a9_xScaleMatrix[1][2] ,
                       p_stMatrix->a9_xScaleMatrix[2][2] ) ;
}

/****************************************************************************
*		Description:	get transform matrix of a matrix
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stI		: first vector of the transform matrix
*						p_stJ		: second vector of the transform matrix
*						p_stK		: third vector of the transform matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xGetTransformMatrix ( MLT_tdstMatrix *p_stMatrix ,
							   MTH3D_tdstVector *p_stI ,
							   MTH3D_tdstVector *p_stJ ,
							   MTH3D_tdstVector *p_stK )
{
  MLT_M_SetVertexXYZ ( p_stI ,
                       p_stMatrix->a9_xTransformMatrix[0][0] ,
                       p_stMatrix->a9_xTransformMatrix[1][0] ,
                       p_stMatrix->a9_xTransformMatrix[2][0] ) ;

  MLT_M_SetVertexXYZ ( p_stJ ,
                       p_stMatrix->a9_xTransformMatrix[0][1] ,
                       p_stMatrix->a9_xTransformMatrix[1][1] ,
                       p_stMatrix->a9_xTransformMatrix[2][1] ) ;

  MLT_M_SetVertexXYZ ( p_stK ,
                       p_stMatrix->a9_xTransformMatrix[0][2] ,
                       p_stMatrix->a9_xTransformMatrix[1][2] ,
                       p_stMatrix->a9_xTransformMatrix[2][2] ) ;
}



/****************************************************************************
*		Description:	set translation vector of a matrix 
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stTrs		: translation vector
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xSetTranslationMatrix ( MLT_tdstMatrix *p_stMatrix, MTH3D_tdstVector *p_stTrs )
{
  p_stMatrix->stTranslation = *p_stTrs;
}

/****************************************************************************
*		Description:	set rotation matrix of a matrix
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stI		: first vector of the rotation matrix
*						p_stJ		: second vector of the rotation matrix
*						p_stK		: third vector of the rotation matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void  MLT_xSetRotationMatrix ( MLT_tdstMatrix *p_stMatrix ,
							   MTH3D_tdstVector *p_stI ,
							   MTH3D_tdstVector *p_stJ ,
							   MTH3D_tdstVector *p_stK )
{
  MLT_M_GetVertexXYZ ( p_stI ,
                       &p_stMatrix->a9_xRotMatrix[0][0] ,
                       &p_stMatrix->a9_xRotMatrix[1][0] ,
                       &p_stMatrix->a9_xRotMatrix[2][0] ) ;

  MLT_M_GetVertexXYZ ( p_stJ ,
                       &p_stMatrix->a9_xRotMatrix[0][1] ,
                       &p_stMatrix->a9_xRotMatrix[1][1] ,
                       &p_stMatrix->a9_xRotMatrix[2][1] ) ;

  MLT_M_GetVertexXYZ ( p_stK ,
                       &p_stMatrix->a9_xRotMatrix[0][2] ,
                       &p_stMatrix->a9_xRotMatrix[1][2] ,
                       &p_stMatrix->a9_xRotMatrix[2][2] ) ;

  MLT_xComputeTransformMatrix ( p_stMatrix );

  p_stMatrix -> lStateFlag |= MLT_C_lMatrixRotatFlag;
}

/****************************************************************************
*		Description:	get scale matrix of a matrix
*
*		Parameters:		p_stMatrix	: source matrix
*						p_stI		: first vector of the scale matrix
*						p_stJ		: second vector of the scale matrix
*						p_stK		: third vector of the scale matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xSetScaleMatrix ( MLT_tdstMatrix *p_stMatrix ,
						   MTH3D_tdstVector *p_stI ,
						   MTH3D_tdstVector *p_stJ ,
						   MTH3D_tdstVector *p_stK )
{
  MLT_M_GetVertexXYZ ( p_stI ,
                       &p_stMatrix->a9_xScaleMatrix[0][0] ,
                       &p_stMatrix->a9_xScaleMatrix[1][0] ,
                       &p_stMatrix->a9_xScaleMatrix[2][0] ) ;

  MLT_M_GetVertexXYZ ( p_stJ ,
                       &p_stMatrix->a9_xScaleMatrix[0][1] ,
                       &p_stMatrix->a9_xScaleMatrix[1][1] ,
                       &p_stMatrix->a9_xScaleMatrix[2][1] ) ;

  MLT_M_GetVertexXYZ ( p_stK ,
                       &p_stMatrix->a9_xScaleMatrix[0][2] ,
                       &p_stMatrix->a9_xScaleMatrix[1][2] ,
                       &p_stMatrix->a9_xScaleMatrix[2][2] ) ;

  MLT_xComputeTransformMatrix ( p_stMatrix );

  p_stMatrix -> lStateFlag |= MLT_C_lMatrixScaleFlag;
}

/****************************************************************************
*		Description:	set scale matrix to identity 
*
*		Parameters:		p_stMatrix	: result matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xResetScaleMatrix ( MLT_tdstMatrix *p_stMatrix )
{

  p_stMatrix -> a9_xScaleMatrix [0][0] = MLT_M_LongToValue ( 1 ) ;
  p_stMatrix -> a9_xScaleMatrix [1][0] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [2][0] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [0][1] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [1][1] = MLT_M_LongToValue ( 1 ) ;
  p_stMatrix -> a9_xScaleMatrix [2][1] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [0][2] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [1][2] = MLT_M_LongToValue ( 0 ) ;
  p_stMatrix -> a9_xScaleMatrix [2][2] = MLT_M_LongToValue ( 1 ) ;

  MLT_xComputeTransformMatrix ( p_stMatrix ) ;

  p_stMatrix -> lStateFlag &= 0xFFFFFFFF - MLT_C_lMatrixScaleFlag ;
}

/****************************************************************************
*		Description:	compute transform matrix from rotation and scale
*
*		Parameters:		p_stMatrix	: source matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xComputeTransformMatrix ( MLT_tdstMatrix *p_stMatrix )
{
  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) p_stMatrix -> a9_xTransformMatrix ,
                           (MTH_tdxReal *) p_stMatrix -> a9_xRotMatrix ,
                           (MTH_tdxReal *) p_stMatrix -> a9_xScaleMatrix);
}





/****************************************************************************
*		Description:	set matrix to identity
*
*		Parameters:		p_stMatrix	: result matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xSetIdentityMatrix (MLT_tdstMatrix *p_stMatrix)
{

  // set rotation matrix
  p_stMatrix->a9_xRotMatrix [0][0] = MLT_M_LongToValue (1);
  p_stMatrix->a9_xRotMatrix [1][0] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [2][0] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [0][1] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [1][1] = MLT_M_LongToValue (1);
  p_stMatrix->a9_xRotMatrix [2][1] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [0][2] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [1][2] = MLT_M_LongToValue (0);
  p_stMatrix->a9_xRotMatrix [2][2] = MLT_M_LongToValue (1);

  // set scale matrix
  memcpy (p_stMatrix->a9_xScaleMatrix, p_stMatrix->a9_xRotMatrix, sizeof(MTH_tdxReal) * 9);

  // set transform matrix
  memcpy (p_stMatrix->a9_xTransformMatrix, p_stMatrix->a9_xRotMatrix, sizeof(MTH_tdxReal) * 9);

  // set translation
  MLT_M_NullVertex (&p_stMatrix->stTranslation);

  // set flag
  p_stMatrix->lStateFlag = MLT_C_lMatrixIdentityFlag;
}


/****************************************************************************
*		Description:	normalize a matrix 
*
*		Parameters:		p_stMatrix	: result matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xNormalizeMatrix ( MLT_tdstMatrix *p_stMatrix )
{
  MTH3D_tdstVector stI , stJ , stK ;

  MLT_xGetRotationMatrix ( p_stMatrix , &stI , &stJ , &stK ) ;

  MLT_M_CrossProduct ( &stJ , &stK , &stI ) ;
  MLT_M_CrossProduct ( &stK , &stI , &stJ ) ;

  MLT_M_Normalize ( &stI ) ;
  MLT_M_Normalize ( &stJ ) ;
  MLT_M_Normalize ( &stK ) ;

  MLT_xSetRotationMatrix ( p_stMatrix , &stI , &stJ , &stK ) ;
}



/****************************************************************************
*		Description:	invert a matrix 
*
*		Parameters:		p_stDest	: result matrix
*						p_stSource	: source matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xInvertMatrix ( MLT_tdstMatrix *p_stDest , MLT_tdstMatrix *p_stSource )
{
  MTH3D_tdstMatrix	stSrc, stDst;
  MLT_tdstMatrix	stInterm, stInvScale ;
  MTH3D_tdstVector	stVertex;
  
  MLT_xTranspA9Matrix ( stInterm.a9_xRotMatrix,
                        p_stSource -> a9_xRotMatrix );

  memcpy( &stSrc, p_stSource->a9_xScaleMatrix, sizeof(MTH3D_tdstMatrix));
  MTH3D_M_vInverMatrix(&stDst, &stSrc);
  memcpy( &stInvScale, &stDst, sizeof(MTH3D_tdstMatrix));

  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) stInterm.a9_xScaleMatrix ,
                           (MTH_tdxReal *) p_stSource->a9_xRotMatrix ,
                           (MTH_tdxReal *) &stInvScale ) ;

  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) stInterm.a9_xScaleMatrix ,
                           (MTH_tdxReal *) stInterm.a9_xScaleMatrix ,
                           (MTH_tdxReal *) stInterm.a9_xRotMatrix ) ;

  MLT_xComputeTransformMatrix (&stInterm ) ;

  MLT_M_NullVertex ( &stVertex ) ;
  MLT_M_SubVertex ( &stVertex , &p_stSource->stTranslation ) ;

  MLT_M_SetVertexXYZ ( &stInterm.stTranslation ,
                       MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [0][0] ,
                                               stVertex.xX ) ,
                                   MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [0][1] ,
                                                           stVertex.xY ) ,
                                               MLT_M_Mul ( stInterm.a9_xTransformMatrix [0][2] ,
                                                           stVertex.xZ ) ) ) ,

                       MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [1][0] ,
                                               stVertex.xX ) ,
                                   MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [1][1] ,
                                                           stVertex.xY ) ,
                                               MLT_M_Mul ( stInterm.a9_xTransformMatrix [1][2] ,
                                                           stVertex.xZ ) ) ) ,

                       MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [2][0] ,
                                               stVertex.xX ) ,
                                   MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xTransformMatrix [2][1] ,
                                                           stVertex.xY ) ,
                                               MLT_M_Mul ( stInterm.a9_xTransformMatrix [2][2] ,
                                                           stVertex.xZ ) ) ) ) ;


  *p_stDest = stInterm ;
}

/****************************************************************************
*		Description:	invert an isometric matrix 
*
*		Parameters:		p_stDest	: result matrix
*						p_stSource	: source matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xInvertIsoMatrix ( MLT_tdstMatrix *p_stDest , MLT_tdstMatrix *p_stSource )
{
  MLT_tdstMatrix stInterm ;
  MTH3D_tdstVector stVertex;
  
  MLT_xTranspA9Matrix    ( stInterm.a9_xRotMatrix,
                                                   p_stSource -> a9_xRotMatrix );

  MLT_M_NullVertex ( &stVertex ) ;
  MLT_M_SubVertex ( &stVertex , &p_stSource->stTranslation ) ;

  stInterm.stTranslation = stVertex ;

  MLT_M_SetVertexXYZ (
  
    &stInterm.stTranslation ,
    
    MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xRotMatrix [0][0] ,
                            stVertex.xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [0][1] ,
                              stVertex.xY ) ,
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [0][2] ,
                              stVertex.xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xRotMatrix [1][0] ,
                            stVertex.xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [1][1] ,
                              stVertex.xY ) ,
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [1][2] ,
                              stVertex.xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( stInterm.a9_xRotMatrix [2][0] ,
                            stVertex.xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [2][1] ,
                              stVertex.xY ) ,
                  MLT_M_Mul ( stInterm.a9_xRotMatrix [2][2] ,
                              stVertex.xZ ) ) ) ) ;

  memcpy ( stInterm.a9_xScaleMatrix  ,p_stSource -> a9_xScaleMatrix  , sizeof(MTH_tdxReal) * 9 ) ;

  MLT_xComputeTransformMatrix (&stInterm ) ;

  *p_stDest = stInterm ;
}


/****************************************************************************
*		Description:	multiply a vertex by a matrix 
*
*		Parameters:		p_stDest	: result vertex
*						p_stMatrix	: source matrix
*						p_stSource	: source vertex
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xMulMatrixVertex ( MTH3D_tdstVector	*p_stDest,
                            MLT_tdstMatrix		*p_stMatrix,
                            MTH3D_tdstVector	*p_stSource )
{
  MTH3D_tdstVector stInterm;
  
  MLT_M_SetVertexXYZ (
  
    &stInterm ,
    
    MLT_M_Add ( MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [0][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [0][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [0][2] ,
                              p_stSource -> xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [1][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [1][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [1][2] ,
                              p_stSource -> xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [2][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [2][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( p_stMatrix -> a9_xTransformMatrix [2][2] ,
                              p_stSource -> xZ ) ) ) ) ;

  MLT_M_AddVertex ( &stInterm , &p_stMatrix -> stTranslation ) ;//*/


                     
  *(p_stDest) = stInterm ;
}


/****************************************************************************
*		Description:	multiply a matrix by a matrix 
*
*		Parameters:		p_stDest	: result matrix
*						p_stA		: first source matrix
*						p_stB		: second source matrix
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xMulMatrixMatrix ( MLT_tdstMatrix *p_stDest ,
							MLT_tdstMatrix *p_stA ,
							MLT_tdstMatrix *p_stB )
{
  MLT_tdstMatrix stIntern;

  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) stIntern .  a9_xRotMatrix ,
                           (MTH_tdxReal *) p_stA -> a9_xRotMatrix ,
                           (MTH_tdxReal *) p_stB -> a9_xRotMatrix ) ;

  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) stIntern . a9_xTransformMatrix ,
                           (MTH_tdxReal *) p_stA -> a9_xTransformMatrix ,
                           (MTH_tdxReal *) p_stB -> a9_xTransformMatrix ) ;

  MLT_xTranspA9Matrix    ( stIntern . a9_xScaleMatrix ,
                              stIntern . a9_xRotMatrix );

  MLT_xMulA9MatrixMatrix ( (MTH_tdxReal *) stIntern . a9_xScaleMatrix ,
                           (MTH_tdxReal *) stIntern . a9_xScaleMatrix ,
                           (MTH_tdxReal *) stIntern . a9_xTransformMatrix ) ;

  MLT_xMulMatrixVertex (  &stIntern . stTranslation ,
                             p_stA , &p_stB -> stTranslation ) ;//*/

  *p_stDest = stIntern ;

  p_stDest -> lStateFlag = p_stA -> lStateFlag | p_stB -> lStateFlag;
}




/****************************************************************************
*		Description:	transpose a 3*3 Table(real matrix) 
*
*		Parameters:		p_stDest	: result 3*3 Table
*						p_xA		: source 3*3 Table
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xTranspA9Matrix ( MTH_tdxReal p_xDest[3][3] ,
						   MTH_tdxReal p_xA[3][3] )
{
  MTH_tdxReal A9_xInterm[3][3] ;
  long lLine , lColumn ;

  for ( lLine = 0 ; lLine < 3 ; lLine ++ )
    for ( lColumn = 0 ; lColumn < 3 ; lColumn ++ )
    {
          A9_xInterm[lColumn][lLine] = p_xA[lLine][ lColumn ];
    }

  memcpy ( p_xDest , A9_xInterm , sizeof(MTH_tdxReal) * 9 ) ;
}


/****************************************************************************
*		Description:	multiply a 3*3 Table(real matrix) with a vertex 
*
*		Parameters:		p_stDest	: result vertex
*						a9_xTable	: 3*3 Table
*						p_stSource	: source vertex
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xMulA9MatrixVertex ( MTH3D_tdstVector *p_stDest ,
                               MTH_tdxReal a9_xTable[3][3] ,
                               MTH3D_tdstVector *p_stSource )
{
  MTH3D_tdstVector stInterm ;

  MLT_M_SetVertexXYZ (
  
    &stInterm  ,
    
    MLT_M_Add ( MLT_M_Mul ( a9_xTable [0][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( a9_xTable[0][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( a9_xTable[0][2] ,
                              p_stSource -> xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( a9_xTable[1][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( a9_xTable[1][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( a9_xTable[1][2] ,
                              p_stSource -> xZ ) ) ) ,

    MLT_M_Add ( MLT_M_Mul ( a9_xTable[2][0] ,
                            p_stSource -> xX ) ,
                MLT_M_Add (
                  MLT_M_Mul ( a9_xTable[2][1] ,
                              p_stSource -> xY ) ,
                  MLT_M_Mul ( a9_xTable[2][2] ,
                              p_stSource -> xZ ) ) ) ) ;

  *(p_stDest) = stInterm ;
}


/****************************************************************************
*		Description:	multiply a 3*3 Table(real matrix) with another one 
*
*		Parameters:		p_stDest	: result 3*3 Table
*						p_xA		: first source 3*3 Table
*						p_xB		: second source 3*3 Table
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
void MLT_xMulA9MatrixMatrix ( MTH_tdxReal *p_xDest ,
							  MTH_tdxReal *p_xA ,
							  MTH_tdxReal *p_xB )
{
  MTH_tdxReal A9_xInterm[3][3] ;
  long lLine , lColumn ;

  for ( lLine = 0 ; lLine < 3 ; lLine ++ )
    for ( lColumn = 0 ; lColumn < 3 ; lColumn ++ )
    {
      A9_xInterm[lLine][lColumn]
        = MLT_M_Add (
             MLT_M_Add (
               MLT_M_Mul ( *(p_xA + 3*lLine + 0 ) , *(p_xB + 3*0 + lColumn) ) ,
               MLT_M_Mul ( *(p_xA + 3*lLine + 1 ) , *(p_xB + 3*1 + lColumn) ) ) ,
             MLT_M_Mul ( *(p_xA + 3*lLine + 2 ) , *(p_xB + 3*2 + lColumn) ) 
                                )  ;
    }

  memcpy ( p_xDest , A9_xInterm , sizeof(MTH_tdxReal) * 9 ) ;
}





/****************************************************************************
*		Description:	check if a matrix is scaled 
*
*		Parameters:		p_stMatrix	: matrix to test
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
long MLT_lIsNotScaledMatrix ( MLT_tdstMatrix *p_stMatrix )
{
  return (p_stMatrix -> lStateFlag & MLT_C_lMatrixScaleFlag);
}

/****************************************************************************
*		Description:	check if a matrix is the identity 
*
*		Parameters:		p_stMatrix	: matrix to test
*---------------------------------------------------------------------------
*		Revision date:                                      Author:         
*****************************************************************************/
long MLT_lIsIdentityMatrix ( MLT_tdstMatrix *p_stMatrix )
{
  return (p_stMatrix -> lStateFlag == MLT_C_lMatrixIdentityFlag );
}
