//----------------------------------------------------------------------------
//	file: matrix.cpp
//	desc: implementation of gem_Matrix methods
//----------------------------------------------------------------------------
#include <math.h>
#include "directgem.h"
#include "gemmath.h"

gem_Matrix ZeroMtx()
{
	gem_Matrix		mat;

	ZeroMemory( &mat, sizeof(mat) );

	return			mat;
}


gem_Matrix IdentMtx()
{
	gem_Matrix		mat;

	ZeroMemory( &mat, sizeof(mat) );
	mat( 1, 1 ) = 1.0f;
	mat( 2, 2 ) = 1.0f;
	mat( 3, 3 ) = 1.0f;
	mat( 4, 4 ) = 1.0f;

	return mat;
}


gem_Matrix operator* (gem_Matrix& A, gem_Matrix& B)
{
	gem_Matrix		tmp;

	tmp(1,1)=A(1,1)*B(1,1) + A(1,2)*B(2,1) + A(1,3)*B(3,1) + A(1,4)*B(4,1);
	tmp(1,2)=A(1,1)*B(1,2) + A(1,2)*B(2,2) + A(1,3)*B(3,2) + A(1,4)*B(4,2);
	tmp(1,3)=A(1,1)*B(1,3) + A(1,2)*B(2,3) + A(1,3)*B(3,3) + A(1,4)*B(4,3);
	tmp(1,4)=A(1,1)*B(1,4) + A(1,2)*B(2,4) + A(1,3)*B(3,4) + A(1,4)*B(4,4);

	tmp(2,1)=A(2,1)*B(1,1) + A(2,2)*B(2,1) + A(2,3)*B(3,1) + A(2,4)*B(4,1);
	tmp(2,2)=A(2,1)*B(1,2) + A(2,2)*B(2,2) + A(2,3)*B(3,2) + A(2,4)*B(4,2);
	tmp(2,3)=A(2,1)*B(1,3) + A(2,2)*B(2,3) + A(2,3)*B(3,3) + A(2,4)*B(4,3);
	tmp(2,4)=A(2,1)*B(1,4) + A(2,2)*B(2,4) + A(2,3)*B(3,4) + A(2,4)*B(4,4);

	tmp(3,1)=A(3,1)*B(1,1) + A(3,2)*B(2,1) + A(3,3)*B(3,1) + A(3,4)*B(4,1);
	tmp(3,2)=A(3,1)*B(1,2) + A(3,2)*B(2,2) + A(3,3)*B(3,2) + A(3,4)*B(4,2);
	tmp(3,3)=A(3,1)*B(1,3) + A(3,2)*B(2,3) + A(3,3)*B(3,3) + A(3,4)*B(4,3);
	tmp(3,4)=A(3,1)*B(1,4) + A(3,2)*B(2,4) + A(3,3)*B(3,4) + A(3,4)*B(4,4);

	tmp(4,1)=A(4,1)*B(1,1) + A(4,2)*B(2,1) + A(4,3)*B(3,1) + A(4,4)*B(4,1);
	tmp(4,2)=A(4,1)*B(1,2) + A(4,2)*B(2,2) + A(4,3)*B(3,2) + A(4,4)*B(4,2);
	tmp(4,3)=A(4,1)*B(1,3) + A(4,2)*B(2,3) + A(4,3)*B(3,3) + A(4,4)*B(4,3);
	tmp(4,4)=A(4,1)*B(1,4) + A(4,2)*B(2,4) + A(4,3)*B(3,4) + A(4,4)*B(4,4);

	return tmp;
}

D3DVERTEX operator* ( D3DVERTEX& a, gem_Matrix& A )
{
	D3DVERTEX		tmp;
	FLOAT			len;

	tmp.tu = a.tu;
	tmp.tv = a.tv;

	tmp.x = a.x*A(1,1) + a.y*A(2,1) + a.z*A(3,1) + A(4,1);
	tmp.y = a.x*A(1,2) + a.y*A(2,2) + a.z*A(3,2) + A(4,2);
	tmp.z = a.x*A(1,3) + a.y*A(2,3) + a.z*A(3,3) + A(4,3);

	tmp.nx = a.nx*A(1,1) + a.ny*A(2,1) + a.nz*A(3,1);
	tmp.ny = a.nx*A(1,2) + a.ny*A(2,2) + a.nz*A(3,2);
	tmp.nz = a.nx*A(1,3) + a.ny*A(2,3) + a.nz*A(3,3);

	len = sqrt( tmp.nx*tmp.nx + tmp.ny*tmp.ny + tmp.nz*tmp.nz );

	if( len != 0 )
		len = 1.0/len;	

	tmp.nx = tmp.nx*len;
	tmp.ny = tmp.ny*len;
	tmp.nz = tmp.nz*len;	

	return tmp;
}

