//-------------------------------------------------------------------------------------------------------+
// Copyright (C), 1998-2007, Beijing Tangent Software Co. Ltd.
// = FileName	:   TGMatrix 类
// = Version	:   ver2.0
// = Author		:   wlw
// = CreateDate	:   2002-09-09
// = Description:   TGMatrix 定义
// = Maintainers: 
//                  
//-------------------------------------------------------------------------------------------------------+
#include "StdAfx.h"
#include "TGMatrix.h"

TGMatrix::TGMatrix()
{
	InitIdentity();
}

TGMatrix::~TGMatrix()
{

}

double* TGMatrix::operator [](int i) const
{
	return (double*)(matrix[i]);
}

bool TGMatrix::CanReverse() const
{
	double temp = Rank();
	return !EQUAL_ZERO(temp);
}

void TGMatrix::GetCoordSys(TPGeCoordSys&sys) const
{
	sys.Reset();
	sys.TransformBy(this);
}

void TGMatrix::Init()
{
	memset((void*)matrix,0,sizeof(double)*16);
	matrix[3][3] = 1.0;
}

void TGMatrix::InitIdentity()
{
	Init();matrix[0][0] = 1.0;
	matrix[1][1] = 1.0;
	matrix[2][2] = 1.0;
}

TGMatrix::TGMatrix(const TGMatrix&m2)
{
	memcpy(matrix,m2[0],sizeof(double)*16);
}

TGMatrix& TGMatrix::operator = (const TGMatrix&m2)
{
	memcpy(matrix,m2[0],sizeof(double)*16);
	return *this;
}

TGMatrix& TGMatrix::operator  = (const float*m)
{
	for (int i = 0;i < 4;i++)
	{
		for (int j = 0;j < 4;j++)
		{
			matrix[i][j] = m[i*4+j];
		}
	}
	return *this;
}

void TGMatrix::Export(float*m) const
{
	for (int i = 0;i < 4;i++)
	{
		for (int j = 0;j < 4;j++)
		{
			m[i*4+j] = (float)(matrix[i][j]);
		}
	}
}

void TGMatrix::Transform3D(TGPoint&point) const
{
	double a = matrix[0][0]*point.x+matrix[1][0]*point.y+matrix[2][0]*point.z+matrix[3][0];
	double b = matrix[0][1]*point.x+matrix[1][1]*point.y+matrix[2][1]*point.z+matrix[3][1];
	double c = matrix[0][2]*point.x+matrix[1][2]*point.y+matrix[2][2]*point.z+matrix[3][2];
	double w = matrix[0][3]*point.x+matrix[1][3]*point.y+matrix[2][3]*point.z+matrix[3][3];
	point.x = a/w;
	point.y = b/w;
	point.z = c/w;
}

void TGMatrix::Transform2D(TGPoint&point) const
{
	double a = matrix[0][0]*point.x+matrix[1][0]*point.y+matrix[3][0];
	double b = matrix[0][1]*point.x+matrix[1][1]*point.y+matrix[3][1];
	double w = matrix[0][3]*point.x+matrix[1][3]*point.y+matrix[3][3];
	point.x = a/w;
	point.y = b/w;
}

void TGMatrix::Transform(TGVector3D&vt) const
{
	double a = matrix[0][0]*vt.x+matrix[1][0]*vt.y+matrix[2][0]*vt.z;
	double b = matrix[0][1]*vt.x+matrix[1][1]*vt.y+matrix[2][1]*vt.z;
	double c = matrix[0][2]*vt.x+matrix[1][2]*vt.y+matrix[2][2]*vt.z;
	double w = matrix[0][3]*vt.x+matrix[1][3]*vt.y+matrix[2][3]*vt.z+matrix[3][3];
	vt.x = a/w;
	vt.y = b/w;
	vt.z = c/w;
}

void TGMatrix::Transform(TGVector2D&vt) const
{
	double a = matrix[0][0]*vt.x+matrix[1][0]*vt.y;
	double b = matrix[0][1]*vt.x+matrix[1][1]*vt.y;
	double w = matrix[0][3]*vt.x+matrix[1][3]*vt.y+matrix[3][3];
	vt.x = a/w;
	vt.y = b/w;
}

TGMatrix& TGMatrix::operator  *=  (const TGMatrix &m2)
{
	GTransformMatrix r;
	for (int i = 0;i < 4;i++)
	{
		for (int j = 0;j < 4;j++)
		{
			r[i][j] = 0.0;
			for (int k = 0;k < 4;k++)
			{
				r[i][j] += matrix[i][k]*m2[k][j];
			}
		}
	}
	memcpy(matrix,r,sizeof(double)*16);
	return *this;
}

TGMatrix TGMatrix::operator *(const TGMatrix&m2) const
{
	TGMatrix r;
	for (int i = 0;i < 4;i++)
	{
		for (int j = 0;j < 4;j++)
		{
			r[i][j] = 0.0;
			for (int k = 0;k < 4;k++)
			{
				r[i][j] += matrix[i][k]*m2[k][j];
			}
		}
	}
	return r;
}

