/*======================================================================*/
/* 3DLib								*/
/*									*/
/* Matrix functions							*/
/*									*/
/* AUTHOR:	Gabor Nagy						*/
/* DATE:	1996-Dec-18 23:57:41					*/
/*									*/
/* 3DLib(TM) Copyright (C) 1995 by Gabor Nagy. All rights reserved.	*/
/*======================================================================*/
#include <math.h>
#include <float.h>
#include <stdarg.h>		// For varargs
#include <stdio.h>

#include <EStrings.h>

#include <E3D/E3D.h>

#include <E3D/Math.h>

#include <E3D/Matrix.h>


E3dMatrix E3d_IdentityMatrix=
{1.0, 0.0, 0.0, 0.0,
 0.0, 1.0, 0.0, 0.0,
 0.0, 0.0, 1.0, 0.0,
 0.0, 0.0, 0.0, 1.0};


int		E3d_MatrixPtr, E3d_MatrixScalePtr;

E3dCoordinate	E3d_ActScaleX, E3d_ActScaleY, E3d_ActScaleZ;
E3d3DPosition	E3d_ScaleStack[E3dMAX_HIERARCHY_DEPTH];
E3dMatrix	E3d_MatrixStack[E3dMAX_HIERARCHY_DEPTH];


int	E3d_NXT[3] = { E3dY, E3dZ, E3dX };


//================================================================================
// Print out a Matrix's contents
//
// Argument
//  E3dMatrix      LMatrix         The E3dMatrix to dump out
//
// Description
//  Prints the contents of the given matrix to the standard output.
//================================================================================
void E3d_PrintMatrix(E3dMatrix LMatrix)
{
 printf("%f, %f, %f, %f\n", LMatrix[M00], LMatrix[M01], LMatrix[M02], LMatrix[M03]);
 printf("%f, %f, %f, %f\n", LMatrix[M10], LMatrix[M11], LMatrix[M12], LMatrix[M13]);
 printf("%f, %f, %f, %f\n", LMatrix[M20], LMatrix[M21], LMatrix[M22], LMatrix[M23]);
 printf("%f, %f, %f, %f\n", LMatrix[M30], LMatrix[M31], LMatrix[M32], LMatrix[M33]);
 fflush(stdout);
}


//========================================================
// Convert a matrix's rotation part to a quaternion
// based on the source in the book 'Advanced Animation
// and rendering techniques' by Alan Watt & Mark Watt
//========================================================
void E3d_MatrixToQuaternion(E3dMatrix LMatrix, E3dQuaternion* LQuaternion)
{
 double		LTr, LS;
 E3dCoordinate	LQ[3];
 register int	LI, LJ, LK;


 LTr=LMatrix[M00]+LMatrix[M11]+LMatrix[M22];
 if(LTr>0.0)
 {
  LS=sqrt(LTr+1.0);LQuaternion->Angle=LS*0.5;LS=0.5/LS;
  LQuaternion->X=(LMatrix[M12]-LMatrix[M21])*LS;
  LQuaternion->Y=(LMatrix[M20]-LMatrix[M02])*LS;
  LQuaternion->Z=(LMatrix[M01]-LMatrix[M10])*LS;
 }
 else
 {
  LI=E3dX;
  if(LMatrix[M11]>LMatrix[M00]) LI=E3dY;
  if(LMatrix[M22]>LMatrix[E3dMtxPos(LI, LI)]) LI=E3dZ;

  LJ=E3d_NXT[LI];LK=E3d_NXT[LJ];
  LS=sqrt(LMatrix[E3dMtxPos(LI, LI)]-LMatrix[E3dMtxPos(LJ, LJ)]-LMatrix[E3dMtxPos(LK, LK)]+1.0);
  LQ[LI]=LS*0.5;LS=0.5/LS;
  LQ[LJ]=(LMatrix[E3dMtxPos(LI, LJ)]+LMatrix[E3dMtxPos(LJ, LI)])*LS;
  LQ[LK]=(LMatrix[E3dMtxPos(LI, LK)]+LMatrix[E3dMtxPos(LK, LI)])*LS;

  LQuaternion->X=LQ[0];
  LQuaternion->Y=LQ[1];
  LQuaternion->Z=LQ[2];
  LQuaternion->Angle=(LMatrix[E3dMtxPos(LJ, LK)]-LMatrix[E3dMtxPos(LK, LJ)])*LS;
 }
}


//========================================================
// Convert a quaternion to a matrix's rotation part
// based on the source in the book 'Advanced Animation
// and rendering techniques by Alan Watt & Mark Watt
//========================================================
void E3d_QuaternionToMatrix(E3dQuaternion* LQ, E3dMatrix LMatrix)
{
 double		LS, LXS, LYS, LZS, LWX, LWY, LWZ, LXX, LXY, LXZ, LYY, LYZ, LZZ;

 LS=2.0/(LQ->X*LQ->X+LQ->Y*LQ->Y+LQ->Z*LQ->Z+LQ->Angle*LQ->Angle);

 LXS=LQ->X*LS;LYS=LQ->Y*LS;LZS=LQ->Z*LS;
 LWX=LQ->Angle*LXS;LWY=LQ->Angle*LYS;LWZ=LQ->Angle*LZS;
 LXX=LQ->X*LXS;LXY=LQ->X*LYS;LXZ=LQ->X*LZS;
 LYY=LQ->Y*LYS;LYZ=LQ->Y*LZS;LZZ=LQ->Z*LZS;

 LMatrix[M00]=1.0-(LYY+LZZ);
 LMatrix[M01]=LXY+LWZ;
 LMatrix[M02]=LXZ-LWY;

 LMatrix[M10]=LXY-LWZ;
 LMatrix[M11]=1.0-(LXX+LZZ);
 LMatrix[M12]=LYZ+LWX;

 LMatrix[M20]=LXZ+LWY;
 LMatrix[M21]=LYZ-LWX;
 LMatrix[M22]=1.0-(LXX+LYY);

 LMatrix[M03]=0.0;
 LMatrix[M13]=0.0;
 LMatrix[M23]=0.0;
 LMatrix[M30]=0.0;LMatrix[M31]=0.0;LMatrix[M32]=0.0;LMatrix[M33]=1.0;
}