VOID MtxMulVertex( gem_Matrix& A, LPD3DVERTEX a, LPD3DVERTEX b )
{	
	FLOAT			len;

	b->x = a->x*A(1,1) + a->y*A(2,1) + a->z*A(3,1) + A(4,1);
	b->y = a->x*A(1,2) + a->y*A(2,2) + a->z*A(3,2) + A(4,2);
	b->z = a->x*A(1,3) + a->y*A(2,3) + a->z*A(3,3) + A(4,3);
	
	b->nx = a->nx*A(1,1) + a->ny*A(2,1) + a->nz*A(3,1);
	b->ny = a->nx*A(1,2) + a->ny*A(2,2) + a->nz*A(3,2);
	b->nz = a->nx*A(1,3) + a->ny*A(2,3) + a->nz*A(3,3);

	len = sqrt( b->nx*b->nx + b->ny*b->ny + b->nz*b->nz );

	if( len != 0 )
		len = 1.0/len;	

	b->nx *= len;
	b->ny *= len;
	b->nz *= len;

	// przepisanie wspolzednych tekstury
	b->tu = a->tu;
	b->tv = a->tv;
}

gem_Vector operator* (gem_Vector& a, gem_Matrix& A)
{
	gem_Vector		tmp;
	
	FLOAT			x = a.x*A(1,1) + a.y*A(2,1) + a.z*A(3,1) + A(4,1);
	FLOAT			y = a.x*A(1,2) + a.y*A(2,2) + a.z*A(3,2) + A(4,2);
	FLOAT			z = a.x*A(1,3) + a.y*A(2,3) + a.z*A(3,3) + A(4,3);
	FLOAT			w = a.x*A(1,4) + a.y*A(2,4) + a.z*A(3,4) + A(4,4);

	if ( !w )
		return gem_Vector( 0, 0, 0 );

	FLOAT		_1w = 1/w;

	tmp.x = x * _1w;
	tmp.y = y * _1w;
	tmp.z = z * _1w;

	return tmp;
}


gem_Matrix operator* (FLOAT a, gem_Matrix& A)
{
	gem_Matrix		tmp;

	for( int i = 1 ; i<=4 ; i++ )
		for (int j = 1 ; j<=4 ; j++ )
			tmp( i, j )= a*A( i, j );

	return tmp;
}

gem_Matrix operator* (gem_Matrix& A, FLOAT a)
{
	gem_Matrix		tmp;

	for( int i = 1 ; i<=4 ; i++ )
		for (int j = 1 ; j<=4 ; j++ )
			tmp( i, j )= a*A( i, j );

	return tmp;
}


gem_Matrix operator+ (gem_Matrix& A, gem_Matrix& B)
{
	gem_Matrix		tmp;

	for( int i = 1 ; i<=4 ; i++ )
		for( int j = 1 ; j<=4; j++ )
			tmp( i, j ) = A( i, j ) + B( i, j );

	return tmp;
}

gem_Matrix operator- (gem_Matrix& A)
{
	gem_Matrix		tmp;

	for( int i = 1 ; i<=4 ; i++ )
		for( int j = 1 ; j<=4; j++ )
			tmp( i, j ) = -A( i, j );

	return tmp;
}

gem_Matrix operator- (gem_Matrix& A, gem_Matrix& B)
{
	gem_Matrix		tmp;

	for( int i = 1 ; i<=4 ; i++ )
		for( int j = 1 ; j<=4; j++ )
			tmp( i, j ) = A( i, j ) - B( i, j );

	return tmp;
}


gem_Matrix Transpose(gem_Matrix& A)
{
	gem_Matrix		tmp;

	for( int i=1 ; i<=4; i++ )
		for( int j=0 ; j<=4 ; j++ )
			tmp( j, i ) = A( i, j );

	return tmp;
}