bool TGMatrix::Reverse()
{
	double r = Rank();
	if (EQUAL_ZERO(r))
	{
		return false;
	}
	
	TGMatrix m(*this);
	double row[4];
	for (long j = 0;j < 4;j++)
	{
		memcpy(row,m[j],4*sizeof(double));
		memset(m[j],0,4*sizeof(double));
		for (long i = 0;i < 4;i++)
		{
			m[j][i] = 1;
			(*this)[i][j] = m.Rank()/r;
			m[j][i] = 0;
		}
		memcpy(m[j],row,4*sizeof(double));
	}
	return true;
}

double TGMatrix::Rank() const
{
	const TGMatrix& m = *this;
	double r = 0;
	r += m[0][0]*m[1][1]*m[2][2]*m[3][3];
	r -= m[0][0]*m[1][1]*m[2][3]*m[3][2];
	r -= m[0][0]*m[1][2]*m[2][1]*m[3][3];
	r += m[0][0]*m[1][2]*m[2][3]*m[3][1];
	r -= m[0][0]*m[1][3]*m[2][2]*m[3][1];
	r += m[0][0]*m[1][3]*m[2][1]*m[3][2];

	r -= m[0][1]*m[1][0]*m[2][2]*m[3][3];
	r += m[0][1]*m[1][0]*m[2][3]*m[3][2];
	r += m[0][1]*m[1][2]*m[2][0]*m[3][3];
	r -= m[0][1]*m[1][2]*m[2][3]*m[3][0];
	r -= m[0][1]*m[1][3]*m[2][0]*m[3][2];
	r += m[0][1]*m[1][3]*m[2][2]*m[3][0];

	r += m[0][2]*m[1][0]*m[2][1]*m[3][3];
	r -= m[0][2]*m[1][0]*m[2][3]*m[3][1];
	r -= m[0][2]*m[1][1]*m[2][0]*m[3][3];
	r += m[0][2]*m[1][1]*m[2][3]*m[3][0];
	r += m[0][2]*m[1][3]*m[2][0]*m[3][1];
	r -= m[0][2]*m[1][3]*m[2][1]*m[3][0];

	r -= m[0][3]*m[1][0]*m[2][1]*m[3][2];
	r += m[0][3]*m[1][0]*m[2][2]*m[3][1];
	r += m[0][3]*m[1][1]*m[2][0]*m[3][2];
	r -= m[0][3]*m[1][1]*m[2][2]*m[3][0];
	r -= m[0][3]*m[1][2]*m[2][0]*m[3][1];
	r += m[0][3]*m[1][2]*m[2][1]*m[3][0];

	return r;
}

void TGMatrix::Swap()
{
	Exchange((*this)[0][1],(*this)[1][0]);
	Exchange((*this)[0][2],(*this)[2][0]);
	Exchange((*this)[0][3],(*this)[3][0]);
	Exchange((*this)[1][2],(*this)[2][1]);
	Exchange((*this)[1][3],(*this)[3][1]);
	Exchange((*this)[2][3],(*this)[3][2]);
}

void TGMatrix::CreateMoveMatrix(double cx,double cy,double cz)
{
	matrix[3][0] = cx;
	matrix[3][1] = cy;
	matrix[3][2] = cz;
}

void TGMatrix::CreateMoveMatrix(double cx,double cy)
{
	matrix[3][0] = cx;
	matrix[3][1] = cy;
}

void TGMatrix::CreateMoveMatrix(const TGVector3D&vt)
{
	matrix[3][0] = vt.x;
	matrix[3][1] = vt.y;
	matrix[3][2] = vt.z;
}

void TGMatrix::CreateMoveMatrix(const TGVector2D&vt)
{
	matrix[3][0] = vt.x;
	matrix[3][1] = vt.y;
}

void TGMatrix::CreateScaleMatrix(double sx,double sy,double sz)
{
	matrix[0][0] = sx;
	matrix[1][1] = sy;
	matrix[2][2] = sz;
}

void TGMatrix::CreateScaleMatrix(double sx,double sy)
{
	matrix[0][0] = sx;
	matrix[1][1] = sy;
}

void TGMatrix::CreateRotateMatrix(double angle,const TGPoint&basePoint)
{
	double C = cos(angle),S = sin(angle);
	matrix[0][0] = C;
	matrix[0][1] = S;
	matrix[1][0] = -S;
	matrix[1][1] = C;
	matrix[3][0] = (1-C)*basePoint.x+basePoint.y*S;
	matrix[3][1] = (1-C)*basePoint.y-basePoint.x*S;
}

void TGMatrix::CreateScaleMatrix(double sx,double sy,double sz,const TGPoint&basePoint)
{
	matrix[0][0] = sx;
	matrix[1][1] = sy;
	matrix[2][2] = sz;
	matrix[3][0] = (1-sx)*basePoint.x;
	matrix[3][1] = (1-sy)*basePoint.y;
	matrix[3][2] = (1-sz)*basePoint.z;
}