//================================================================
// Compute Matrix that rotates a unit vector into another
//================================================================
void E3d_VectorGetRotMatrix(E3dCoordinate LX1, E3dCoordinate LY1, E3dCoordinate LZ1, E3dCoordinate LX2, E3dCoordinate LY2, E3dCoordinate LZ2, E3dMatrix LMatrix)
{
 E3dCoordinate	LAxisX, LAxisY, LAxisZ, Lr, LsinAh, LcosA, LA;

// Get the angle between the two vectors
//
 LcosA=(LX1*LX2+LY1*LY2+LZ1*LZ2);
 if((LcosA>0.999)&&(LcosA<1.001)) E3d_MatrixLoadIdentity(LMatrix);	// Close to 0.0 degrees ?
 else if((LcosA<-0.999)&&(LcosA>-1.001))				// Close to 180.0 degrees ?
 {
  E3dQuaternion		LQ;

  LQ.X=1.0;LQ.Y=0.0;LQ.Z=0.0;LQ.Angle=0.0;
  E3d_QuaternionToMatrix(&LQ, LMatrix);
 }
 else
 {
  E3dQuaternion		LQ;

// The 'axis of the quaternion will be the normal of the triangle defined by the two vectors,
// i.e.: the cross product of the two unit vectors
//
  E3dM_CrossProduct3D(LX2, LY2, LZ2,  LX1, LY1, LZ1, LAxisX, LAxisY, LAxisZ);


  LA=-acos(LcosA);LsinAh=sin(LA*0.5);
  Lr=sqrt(LAxisX*LAxisX+LAxisY*LAxisY+LAxisZ*LAxisZ);
  if(Lr>0.0) { Lr=1.0/Lr*LsinAh;LAxisX*=Lr;LAxisY*=Lr;LAxisZ*=Lr; }
  LQ.X=LAxisX;LQ.Y=LAxisY;LQ.Z=LAxisZ;
  LQ.Angle=cos(LA*0.5);

// Convert the Quaternion to a rotation Matrix
//
  E3d_QuaternionToMatrix(&LQ, LMatrix);
 }
}


//========================================================================
// Create Matrix that rotates a Vector into the 0,0,1 Vector (the Z-axis)
// This is a special case of E3d_VectorGetRotMatrix()
//========================================================================
void E3d_NormalGetDefRotMatrix(E3dCoordinate LNx, E3dCoordinate LNy, E3dCoordinate LNz, E3dMatrix LMatrix)
{
 E3dCoordinate	LAxisX, LAxisY, LAxisZ, Lr, LsinAh, LA;

// Get the angle between the Normal of the Polygon and the destination Normal
//
// Originally:
//  LcosA=(LNx*LX2+LNy*LY2+LNz*LZ2);
// After substituting:
//
// LcosA=LNz;
//
 if((LNz>0.999)&&(LNz<1.001)) E3d_MatrixLoadIdentity(LMatrix);	// Close to 0.0 degrees ?
 else if((LNz<-0.999)&&(LNz>-1.001))				// Close to 180.0 degrees ?
 {
  E3dQuaternion		LQ;

  LQ.X=1.0;LQ.Y=0.0;LQ.Z=0.0;LQ.Angle=0.0;
  E3d_QuaternionToMatrix(&LQ, LMatrix);
 }
 else
 {
  E3dQuaternion		LQ;

// Originally:
//
//  E3dM_CrossProduct3D(0.0, 0.0, 1.0,  LNx, LNy, LNz,  LAxisX, LAxisY, LAxisZ);
//
// Substituting Lxyz1={0, 0, 0} and Lxyz2={0, 0, 1}, we get:
//
  LAxisX=-LNy;
  LAxisY=LNx;
  LAxisZ=0.0;

  LA=-acos(LNz);LsinAh=sin(LA*0.5);
  Lr=sqrt(LAxisX*LAxisX+LAxisY*LAxisY);				// Note: LAxisZ*LAxisZ=0, so we ommitted it
  if(Lr>0.0) { Lr=1.0/Lr*LsinAh;LAxisX*=Lr;LAxisY*=Lr; }
  LQ.X=LAxisX;LQ.Y=LAxisY;LQ.Z=LAxisZ;
  LQ.Angle=cos(LA*0.5);

// Convert the Quaternion to a rotation Matrix
//
  E3d_QuaternionToMatrix(&LQ, LMatrix);
 }
}