gem_Matrix Invert(gem_Matrix& a)
{
	gem_Matrix			q;

    FLOAT fDetInv = 1.0f / ( a(1,1)*( a(2,2)*a(3,3) - a(2,3)*a(3,2) ) -
                             a(1,2)*( a(2,1)*a(3,3) - a(2,3)*a(3,1) ) +
                             a(1,3)*( a(2,1)*a(3,2) - a(2,2)*a(3,1) ) );

    q(1,1) =  fDetInv*( a(2,2)*a(3,3) - a(2,3)*a(3,2) );
    q(1,2) = -fDetInv*( a(1,2)*a(3,3) - a(1,3)*a(3,2) );
    q(1,3) =  fDetInv*( a(1,2)*a(2,3) - a(1,3)*a(2,2) );
    q(1,4) = 0.0f;

    q(2,1) = -fDetInv*( a(2,1)*a(3,3) - a(2,3)*a(3,1) );
    q(2,2) =  fDetInv*( a(1,1)*a(3,3) - a(1,3)*a(3,1) );
    q(2,3) = -fDetInv*( a(1,1)*a(2,3) - a(1,3)*a(2,1) );
    q(2,4) = 0.0f;

    q(3,1) =  fDetInv*( a(2,1)*a(3,2) - a(2,2)*a(3,1) );
    q(3,2) = -fDetInv*( a(1,1)*a(3,2) - a(1,2)*a(3,1) );
    q(3,3) =  fDetInv*( a(1,1)*a(2,2) - a(1,2)*a(2,1) );
    q(3,4) = 0.0f;

    q(4,1) = -( a(4,1)*q(1,1) + a(4,2)*q(2,1) + a(4,3)*q(3,1) );
    q(4,2) = -( a(4,1)*q(1,2) + a(4,2)*q(2,2) + a(4,3)*q(3,2) );
    q(4,3) = -( a(4,1)*q(1,3) + a(4,2)*q(2,3) + a(4,3)*q(3,3) );
    q(4,4) = 1.0f;

    return q;
}


D3DMATRIX GetD3DMATRIX(gem_Matrix& mat)
{
	D3DMATRIX		d3dmat;
	
	d3dmat._11 = mat(1,1); d3dmat._12 = mat(1,2); d3dmat._13 = mat(1,3); d3dmat._14 = mat(1,4);
	d3dmat._21 = mat(2,1); d3dmat._22 = mat(2,2); d3dmat._23 = mat(2,3); d3dmat._24 = mat(2,4);
	d3dmat._31 = mat(3,1); d3dmat._32 = mat(3,2); d3dmat._33 = mat(3,3); d3dmat._34 = mat(3,4);
	d3dmat._41 = mat(4,1); d3dmat._42 = mat(4,2); d3dmat._43 = mat(4,3); d3dmat._44 = mat(4,4);

	return d3dmat;
}

gem_Matrix::operator D3DMATRIX ()
{
	D3DMATRIX		d3dmat;
	
	d3dmat._11 = MATRIX[0][0]; 
	d3dmat._12 = MATRIX[0][1]; 
	d3dmat._13 = MATRIX[0][2];
	d3dmat._14 = MATRIX[0][3];

	d3dmat._21 = MATRIX[1][0]; 
	d3dmat._22 = MATRIX[1][1]; 
	d3dmat._23 = MATRIX[1][2];  
	d3dmat._24 = MATRIX[1][3]; 

	d3dmat._31 = MATRIX[2][0]; 
	d3dmat._32 = MATRIX[2][1]; 
	d3dmat._33 = MATRIX[2][2]; 
	d3dmat._34 = MATRIX[2][3]; 

	d3dmat._41 = MATRIX[3][0]; 
	d3dmat._42 = MATRIX[3][1]; 
	d3dmat._43 = MATRIX[3][2]; 
	d3dmat._44 = MATRIX[3][3]; 
	
	return d3dmat;
}

gem_Matrix TranslationMtx(gem_Vector& vTrans)
{
	gem_Matrix		mat = IdentMtx();

	mat( 4, 1 ) = vTrans.x;
	mat( 4, 2 ) = vTrans.y;
	mat( 4, 3 ) = vTrans.z;

	return mat;
}

gem_Matrix TranslationMtx( FLOAT x, FLOAT y, FLOAT z)
{
	gem_Matrix		mat = IdentMtx();

	mat( 4, 1 ) = x;
	mat( 4, 2 ) = y;
	mat( 4, 3 ) = z;

	return mat;
}