void TGMatrix::CreateScaleMatrix(double sx,double sy,const TGPoint&basePoint)
{
	matrix[0][0] = sx;
	matrix[1][1] = sy;
	matrix[3][0] = (1-sx)*basePoint.x;
	matrix[3][1] = (1-sy)*basePoint.y;
}

void TGMatrix::CreateXRotateMatrix(double angle)
{
	matrix[1][1] = cos(angle);
	matrix[2][2] = matrix[1][1];
	matrix[1][2] = sin(angle);
	matrix[2][1] = -matrix[1][2];
}

void TGMatrix::CreateYRotateMatrix(double angle)
{
	matrix[0][0] = cos(angle);
	matrix[2][2] = matrix[0][0];
	matrix[2][0] = sin(angle);
	matrix[0][2] = -matrix[2][0];
}

void TGMatrix::CreateZRotateMatrix(double angle)
{
	matrix[0][0] = cos(angle);
	matrix[1][1] = matrix[0][0];
	matrix[0][1] = sin(angle);
	matrix[1][0] = -matrix[0][1];
}

void TGMatrix::CreateRotateMatrix(double angle,const TPGeLine3D&axis)
{
	TGVector3D vt(axis);
	if (!vt.Normalize())
	{
		return;
	}

	double a = vt.x,b = vt.y,c = vt.z;
	double x = axis.basePoint.x,y = axis.basePoint.y,z = axis.basePoint.z;
	double v = sqrt(c*c+b*b);
	TGMatrix &taMat = *this,uaMat,rxMat,uxMat,ryMat,uyMat,rzmat;

	taMat[3][0] = -x;
	taMat[3][1] = -y;
	taMat[3][2] = -z;

	uaMat[3][0] = x;
	uaMat[3][1] = y;
	uaMat[3][2] = z;

	rxMat[1][1] = rxMat[2][2] = c/v;
	rxMat[1][2] = b/v;
	rxMat[2][1] = -rxMat[1][2];

	uxMat[1][1] = uxMat[2][2] = c/v;
	uxMat[1][2] = -b/v;
	uxMat[2][1] = -uxMat[1][2];

	ryMat[0][0] = ryMat[2][2] = v;
	ryMat[0][2] = a;
	ryMat[2][0] = -a;

	uyMat[0][0] = uyMat[2][2] = v;
	uyMat[0][2] = -a;
	uyMat[2][0] = a;

	rzmat[0][0] = rzmat[1][1] = cos(angle);
	rzmat[0][1] = sin(angle);
	rzmat[1][0] = -rzmat[0][1];

	taMat *= rxMat;
	taMat *= ryMat;
	taMat *= rzmat;
	taMat *= uyMat;
	taMat *= uxMat;
	taMat *= uaMat;
}

bool TGMatrix::CreateXYProjectMatrix(const TGPoint&pt,bool bParallel)
{
	if (EQUAL_ZEROD(pt.z))
	{
		return false;
	}

	CreateXYProjectMatrix();
	matrix[2][0] = -pt.x/pt.z;
	matrix[2][1] = -pt.y/pt.z;
	if (!bParallel)
	{
		matrix[2][3] = -1/pt.z;
	}

	return true;
}

bool TGMatrix::CreateProjectMatrix(const TPGePlane&plane,const TGPoint&pt,bool bParallel)
{
	if (bParallel)
	{
		double A = plane.a;
		double B = plane.b;
		double C = plane.c;
		double D = plane.d;
		TGVector3D vt = pt;
		vt.Normalize();
		double dot = A*vt[0]+B*vt[1]+C*vt[2];
		if (EQUAL_ZERO(dot))
		{
			return false;
		}
		else
		{
			matrix[0][0] = 1.0-vt[0]*A/dot;

			matrix[1][0] = -vt[0]*B/dot;
			matrix[2][0] = -vt[0]*C/dot;
			matrix[3][0] = -vt[0]*D/dot;

			matrix[0][1] = -vt[1]*A/dot;

			matrix[1][1] = 1.0-vt[1]*B/dot;
			matrix[2][1] = -vt[1]*C/dot;
			matrix[3][1] = -vt[1]*D/dot;

			matrix[0][2] = -vt[2]*A/dot;

			matrix[1][2] = -vt[2]*B/dot;
			matrix[2][2] = 1.0-vt[2]*C/dot;
			matrix[3][2] = -vt[2]*D/dot;

			return true;
		}
	}
	else
	{
		if (plane.HitTest(pt).Is(GR_IN))
		{
			return false;
		}

		TGPoint pt2(pt);
		TGMatrix mPlane,mProject;
		mPlane.SetCoordSys(plane);
		(*this) *= mPlane;
		mPlane.Transform3D(pt2);
		mProject.CreateXYProjectMatrix(pt2,bParallel);
		(*this) *= mProject;
		mPlane.Reverse();
		(*this) *= mPlane;
		return true;
	}
}