//========================================
// Invert a 3x3 Matrix
//========================================
void E3d_MatrixInvert3x3(E3dMatrix LM, E3dMatrix LMI)
{
 LMI[M00]=(LM[M22]*LM[M11]-(LM[M21]*LM[M12]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M01]=(-(LM[M22]*LM[M01])+LM[M21]*LM[M02])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M02]=(LM[M12]*LM[M01]-(LM[M11]*LM[M02]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M10]=(-(LM[M22]*LM[M10])+LM[M20]*LM[M12])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M11]=(LM[M22]*LM[M00]-(LM[M20]*LM[M02]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M12]=(-(LM[M12]*LM[M00])+LM[M10]*LM[M02])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M20]=(LM[M21]*LM[M10]-(LM[M20]*LM[M11]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M21]=(-(LM[M21]*LM[M00])+LM[M20]*LM[M01])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M22]=(LM[M11]*LM[M00]-(LM[M10]*LM[M01]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
}


//========================================
// Invert a 3x4 Matrix
//========================================
void E3d_MatrixInvert3x4(E3dMatrix LM, E3dMatrix LMI)
{
 E3dCoordinate	mX, mY, mZ;

 LMI[M00]=(LM[M22]*LM[M11]-(LM[M21]*LM[M12]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M01]=(-(LM[M22]*LM[M01])+LM[M21]*LM[M02])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M02]=(LM[M12]*LM[M01]-(LM[M11]*LM[M02]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M10]=(-(LM[M22]*LM[M10])+LM[M20]*LM[M12])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M11]=(LM[M22]*LM[M00]-(LM[M20]*LM[M02]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M12]=(-(LM[M12]*LM[M00])+LM[M10]*LM[M02])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M20]=(LM[M21]*LM[M10]-(LM[M20]*LM[M11]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M21]=(-(LM[M21]*LM[M00])+LM[M20]*LM[M01])/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));
 LMI[M22]=(LM[M11]*LM[M00]-(LM[M10]*LM[M01]))/(LM[M22]*LM[M11]*LM[M00]-(LM[M22]*LM[M10]*LM[M01])-(LM[M21]*LM[M12]*LM[M00])+LM[M21]*LM[M10]*LM[M02]+LM[M20]*LM[M12]*LM[M01]-(LM[M20]*LM[M11]*LM[M02]));

 mX=-LM[M30];mY=-LM[M31];mZ=-LM[M32];
 E3dM_MatrixTransform3x3(LMI, LMI[M30], LMI[M31], LMI[M32]);
}


//========================================
// Push current Matrix			*/
//========================================
void E3d_MatrixPush()
{
 glPushMatrix();
 E3d_MatrixScalePtr--;
 E3d_ScaleStack[E3d_MatrixScalePtr].X=E3d_ActScaleX;
 E3d_ScaleStack[E3d_MatrixScalePtr].Y=E3d_ActScaleY;
 E3d_ScaleStack[E3d_MatrixScalePtr].Z=E3d_ActScaleZ;
}


//========================================
// Pop current Matrix
//========================================
void E3d_MatrixPop()
{
 glPopMatrix();
 E3d_ActScaleX=E3d_ScaleStack[E3d_MatrixScalePtr].X;
 E3d_ActScaleY=E3d_ScaleStack[E3d_MatrixScalePtr].Y;
 E3d_ActScaleZ=E3d_ScaleStack[E3d_MatrixScalePtr].Z;
 E3d_MatrixScalePtr++;
}


//========================================
// Copy a Matrix
//========================================
void E3d_MatrixCopy(E3dMatrix LSrcMtx, E3dMatrix LDestMtx)
{
 LDestMtx[M00]=LSrcMtx[M00];LDestMtx[M01]=LSrcMtx[M01];LDestMtx[M02]=LSrcMtx[M02];LDestMtx[M03]=LSrcMtx[M03];
 LDestMtx[M10]=LSrcMtx[M10];LDestMtx[M11]=LSrcMtx[M11];LDestMtx[M12]=LSrcMtx[M12];LDestMtx[M13]=LSrcMtx[M13];
 LDestMtx[M20]=LSrcMtx[M20];LDestMtx[M21]=LSrcMtx[M21];LDestMtx[M22]=LSrcMtx[M22];LDestMtx[M23]=LSrcMtx[M23];
 LDestMtx[M30]=LSrcMtx[M30];LDestMtx[M31]=LSrcMtx[M31];LDestMtx[M32]=LSrcMtx[M32];LDestMtx[M33]=LSrcMtx[M33];
}


//=======================================================================================
// Pre-multiply LSrcMtx with LDestMtx
//
// Arguments
//  E3dMatrix*  LSrcMtx            Pointer to the source Matrix
//  E3dMatrix*  LDestMtx           Pointer to the destination Matrix
//
// Description
//  E3d_MatrixMult Pre-multiplies LSrcMtx with LDestMtx and stores the result in LDestMtx.
//
// See also
//   E3d_MatrixLoadIdentity, E3d_MatrixScale, E3d_MatrixRotate,	E3d_MatrixRotateSinCos, E3d_MatrixTranslate
//========================================================================================
void E3d_MatrixMult(E3dMatrix LSrcMtx, E3dMatrix LDestMtx)
{
 E3dCoordinate	L00, L01, L02, L03, L10, L11, L12, L13,
		L20, L21, L22, L23, L30, L31, L32, L33;


 L00=LDestMtx[M00];L01=LDestMtx[M01];L02=LDestMtx[M02];L03=LDestMtx[M03];
 L10=LDestMtx[M10];L11=LDestMtx[M11];L12=LDestMtx[M12];L13=LDestMtx[M13];
 L20=LDestMtx[M20];L21=LDestMtx[M21];L22=LDestMtx[M22];L23=LDestMtx[M23];
 L30=LDestMtx[M30];L31=LDestMtx[M31];L32=LDestMtx[M32];L33=LDestMtx[M33];

 LDestMtx[M00]=LSrcMtx[M00]*L00+LSrcMtx[M01]*L10+LSrcMtx[M02]*L20+LSrcMtx[M03]*L30;
 LDestMtx[M01]=LSrcMtx[M00]*L01+LSrcMtx[M01]*L11+LSrcMtx[M02]*L21+LSrcMtx[M03]*L31;
 LDestMtx[M02]=LSrcMtx[M00]*L02+LSrcMtx[M01]*L12+LSrcMtx[M02]*L22+LSrcMtx[M03]*L32;
 LDestMtx[M03]=LSrcMtx[M00]*L03+LSrcMtx[M01]*L13+LSrcMtx[M02]*L23+LSrcMtx[M03]*L33;

 LDestMtx[M10]=LSrcMtx[M10]*L00+LSrcMtx[M11]*L10+LSrcMtx[M12]*L20+LSrcMtx[M13]*L30;
 LDestMtx[M11]=LSrcMtx[M10]*L01+LSrcMtx[M11]*L11+LSrcMtx[M12]*L21+LSrcMtx[M13]*L31;
 LDestMtx[M12]=LSrcMtx[M10]*L02+LSrcMtx[M11]*L12+LSrcMtx[M12]*L22+LSrcMtx[M13]*L32;
 LDestMtx[M13]=LSrcMtx[M10]*L03+LSrcMtx[M11]*L13+LSrcMtx[M12]*L23+LSrcMtx[M13]*L33;

 LDestMtx[M20]=LSrcMtx[M20]*L00+LSrcMtx[M21]*L10+LSrcMtx[M22]*L20+LSrcMtx[M23]*L30;
 LDestMtx[M21]=LSrcMtx[M20]*L01+LSrcMtx[M21]*L11+LSrcMtx[M22]*L21+LSrcMtx[M23]*L31;
 LDestMtx[M22]=LSrcMtx[M20]*L02+LSrcMtx[M21]*L12+LSrcMtx[M22]*L22+LSrcMtx[M23]*L32;
 LDestMtx[M23]=LSrcMtx[M20]*L03+LSrcMtx[M21]*L13+LSrcMtx[M22]*L23+LSrcMtx[M23]*L33;

 LDestMtx[M30]=LSrcMtx[M30]*L00+LSrcMtx[M31]*L10+LSrcMtx[M32]*L20+LSrcMtx[M33]*L30;
 LDestMtx[M31]=LSrcMtx[M30]*L01+LSrcMtx[M31]*L11+LSrcMtx[M32]*L21+LSrcMtx[M33]*L31;
 LDestMtx[M32]=LSrcMtx[M30]*L02+LSrcMtx[M31]*L12+LSrcMtx[M32]*L22+LSrcMtx[M33]*L32;
 LDestMtx[M33]=LSrcMtx[M30]*L03+LSrcMtx[M31]*L13+LSrcMtx[M32]*L23+LSrcMtx[M33]*L33;
}


//=======================================================================================
// Pre-multiply LMatrix1 with LMatrix2
//
// Arguments
//  E3dMatrix*  LSrcMtx            Pointer to the source Matrix
//  E3dMatrix*  LDestMtx           Pointer to the destination Matrix
//
// Description
//  E3d_MatrixMult Pre-multiplies LMatrix1 with LMatrix2 and stores the result in LDestMtx.
//
// See also
//   E3d_MatrixLoadIdentity, E3d_MatrixScale, E3d_MatrixRotate,	E3d_MatrixRotateSinCos, E3d_MatrixTranslate
//========================================================================================
void E3d_MatrixMult3(E3dMatrix LMatrix1, E3dMatrix LMatrix2, E3dMatrix LDestMtx)
{
 E3dCoordinate	L00, L01, L02, L03, L10, L11, L12, L13,
		L20, L21, L22, L23, L30, L31, L32, L33;


 L00=LMatrix2[M00];L01=LMatrix2[M01];L02=LMatrix2[M02];L03=LMatrix2[M03];
 L10=LMatrix2[M10];L11=LMatrix2[M11];L12=LMatrix2[M12];L13=LMatrix2[M13];
 L20=LMatrix2[M20];L21=LMatrix2[M21];L22=LMatrix2[M22];L23=LMatrix2[M23];
 L30=LMatrix2[M30];L31=LMatrix2[M31];L32=LMatrix2[M32];L33=LMatrix2[M33];

 LDestMtx[M00]=LMatrix1[M00]*L00+LMatrix1[M01]*L10+LMatrix1[M02]*L20+LMatrix1[M03]*L30;
 LDestMtx[M01]=LMatrix1[M00]*L01+LMatrix1[M01]*L11+LMatrix1[M02]*L21+LMatrix1[M03]*L31;
 LDestMtx[M02]=LMatrix1[M00]*L02+LMatrix1[M01]*L12+LMatrix1[M02]*L22+LMatrix1[M03]*L32;
 LDestMtx[M03]=LMatrix1[M00]*L03+LMatrix1[M01]*L13+LMatrix1[M02]*L23+LMatrix1[M03]*L33;

 LDestMtx[M10]=LMatrix1[M10]*L00+LMatrix1[M11]*L10+LMatrix1[M12]*L20+LMatrix1[M13]*L30;
 LDestMtx[M11]=LMatrix1[M10]*L01+LMatrix1[M11]*L11+LMatrix1[M12]*L21+LMatrix1[M13]*L31;
 LDestMtx[M12]=LMatrix1[M10]*L02+LMatrix1[M11]*L12+LMatrix1[M12]*L22+LMatrix1[M13]*L32;
 LDestMtx[M13]=LMatrix1[M10]*L03+LMatrix1[M11]*L13+LMatrix1[M12]*L23+LMatrix1[M13]*L33;

 LDestMtx[M20]=LMatrix1[M20]*L00+LMatrix1[M21]*L10+LMatrix1[M22]*L20+LMatrix1[M23]*L30;
 LDestMtx[M21]=LMatrix1[M20]*L01+LMatrix1[M21]*L11+LMatrix1[M22]*L21+LMatrix1[M23]*L31;
 LDestMtx[M22]=LMatrix1[M20]*L02+LMatrix1[M21]*L12+LMatrix1[M22]*L22+LMatrix1[M23]*L32;
 LDestMtx[M23]=LMatrix1[M20]*L03+LMatrix1[M21]*L13+LMatrix1[M22]*L23+LMatrix1[M23]*L33;

 LDestMtx[M30]=LMatrix1[M30]*L00+LMatrix1[M31]*L10+LMatrix1[M32]*L20+LMatrix1[M33]*L30;
 LDestMtx[M31]=LMatrix1[M30]*L01+LMatrix1[M31]*L11+LMatrix1[M32]*L21+LMatrix1[M33]*L31;
 LDestMtx[M32]=LMatrix1[M30]*L02+LMatrix1[M31]*L12+LMatrix1[M32]*L22+LMatrix1[M33]*L32;
 LDestMtx[M33]=LMatrix1[M30]*L03+LMatrix1[M31]*L13+LMatrix1[M32]*L23+LMatrix1[M33]*L33;
}


//========================================================================================
// Compare Matrices
//
// Arguments
//  E3dMatrix* LMtx1           Pointer to matrix-1
//  E3dMatrix* LMtx2           Pointer to matrix-2
//
// Description
//  E3d_MatricesEqual compares LMtx1 with LMtx2 and returns TRUE if they are equal.
//
// Return value
//  TRUE if LMtx1 and LMtx2 are equal, FALSE otherwise.
//
// See also
//  E3d_MatrixLoadIdentity, E3d_MatrixScale, E3d_MatrixRotate,	E3d_MatrixRotateSinCos
//  E3d_MatrixTranslat
//========================================================================================
EBool E3d_MatricesEqual(E3dMatrix LMtx1, E3dMatrix LMtx2)
{
 if((LMtx1[M00]==LMtx2[M00])&&(LMtx1[M01]==LMtx2[M01])&&(LMtx1[M02]==LMtx2[M02])&&(LMtx1[M03]==LMtx2[M03])&&
    (LMtx1[M10]==LMtx2[M10])&&(LMtx1[M11]==LMtx2[M11])&&(LMtx1[M12]==LMtx2[M12])&&(LMtx1[M13]==LMtx2[M13])&&
    (LMtx1[M20]==LMtx2[M20])&&(LMtx1[M21]==LMtx2[M21])&&(LMtx1[M22]==LMtx2[M22])&&(LMtx1[M23]==LMtx2[M23])&&
    (LMtx1[M30]==LMtx2[M30])&&(LMtx1[M31]==LMtx2[M31])&&(LMtx1[M32]==LMtx2[M32])&&(LMtx1[M33]==LMtx2[M33])) return(TRUE);
 return(FALSE);
}


//========================================================================================
// Make a matrix an identity Matrix
//
// Arguments
//  E3dMatrix* LDestMtx        Pointer to the Matrix
//
// Description
//  E3d_MatrixLoadIdentity makes LDestMtx an identity Matrix.
//
// See also
//  E3d_MatricesEqual
//========================================================================================
void E3d_MatrixLoadIdentity(E3dMatrix LDestMtx)
{
 LDestMtx[M00]=1.0;LDestMtx[M01]=0.0;LDestMtx[M02]=0.0;LDestMtx[M03]=0.0;
 LDestMtx[M10]=0.0;LDestMtx[M11]=1.0;LDestMtx[M12]=0.0;LDestMtx[M13]=0.0;
 LDestMtx[M20]=0.0;LDestMtx[M21]=0.0;LDestMtx[M22]=1.0;LDestMtx[M23]=0.0;
 LDestMtx[M30]=0.0;LDestMtx[M31]=0.0;LDestMtx[M32]=0.0;LDestMtx[M33]=1.0;
}


//================================================
// Reset a Matrix to contain translation only
//================================================
void E3d_MatrixResetTrans(E3dMatrix LMatrix, double LX, double LY, double LZ)
{
 LMatrix[M00]=1.0;LMatrix[M01]=0.0;LMatrix[M02]=0.0;LMatrix[M03]=0.0;
 LMatrix[M10]=0.0;LMatrix[M11]=1.0;LMatrix[M12]=0.0;LMatrix[M13]=0.0;
 LMatrix[M20]=0.0;LMatrix[M21]=0.0;LMatrix[M22]=1.0;LMatrix[M23]=0.0;
 LMatrix[M30]=LX;LMatrix[M31]=LY;LMatrix[M32]=LZ;LMatrix[M33]=1.0;
}


//================================================================================
// "Translate" a Mmatrix
//
// Arguments
//  E3dMatrix  LMatrix                 The E3dMatrix to transform
//  double     LX, LY, LZ              Translation coordinates
//
// Description
//  E3d_MatrixTranslate performs a standard "translate" operation on the given Matrix.
//
// See also
//  E3d_MatrixScale, E3d_MatrixRotate, E3d_MatrixRotateSinCos
//================================================================================
void E3d_MatrixTranslate(E3dMatrix LMatrix, double LX, double LY, double LZ)
{
 E3dCoordinate	LDX, LDY, LDZ;

 LDX=LX*LMatrix[M00]+LY*LMatrix[M10]+LZ*LMatrix[M20];
 LDY=LX*LMatrix[M01]+LY*LMatrix[M11]+LZ*LMatrix[M21];
 LDZ=LX*LMatrix[M02]+LY*LMatrix[M12]+LZ*LMatrix[M22];

 LMatrix[M30]+=LDX;
 LMatrix[M31]+=LDY;
 LMatrix[M32]+=LDZ;
}


//================================================================================
// "Scale" a Matrix
//
// Arguments
//  E3dMatrix  LMatrix                 The E3dMatrix to transform
//  double     LX, LY, LZ              Translation coordinates
//
// Description
//  E3d_MatrixScale performs a standard "scale" operation on the given Matrix.
//
// See also
//  E3d_MatrixTranslate, E3d_MatrixRotate, E3d_MatrixRotateSinCos
//================================================================================
void E3d_MatrixScale(E3dMatrix LMatrix, double LXScale, double LYScale, double LZScale)
{
 LMatrix[M00]*=LXScale;LMatrix[M01]*=LXScale;LMatrix[M02]*=LXScale;
 LMatrix[M10]*=LYScale;LMatrix[M11]*=LYScale;LMatrix[M12]*=LYScale;
 LMatrix[M20]*=LZScale;LMatrix[M21]*=LZScale;LMatrix[M22]*=LZScale;
}


//================================================================================
// "Rotate" a Matrix
//
// Arguments
//  E3dMatrix  LMatrix             The E3dMatrix to transform
//  int        LAxis               The axis to rotate around
//  double     LAngle              The the rotation angle in degrees
//
// Description
//  E3d_MatrixRotate performs a standard "rotate" operation on the matrix around
//  the given axis.
//  The angle is given in degrees according to the right hand rule, the axis can
//  be one of the 'x', 'y' or 'z' ASCII character values.
//
// See also
//  E3d_MatrixScale, E3d_MatrixTranslate, E3d_MatrixRotateSinCos
//================================================================================
void E3d_MatrixRotate(E3dMatrix LMatrix, int LAxis, double LAngle)
{
 E3dAngle	LSinA, LCosA, LMSinA;
 E3dCoordinate	LM0, LM1, LM2, LM3, LM4, LM5;


 LSinA=sin(LAngle*E3dDEGREES_TO_RADIANS);LMSinA=-LSinA;
 LCosA=cos(LAngle*E3dDEGREES_TO_RADIANS);
 switch(LAxis)
 {
  case 'x':
   LM0=LMatrix[M10]*LCosA+LMatrix[M20]*LSinA;LM1=LMatrix[M11]*LCosA+LMatrix[M21]*LSinA;LM2=LMatrix[M12]*LCosA+LMatrix[M22]*LSinA;
   LM3=LMatrix[M10]*LMSinA+LMatrix[M20]*LCosA;LM4=LMatrix[M11]*LMSinA+LMatrix[M21]*LCosA;LM5=LMatrix[M12]*LMSinA+LMatrix[M22]*LCosA;

   LMatrix[M10]=LM0;LMatrix[M11]=LM1;LMatrix[M12]=LM2;
   LMatrix[M20]=LM3;LMatrix[M21]=LM4;LMatrix[M22]=LM5;
  break;

  case 'y':
   LM0=LMatrix[M00]*LCosA+LMatrix[M20]*LMSinA;LM1=LMatrix[M01]*LCosA+LMatrix[M21]*LMSinA;LM2=LMatrix[M02]*LCosA+LMatrix[M22]*LMSinA;
   LM3=LMatrix[M00]*LSinA+LMatrix[M20]*LCosA;LM4=LMatrix[M01]*LSinA+LMatrix[M21]*LCosA;LM5=LMatrix[M02]*LSinA+LMatrix[M22]*LCosA;

   LMatrix[M00]=LM0;LMatrix[M01]=LM1;LMatrix[M02]=LM2;
   LMatrix[M20]=LM3;LMatrix[M21]=LM4;LMatrix[M22]=LM5;
  break;

  case 'z':
   LM0=LMatrix[M00]*LCosA+LMatrix[M10]*LSinA;LM1=LMatrix[M01]*LCosA+LMatrix[M11]*LSinA;LM2=LMatrix[M02]*LCosA+LMatrix[M12]*LSinA;
   LM3=LMatrix[M00]*LMSinA+LMatrix[M10]*LCosA;LM4=LMatrix[M01]*LMSinA+LMatrix[M11]*LCosA;LM5=LMatrix[M02]*LMSinA+LMatrix[M12]*LCosA;

   LMatrix[M00]=LM0;LMatrix[M01]=LM1;LMatrix[M02]=LM2;
   LMatrix[M10]=LM3;LMatrix[M11]=LM4;LMatrix[M12]=LM5;
  break;
 }
}


//========================================================================
// "Rotate" a Matrix. The angle is given by its sin and cos values
//========================================================================
void E3d_MatrixRotateSinCos(E3dMatrix LMatrix, int LAxis, double LSinA, double LCosA)
{
 E3dAngle	LMSinA;
 E3dMatrix	LTMtx;


 LMSinA=-LSinA;
 switch(LAxis)
 {
  case 'x':
   LTMtx[M10]=LMatrix[M10]*LCosA+LMatrix[M20]*LSinA;LTMtx[M11]=LMatrix[M11]*LCosA+LMatrix[M21]*LSinA;LTMtx[M12]=LMatrix[M12]*LCosA+LMatrix[M22]*LSinA;

   LTMtx[M20]=LMatrix[M10]*LMSinA+LMatrix[M20]*LCosA;LTMtx[M21]=LMatrix[M11]*LMSinA+LMatrix[M21]*LCosA;LTMtx[M22]=LMatrix[M12]*LMSinA+LMatrix[M22]*LCosA;

   LMatrix[M10]=LTMtx[M10];LMatrix[M11]=LTMtx[M11];LMatrix[M12]=LTMtx[M12];
   LMatrix[M20]=LTMtx[M20];LMatrix[M21]=LTMtx[M21];LMatrix[M22]=LTMtx[M22];
  break;

  case 'y':
   LTMtx[M00]=LMatrix[M00]*LCosA+LMatrix[M20]*LMSinA;LTMtx[M01]=LMatrix[M01]*LCosA+LMatrix[M21]*LMSinA;LTMtx[M02]=LMatrix[M02]*LCosA+LMatrix[M22]*LMSinA;
   LTMtx[M20]=LMatrix[M00]*LSinA+LMatrix[M20]*LCosA;LTMtx[M21]=LMatrix[M01]*LSinA+LMatrix[M21]*LCosA;LTMtx[M22]=LMatrix[M02]*LSinA+LMatrix[M22]*LCosA;

   LMatrix[M00]=LTMtx[M00];LMatrix[M01]=LTMtx[M01];LMatrix[M02]=LTMtx[M02];
   LMatrix[M20]=LTMtx[M20];LMatrix[M21]=LTMtx[M21];LMatrix[M22]=LTMtx[M22];
  break;

  case 'z':
   LTMtx[M00]=LMatrix[M00]*LCosA+LMatrix[M10]*LSinA;LTMtx[M01]=LMatrix[M01]*LCosA+LMatrix[M11]*LSinA;LTMtx[M02]=LMatrix[M02]*LCosA+LMatrix[M12]*LSinA;

   LTMtx[M10]=LMatrix[M00]*LMSinA+LMatrix[M10]*LCosA;LTMtx[M11]=LMatrix[M01]*LMSinA+LMatrix[M11]*LCosA;LTMtx[M12]=LMatrix[M02]*LMSinA+LMatrix[M12]*LCosA;

   LMatrix[M00]=LTMtx[M00];LMatrix[M01]=LTMtx[M01];LMatrix[M02]=LTMtx[M02];
   LMatrix[M10]=LTMtx[M10];LMatrix[M11]=LTMtx[M11];LMatrix[M12]=LTMtx[M12];
  break;
 }
}


//================================================================
// "Rotate" a Matrix with X, Y and Z angles in the given order
//================================================================
void E3d_MatrixRotateXYZ(E3dMatrix LMatrix, E3dAngle LX, E3dAngle LY, E3dAngle LZ, int LOrder)
{
 switch(LOrder)
 {
  case E3dXYZ:
   E3d_MatrixRotate(LMatrix, 'x', LX);
   E3d_MatrixRotate(LMatrix, 'y', LY);
   E3d_MatrixRotate(LMatrix, 'z', LZ);
  break;

  case E3dXZY:
   E3d_MatrixRotate(LMatrix, 'x', LX);
   E3d_MatrixRotate(LMatrix, 'z', LZ);
   E3d_MatrixRotate(LMatrix, 'y', LY);
  break;

  case E3dYXZ:
   E3d_MatrixRotate(LMatrix, 'y', LY);
   E3d_MatrixRotate(LMatrix, 'x', LX);
   E3d_MatrixRotate(LMatrix, 'z', LZ);
  break;

  case E3dYZX:
   E3d_MatrixRotate(LMatrix, 'y', LY);
   E3d_MatrixRotate(LMatrix, 'z', LZ);
   E3d_MatrixRotate(LMatrix, 'x', LX);
  break;

  case E3dZYX:
   E3d_MatrixRotate(LMatrix, 'z', LZ);
   E3d_MatrixRotate(LMatrix, 'y', LY);
   E3d_MatrixRotate(LMatrix, 'x', LX);
  break;

  case E3dZXY:
   E3d_MatrixRotate(LMatrix, 'z', LZ);
   E3d_MatrixRotate(LMatrix, 'x', LX);
   E3d_MatrixRotate(LMatrix, 'y', LY);
  break;
 }
}


//================================================================
// Ken Shoemake's recommended algorithm is to convert the
//  quaternion to a Matrix and the Matrix to Euler angles.
//  We do this, without generating unused Matrix elements.
//================================================================
void E3d_MatrixToEuler(int LRotOrder, E3dMatrix LM, float LRot[3])
{
 float		sxr, cxr, syr, cyr, szr, czr,
		xr=0.0, yr=0.0, zr=0.0;
 static float	epsilon=1.0e-5;

 switch(LRotOrder)
 {
  case E3dZYX:
   syr=-LM[M02];
   cyr=sqrt(1-syr*syr);

   if(cyr<epsilon)			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=atan2(-LM[M21], LM[M11]);
    yr=(syr>0) ? M_PI_2 : -M_PI_2;	// +/- 90 degrees
    zr=0.0;
   }
   else
   {
    xr=atan2(LM[M12], LM[M22]);
    yr=atan2(syr, cyr);
    zr=atan2(LM[M01], LM[M00]);
   }
  break;

  case E3dYZX:
   szr=LM[M01];
   czr=sqrt(1-szr*szr);
   if(czr<epsilon) 			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=atan2(LM[M12], LM[M22]);
    yr=0.0;
    zr=(szr>0) ? M_PI_2 : -M_PI_2;
   }
   else
   {
    xr=atan2(-LM[M21], LM[M11]);
    yr=atan2(-LM[M02], LM[M00]);
    zr=atan2(szr, czr);
   }
  break;

  case E3dZXY:
   sxr=LM[M12];
   cxr=sqrt(1-sxr*sxr);

   if(cxr<epsilon) 			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=(sxr>0) ? M_PI_2 : -M_PI_2;
    yr=atan2(LM[M20], LM[M00]);
    zr=0.0;
   }
   else
   {
    xr=atan2(sxr, cxr);
    yr=atan2(-LM[M02], LM[M22]);
    zr=atan2(-LM[M10], LM[M11]);
   }
  break;

  case E3dXZY:
   szr=-LM[M10];
   czr=sqrt(1-szr*szr);
   if(czr<epsilon)			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=0.0;
    yr=atan2(-LM[M02], LM[M22]);
    zr=(szr>0) ? M_PI_2 : -M_PI_2;
   }
   else
   {
    xr=atan2(LM[M02], LM[M11]);
    yr=atan2(LM[M20], LM[M00]);
    zr=atan2(szr, czr);
   }
  break;

  case E3dYXZ:
   sxr=-LM[M21];
   cxr=sqrt(1-sxr*sxr);
   if(cxr<epsilon)			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=(sxr>0) ? M_PI_2 : -M_PI_2;
    yr=0.0;
    zr=atan2(-LM[M10], LM[M00]);
   }
   else
   {
    xr=atan2(sxr, cxr);
    yr=atan2(LM[M20], LM[M22]);
    zr=atan2(LM[M01], LM[M11]);
   }
  break;

  case E3dXYZ:
   syr=LM[M20];
   cyr=sqrt(1-syr*syr);
   if(cyr<epsilon)			// Insufficient accuracy, assume that yr=PI/2 && zr=0
   {
    xr=0.0;
    yr=(syr>0) ? M_PI_2 : -M_PI_2;
    zr=atan2(LM[M01], LM[M11]);
   }
   else
   {
    xr=atan2(-LM[M21], LM[M22]);
    yr=atan2(syr, cyr);
    zr=atan2(-LM[M11], LM[M00]);
   }
  break;
 }

 LRot[0]=xr*E3dRADIANS_TO_DEGREES;LRot[1]=yr*E3dRADIANS_TO_DEGREES;LRot[2]=zr*E3dRADIANS_TO_DEGREES;
}