gem_Matrix ScaleMtx( gem_Vector& vScale )
{
	gem_Matrix		mat = IdentMtx();

	mat( 1, 1 ) = 1/vScale.x;
	mat( 2, 2 ) = 1/vScale.y;
	mat( 3, 3 ) = 1/vScale.z;

	return mat;
}

gem_Matrix ScaleMtx( FLOAT dx, FLOAT dy, FLOAT dz )
{
	gem_Matrix		mat = IdentMtx();

	mat( 1, 1 ) = dx;
	mat( 2, 2 ) = dy;
	mat( 3, 3 ) = dz;

	return mat;
}

gem_Matrix ScaleMtx( FLOAT delta )
{
	gem_Matrix		mat = IdentMtx();

	mat( 1, 1 ) = delta;
	mat( 2, 2 ) = delta;
	mat( 3, 3 ) = delta;

	return mat;
}

gem_Matrix RotationMtx(gem_Vector& vAxis, FLOAT theta)
{
	gem_Matrix		tmp		= IdentMtx();

	FLOAT		cost	= (FLOAT)cos(theta);
	FLOAT		_1cost	= 1-cost;
	FLOAT		sint	= (FLOAT)sin(theta);
	FLOAT		x		= vAxis.x;
	FLOAT		y		= vAxis.y;
	FLOAT		z		= vAxis.z;
	FLOAT		x2		= x*x;
	FLOAT		y2		= y*y;
	FLOAT		z2		= z*z;
	FLOAT		xy		= x*y;
	FLOAT		xz		= x*z;
	FLOAT		yz		= y*z;

	tmp( 1, 1 ) = x2 + cost*(1 - x2);
	tmp( 1, 2 ) = xy*_1cost + z*sint;
	tmp( 1, 3 ) = xz*_1cost - y*sint;

	tmp( 2, 1 ) = xy*_1cost - z*sint;
	tmp( 2, 2 ) = y2 + cost*(1 - y2);
	tmp( 2, 3 ) = yz*_1cost + x*sint;

	tmp( 3, 1 ) = xz*_1cost - y*sint;
	tmp( 3, 2 ) = yz*_1cost - x*sint;	
	tmp( 3, 3 ) = z2 + cost*(1 - z2);
    
	return tmp;
}

gem_Matrix RotationMtx( FLOAT alpha, FLOAT beta, FLOAT gamma )
{
	gem_Matrix		tmp = IdentMtx();

	FLOAT			cosg = cos( gamma ), sing = sin( gamma );
	FLOAT			cosb = cos( beta ),  sinb = sin( beta );
	FLOAT			cosa = cos( alpha ), sina = sin( alpha );

	tmp( 1, 1 ) = sina*sinb*sing + cosa*cosg;
	tmp( 2, 1 ) = cosb*sing;
	tmp( 3, 1 ) = sina*cosg - cosa*sinb*sing;

	tmp( 1, 2 ) = sina*sinb*cosg - cosa*sing;
	tmp( 2, 2 ) = cosb*cosg;
	tmp( 3, 2 ) = -cosa*sinb*cosg - sina*sing;

    tmp( 1, 3 ) = -sina*cosb;
	tmp( 2, 3 ) = sinb;
	tmp( 3, 3 ) = cosa*cosb;	

    return tmp;
}

gem_Matrix ViewMtx(gem_Vector& CamPos, gem_Vector& CamTrg)
{
    
    gem_Vector		vView = CamTrg - CamPos;
	gem_Vector		vWorldUp( 0.0f, 1.0f, 0.0f );

    vView = Normalize( vView );

    FLOAT			DotProd = Dot( vWorldUp, vView );
    gem_Vector		vUp = vWorldUp - DotProd * vView;

    vUp = Normalize( vUp );

    gem_Vector		vRight = Cross( vUp, vView );
    
	gem_Matrix		mat = IdentMtx();
    mat(1,1) = vRight.x;    mat(1,2) = vUp.x;    mat(1,3) = vView.x;
    mat(2,1) = vRight.y;    mat(2,2) = vUp.y;    mat(2,3) = vView.y;
    mat(3,1) = vRight.z;    mat(3,2) = vUp.z;    mat(3,3) = vView.z;

    
    mat(4,1) = - Dot( CamPos, vRight );
    mat(4,2) = - Dot( CamPos, vUp );
    mat(4,3) = - Dot( CamPos, vView );

    return mat;
}