//========================================================================================
// Decompose a Matrix to translate, rotate (a quaternion) and scale transformations
//========================================================================================
void E3d_DecomposeMatrix(E3dMatrix LMatrix, E3dDecompMatrix* LDMatrix, int LRotOrder)
{
 float		LSXY, LSXZ, LRot[3], LDet, LScaleX, LScaleY, LScaleZ, LF;
 E3dMatrix	LM;

 LDMatrix->Translate[0]=LMatrix[M30];
 LDMatrix->Translate[1]=LMatrix[M31];
 LDMatrix->Translate[2]=LMatrix[M32];

 LM[M03]=0.0;
 LM[M13]=0.0;
 LM[M23]=0.0;
 LM[M33]=1.0;
 LM[M30]=0.0;
 LM[M31]=0.0;
 LM[M32]=0.0;

 LM[M00]=LMatrix[M00];
 LM[M01]=LMatrix[M01];
 LM[M02]=LMatrix[M02];

 LDMatrix->Scale[0]=LScaleX=sqrt(LM[M00]*LM[M00]+LM[M01]*LM[M01]+LM[M02]*LM[M02]);

// Normalize first row
//
 LF=1.0/LScaleX;
 LM[M00]*=LF;LM[M01]*=LF;LM[M02]*=LF;

// Determine xy shear
//
 LSXY=LMatrix[M00]*LMatrix[M10]+LMatrix[M01]*LMatrix[M11]+LMatrix[M02]*LMatrix[M12];

 LM[M10]=LMatrix[M10]-LSXY*LMatrix[M00];
 LM[M11]=LMatrix[M11]-LSXY*LMatrix[M01];
 LM[M12]=LMatrix[M12]-LSXY*LMatrix[M02];

 LDMatrix->Scale[1]=LScaleY=sqrt(LM[M10]*LM[M10]+LM[M11]*LM[M11]+LM[M12]*LM[M12]);

// Normalize second row
//
 LF=1.0/LScaleY;
 LM[M10]*=LF;LM[M11]*=LF;LM[M12]*=LF;

// Determine xz shear
//
 LSXZ=LMatrix[M00]*LMatrix[M20]+LMatrix[M01]*LMatrix[M21]+LMatrix[M02]*LMatrix[M22];

 LM[M20]=LMatrix[M20]-LSXZ*LMatrix[M00];
 LM[M21]=LMatrix[M21]-LSXZ*LMatrix[M01];
 LM[M22]=LMatrix[M22]-LSXZ*LMatrix[M02];

 LDMatrix->Scale[2]=LScaleZ=sqrt(LM[M20]*LM[M20]+LM[M21]*LM[M21]+LM[M22]*LM[M22]);

// Normalize third row
//
 LF=1.0/LScaleZ;
 LM[M20]*=LF;LM[M21]*=LF;LM[M22]*=LF;

 LDet=(LM[M00]*LM[M11]*LM[M22])+(LM[M01]*LM[M12]*LM[M20])+(LM[M02]*LM[M10]*LM[M21])-(LM[M02]*LM[M11]*LM[M20])-(LM[M00]*LM[M12]*LM[M21])-(LM[M01]*LM[M10]*LM[M22]);

// If the determinant of the rotation matrix is negative, negate the matrix and scale factors
//
 if(LDet<0.0)
 {
  LM[M00]*=-1.0;LM[M01]*=-1.0;LM[M02]*=-1.0;
  LM[M10]*=-1.0;LM[M11]*=-1.0;LM[M12]*=-1.0;
  LM[M20]*=-1.0;LM[M21]*=-1.0;LM[M22]*=-1.0;

  LDMatrix->Scale[0]*=-1.0;
  LDMatrix->Scale[1]*=-1.0;
  LDMatrix->Scale[2]*=-1.0;
 }

// Copy the rotation matrix into the decomposition structure
//
 memcpy(LDMatrix->RotMatrix, LM, sizeof(E3dMatrix));

 LRot[1]=asin(-LM[M02]);
 if(fabs(cos(LRot[1]))>0.0001)
 {
  LRot[0]=asin(LM[M12]/cos(LRot[1]));
  LRot[2]=asin(LM[M01]/cos(LRot[1]));
 }
 else
 {
  LRot[0]=acos(LM[M11]);
  LRot[2]=0.0;
 }

 if(LRotOrder!=E3dNONE)
 {
  E3d_MatrixToEuler(LRotOrder, LM, LRot);
  LDMatrix->RotationAngles[0]=LRot[0];
  LDMatrix->RotationAngles[1]=LRot[1];
  LDMatrix->RotationAngles[2]=LRot[2];
 }
 else
 {
  LDMatrix->RotationAngles[0]=0.0;
  LDMatrix->RotationAngles[1]=0.0;
  LDMatrix->RotationAngles[2]=0.0;
 }
 E3d_MatrixToQuaternion(LM, &(LDMatrix->Orientation));
}