gem_Matrix ProjectionMtx(FLOAT fov, FLOAT nearZ, FLOAT farZ, FLOAT aspect)
{
	FLOAT			ang = 0.5f*fov*3.14159f/180.0f;
	FLOAT			_cos = (FLOAT)cos( ang );
	FLOAT			_sin = (FLOAT)sin( ang );
	FLOAT			_ctg = _cos/_sin;

	FLOAT			w = aspect * _ctg;
	FLOAT			h = _ctg;  
    FLOAT			Q = farZ / ( farZ - nearZ );

    gem_Matrix		mat = ZeroMtx();
    mat(1,1) = w;
    mat(2,2) = h;
    mat(3,3) = Q;
    mat(3,4) = 1.0f;
    mat(4,3) = -Q*nearZ;

	return mat;
}


gem_Matrix CameraMtx(gem_Vector& CamPos, gem_Vector& CamTrg, FLOAT roll)
{
	gem_Vector		dir = CamTrg-CamPos;
	FLOAT			focus = Length( dir );

	FLOAT			alpha = -atan2(dir.x,dir.z);
	FLOAT			beta  = asin(dir.y/focus);
	FLOAT			gamma = -roll*3.14159f/180.0f;

	FLOAT			sina = (FLOAT)sin(alpha); 
	FLOAT			cosa = (FLOAT)cos(alpha);
	FLOAT			sinb = (FLOAT)sin(beta);  
	FLOAT			cosb = (FLOAT)cos(beta);
	FLOAT			sing = (FLOAT)sin(gamma); 
	FLOAT			cosg = (FLOAT)cos(gamma);

	gem_Matrix		mtx = IdentMtx();

	mtx(1,1) = sina*sinb*sing + cosa*cosg;
	mtx(2,1) = cosb*sing;
	mtx(3,1) = sina*cosg - cosa*sinb*sing;

	mtx(1,2) = sina*sinb*cosg - cosa*sing;
	mtx(2,2) = cosb*cosg;
    mtx(3,2) = -cosa*sinb*cosg - sina*sing;

	mtx(1,3) = -sina*cosb;
    mtx(2,3) = sinb;
    mtx(3,3) = cosa*cosb;

	mtx = TranslationMtx(-CamPos)*mtx;

    return mtx;
}

gem_Matrix InvCameraMtx(gem_Vector& CamPos, gem_Vector& CamTrg, FLOAT roll)
{
	gem_Vector		dir = CamTrg-CamPos;
	FLOAT			focus = Length( dir );

	FLOAT			alpha = atan2(dir.x,dir.z);
	FLOAT			beta  = -asin(dir.y/focus);
	FLOAT			gamma = roll*3.14159f/180.0f;

	FLOAT			sina = -(FLOAT)sin(alpha); 
	FLOAT			cosa = (FLOAT)cos(alpha);
	FLOAT			sinb = -(FLOAT)sin(beta);  
	FLOAT			cosb = (FLOAT)cos(beta);
	FLOAT			sing = -(FLOAT)sin(gamma); 
	FLOAT			cosg = (FLOAT)cos(gamma);

	gem_Matrix		mtx = IdentMtx();

	mtx(1,1) = sina*sinb*sing + cosa*cosg;
	mtx(2,1) = cosb*sing;
	mtx(3,1) = sina*cosg - cosa*sinb*sing;

	mtx(1,2) = sina*sinb*cosg - cosa*sing;
	mtx(2,2) = cosb*cosg;
    mtx(3,2) = -cosa*sinb*cosg - sina*sing;

	mtx(1,3) = -sina*cosb;
    mtx(2,3) = sinb;
    mtx(3,3) = cosa*cosb;	

    return Invert( mtx );
}



void SwapMatrix( gem_Matrix& A )
{
	FLOAT			tmp;
	WORD			i;

	for( i = 1 ; i<=4 ; i++ )
	{
		tmp		= A(i,2);
		A(i,2)	= A(i,3);
		A(i,3)	= tmp;
	}

	for( i = 1 ; i<=4 ; i++)
	{
		tmp		= A(2,i);
		A(2,i)	= A(3,i);
		A(3,i)	= tmp;
	}
}