//================================================
// Transform a position with a given Matrix
//================================================
void E3d_MatrixTransformPosition(E3dMatrix LMatrix, E3d3DPosition* LPosition)
{
 E3dCoordinate	LX, LY, LZ;

 LX=LPosition->X;LY=LPosition->Y;LZ=LPosition->Z;
 LPosition->X=LX*LMatrix[M00]+LY*LMatrix[M10]+LZ*LMatrix[M20]+LMatrix[M30];
 LPosition->Y=LX*LMatrix[M01]+LY*LMatrix[M11]+LZ*LMatrix[M21]+LMatrix[M31];
 LPosition->Z=LX*LMatrix[M02]+LY*LMatrix[M12]+LZ*LMatrix[M22]+LMatrix[M32];
}


//========================================================
// Transform a homogeneous position with a given Matrix
//========================================================
void E3d_MatrixTransformHPosition(E3dMatrix LMatrix, E3dHPosition* LPosition)
{
 E3dCoordinate	LX, LY, LZ, LW;

 LX=LPosition->X;LY=LPosition->Y;LZ=LPosition->Z;LW=1.0;
 LPosition->X=LX*LMatrix[M00]+LY*LMatrix[M10]+LZ*LMatrix[M20]+LW*LMatrix[M30];
 LPosition->Y=LX*LMatrix[M01]+LY*LMatrix[M11]+LZ*LMatrix[M21]+LW*LMatrix[M31];
 LPosition->Z=LX*LMatrix[M02]+LY*LMatrix[M12]+LZ*LMatrix[M22]+LW*LMatrix[M32];
 LPosition->W=LX*LMatrix[M03]+LY*LMatrix[M13]+LZ*LMatrix[M23]+LW*LMatrix[M33];
}


//================================================
// Transform a vector with a given Matrix
//================================================
void E3d_MatrixTransformVector(E3dMatrix LMatrix, E3dVector* LVector)
{
 E3dCoordinate	LX, LY, LZ;

 LX=LVector->X;LY=LVector->Y;LZ=LVector->Z;
 LVector->X=LX*LMatrix[M00]+LY*LMatrix[M10]+LZ*LMatrix[M20]+LMatrix[M30];
 LVector->Y=LX*LMatrix[M01]+LY*LMatrix[M11]+LZ*LMatrix[M21]+LMatrix[M31];
 LVector->Z=LX*LMatrix[M02]+LY*LMatrix[M12]+LZ*LMatrix[M22]+LMatrix[M32];
}


//================================================================
// Transform a vector with a given matrix's top-left 3x3 part
//================================================================
void E3d_MatrixTransformVector3x3(E3dMatrix LMatrix, E3dVector* LVector)
{
 E3dCoordinate	LX, LY, LZ;

 LX=LVector->X;LY=LVector->Y;LZ=LVector->Z;
 LVector->X=LX*LMatrix[M00]+LY*LMatrix[M10]+LZ*LMatrix[M20];
 LVector->Y=LX*LMatrix[M01]+LY*LMatrix[M11]+LZ*LMatrix[M21];
 LVector->Z=LX*LMatrix[M02]+LY*LMatrix[M12]+LZ*LMatrix[M22];
}


//================================================================
// Perform 'Lookat' Camera transformation on a given Matrix
//================================================================
void E3d_MatrixLookAt(E3dMatrix LMatrix, double LUpX, double LUpY, double LUpZ, double LCamX, double LCamY, double LCamZ, double LIntX, double LIntY, double LIntZ)
{
 E3dMatrix	LTMatrix;
 double		LFX, LFY, LFZ, LSX, LSY, LSZ, LUX, LUY, LUZ, LL;


 LFX=LIntX-LCamX;
 LFY=LIntY-LCamY;
 LFZ=LIntZ-LCamZ;

 LL=sqrt(LFX*LFX+LFY*LFY+LFZ*LFZ);LL=1.0/LL;
 LFX*=LL;LFY*=LL;LFZ*=LL;

 E3dM_VectorCrossProduct(LFX, LFY, LFZ, LUpX, LUpY, LUpZ, LSX, LSY, LSZ);
 E3dM_VectorCrossProduct(LSX, LSY, LSZ, LFX, LFY, LFZ, LUX, LUY, LUZ);

 LTMatrix[M00]=LSX;LTMatrix[M01]=LUX;LTMatrix[M02]=-LFX;LTMatrix[M03]=0.0;
 LTMatrix[M10]=LSY;LTMatrix[M11]=LUY;LTMatrix[M12]=-LFY;LTMatrix[M13]=0.0;
 LTMatrix[M20]=LSZ;LTMatrix[M21]=LUZ;LTMatrix[M22]=-LFZ;LTMatrix[M23]=0.0;
 LTMatrix[M30]=0.0;LTMatrix[M31]=0.0;LTMatrix[M32]=0.0;LTMatrix[M33]=1.0;

// Translate Eye to Origin
//
 E3d_MatrixTranslate(LTMatrix, -LCamX, -LCamY, -LCamZ);

 E3d_MatrixMult(LTMatrix, LMatrix);
}


//================================================================
// Perform 'Lookat' camera transformation on a given Matrix
//================================================================
void E3d_MatrixLookAlong(E3dMatrix LMatrix, double LFX, double LFY, double LFZ, double LUpX, double LUpY, double LUpZ, double LCamX, double LCamY, double LCamZ)
{
 E3dMatrix	LTMatrix;
 double		LSX, LSY, LSZ, LUX, LUY, LUZ;

 E3dM_VectorCrossProduct(LFX, LFY, LFZ, LUpX, LUpY, LUpZ, LSX, LSY, LSZ);
 E3dM_VectorCrossProduct(LSX, LSY, LSZ, LFX, LFY, LFZ, LUX, LUY, LUZ);

 LTMatrix[M00]=LSX;LTMatrix[M01]=LUX;LTMatrix[M02]=-LFX;LTMatrix[M03]=0.0;
 LTMatrix[M10]=LSY;LTMatrix[M11]=LUY;LTMatrix[M12]=-LFY;LTMatrix[M13]=0.0;
 LTMatrix[M20]=LSZ;LTMatrix[M21]=LUZ;LTMatrix[M22]=-LFZ;LTMatrix[M23]=0.0;
 LTMatrix[M30]=0.0;LTMatrix[M31]=0.0;LTMatrix[M32]=0.0;LTMatrix[M33]=1.0;
 E3d_MatrixTranslate(LTMatrix, -LCamX, -LCamY, -LCamZ);

 E3d_MatrixMult(LTMatrix, LMatrix);
}


//================================================================
// Set 'Look Along' camera transformation on a given Matrix
//================================================================
void E3d_MatrixSetLookAlong(E3dMatrix LMatrix, double LFX, double LFY, double LFZ, double LUpX, double LUpY, double LUpZ, double LCamX, double LCamY, double LCamZ)
{
 double		LSX, LSY, LSZ, LUX, LUY, LUZ;

 E3dM_VectorCrossProduct(LFX, LFY, LFZ, LUpX, LUpY, LUpZ, LSX, LSY, LSZ);
 E3dM_VectorCrossProduct(LSX, LSY, LSZ, LFX, LFY, LFZ, LUX, LUY, LUZ);

 LMatrix[M00]=LSX;LMatrix[M01]=LUX;LMatrix[M02]=-LFX;LMatrix[M03]=0.0;
 LMatrix[M10]=LSY;LMatrix[M11]=LUY;LMatrix[M12]=-LFY;LMatrix[M13]=0.0;
 LMatrix[M20]=LSZ;LMatrix[M21]=LUZ;LMatrix[M22]=-LFZ;LMatrix[M23]=0.0;
 LMatrix[M30]=0.0;LMatrix[M31]=0.0;LMatrix[M32]=0.0;LMatrix[M33]=1.0;
 E3d_MatrixTranslate(LMatrix, -LCamX, -LCamY, -LCamZ);
}


#ifdef USEOpenGL
//========================================
// Push current matrix (GL version)
//========================================
void E3d_MatrixPushGL()
{
 glPushMatrix();
 E3d_MatrixScalePtr--;
 E3d_ScaleStack[E3d_MatrixScalePtr].X=E3d_ActScaleX;
 E3d_ScaleStack[E3d_MatrixScalePtr].Y=E3d_ActScaleY;
 E3d_ScaleStack[E3d_MatrixScalePtr].Z=E3d_ActScaleZ;
}


//========================================
// Pop current matrix (GL version)
//========================================
void E3d_MatrixPopGL()
{
 glPopMatrix();
 E3d_ActScaleX=E3d_ScaleStack[E3d_MatrixScalePtr].X;
 E3d_ActScaleY=E3d_ScaleStack[E3d_MatrixScalePtr].Y;
 E3d_ActScaleZ=E3d_ScaleStack[E3d_MatrixScalePtr].Z;
 E3d_MatrixScalePtr++;
}
#endif // USEOpenGL
