//
//	Copyright © 2005 Nemetschek North America 
//	All Rights Reserved.
//
// Created by Vladislav Stanev

#include "StdAfx.h"
//#include "VWTransformMatrix.h"

using namespace VWFC::Math;

VWTransformMatrix::VWTransformMatrix()
{
	this->SetIdentity();
}

VWTransformMatrix::VWTransformMatrix(const VWPoint3D& uVec, const VWPoint3D& vVec, const VWPoint3D& wVec, bool bTransponse /*=false*/)
{
	this->SetMatrix( uVec, vVec, wVec, bTransponse );
}

VWTransformMatrix::VWTransformMatrix(const VWPoint3D& uVec, const VWPoint3D& vVec, const VWPoint3D& wVec, const VWPoint3D& offset, bool bTransponse /*=false*/)
{
	this->SetMatrix( uVec, vVec, wVec, offset, bTransponse );
}

VWTransformMatrix::VWTransformMatrix(const VWPoint3D& offset)
{
	this->SetMatrix( offset );
}

VWTransformMatrix::VWTransformMatrix(double scaleX, double scaleY, double scaleZ)
{
	this->SetMatrix( scaleX, scaleY, scaleZ );
}

VWTransformMatrix::VWTransformMatrix(const TransformMatrix& mat)
{
	m[0][0] = mat.mat[0][0]; m[0][1] = mat.mat[0][1]; m[0][2] = mat.mat[0][2];
	m[1][0] = mat.mat[1][0]; m[1][1] = mat.mat[1][1]; m[1][2] = mat.mat[1][2];
	m[2][0] = mat.mat[2][0]; m[2][1] = mat.mat[2][1]; m[2][2] = mat.mat[2][2];
	m[3][0] = mat.mat[3][0]; m[3][1] = mat.mat[3][1]; m[3][2] = mat.mat[3][2];
}

VWTransformMatrix::VWTransformMatrix(const VWTransformMatrix& mat)
{
	m[0][0] = mat.m[0][0]; m[0][1] = mat.m[0][1]; m[0][2] = mat.m[0][2];
	m[1][0] = mat.m[1][0]; m[1][1] = mat.m[1][1]; m[1][2] = mat.m[1][2];
	m[2][0] = mat.m[2][0]; m[2][1] = mat.m[2][1]; m[2][2] = mat.m[2][2];
	m[3][0] = mat.m[3][0]; m[3][1] = mat.m[3][1]; m[3][2] = mat.m[3][2];
}

VWTransformMatrix::~VWTransformMatrix()
{
}

VWTransformMatrix& VWTransformMatrix::operator=(const VWTransformMatrix& mat)
{
	m[0][0] = mat.m[0][0]; m[0][1] = mat.m[0][1]; m[0][2] = mat.m[0][2];
	m[1][0] = mat.m[1][0]; m[1][1] = mat.m[1][1]; m[1][2] = mat.m[1][2];
	m[2][0] = mat.m[2][0]; m[2][1] = mat.m[2][1]; m[2][2] = mat.m[2][2];
	m[3][0] = mat.m[3][0]; m[3][1] = mat.m[3][1]; m[3][2] = mat.m[3][2];
	return *this;
}

bool VWTransformMatrix::operator==(const VWTransformMatrix& mat) const
{
	return this->IsEqual( mat );
}

void VWTransformMatrix::SetIdentity()
{
	m[0][0] = 1; m[0][1] = 0; m[0][2] = 0;
	m[1][0] = 0; m[1][1] = 1; m[1][2] = 0;
	m[2][0] = 0; m[2][1] = 0; m[2][2] = 1;
	m[3][0] = 0; m[3][1] = 0; m[3][2] = 0;
}

bool VWTransformMatrix::IsIdentity() const
{
#define EQUAL(a,b)	(Abs(a-b)<0.0001)
	return
		EQUAL( m[0][0], 1 ) && EQUAL( m[0][1], 0 ) && EQUAL( m[0][2], 0 ) &&
		EQUAL( m[1][0], 0 ) && EQUAL( m[1][1], 1 ) && EQUAL( m[1][2], 0 ) &&
		EQUAL( m[2][0], 0 ) && EQUAL( m[2][1], 0 ) && EQUAL( m[2][2], 1 ) &&
		EQUAL( m[3][0], 0 ) && EQUAL( m[3][1], 0 ) && EQUAL( m[3][2], 0 );
#undef EQUAL
}

bool VWTransformMatrix::IsEqual(const VWTransformMatrix& mat) const
{
#define EQUAL(a,b)	(Abs(a-b)<0.0001)
	return
		EQUAL( m[0][0], mat.m[0][0] ) && EQUAL( m[0][1], mat.m[0][1] ) && EQUAL( m[0][2], mat.m[0][2] ) &&
		EQUAL( m[1][0], mat.m[1][0] ) && EQUAL( m[1][1], mat.m[1][1] ) && EQUAL( m[1][2], mat.m[1][2] ) &&
		EQUAL( m[2][0], mat.m[2][0] ) && EQUAL( m[2][1], mat.m[2][1] ) && EQUAL( m[2][2], mat.m[2][2] ) &&
		EQUAL( m[3][0], mat.m[3][0] ) && EQUAL( m[3][1], mat.m[3][1] ) && EQUAL( m[3][2], mat.m[3][2] );
#undef EQUAL
}

void VWTransformMatrix::SetRotation(double angleDeg, VWPoint3D axisRot)
{
	axisRot.Normalize();
	double dAng	= angleDeg * kRadiansPerDegree;
	double dSin = sin( dAng * 0.5 );
	
	double dA = axisRot.x * dSin;
	double dM = axisRot.y * dSin;
	double dN = axisRot.z * dSin;
	double dR = cos( dAng * 0.5 );

	double sqr_A = dA * dA;
	double sqr_M = dM * dM;
	double sqr_N = dN * dN;
	double sqr_R = dR * dR;

	double dNR = dN * dR;
	double dMR = dM * dR;
	double dAR = dA * dR;

	this->SetIdentity();

	m[0][0] = sqr_A - sqr_M - sqr_N + sqr_R;
	m[1][1] = sqr_M - sqr_A - sqr_N + sqr_R;
	m[2][2] = sqr_N - sqr_M - sqr_A + sqr_R;

	m[0][1] = m[1][0] = dA * dM;
	m[0][2] = m[2][0] = dA * dN;
	m[1][2] = m[2][1] = dN * dM;

	m[0][1] = 2.0 * ( m[0][1] - dNR );
	m[1][0] = 2.0 * ( m[1][0] + dNR );
	m[0][2] = 2.0 * ( m[0][2] + dMR );
	m[2][0] = 2.0 * ( m[2][0] - dMR );
	m[1][2] = 2.0 * ( m[1][2] - dAR );
	m[2][1] = 2.0 * ( m[2][1] + dAR );

	// inverted variant
	/*m[0][0] = sqr_A - sqr_M - sqr_N + sqr_R;
	m[1][1] = sqr_M - sqr_A - sqr_N + sqr_R;
	m[2][2] = sqr_N - sqr_M - sqr_A + sqr_R;

	m[1][0] = m[0][1] = dA * dM;
	m[2][0] = m[0][2] = dA * dN;
	m[2][1] = m[1][2] = dN * dM;

	m[1][0] = 2.0 * ( m[1][0] - dNR );
	m[0][1] = 2.0 * ( m[0][1] + dNR );
	m[2][0] = 2.0 * ( m[2][0] + dMR );
	m[0][2] = 2.0 * ( m[0][2] - dMR );
	m[2][1] = 2.0 * ( m[2][1] - dAR );
	m[1][2] = 2.0 * ( m[1][2] + dAR );*/
}

void VWTransformMatrix::SetTranslation(double x, double y, double z)
{
	m[3][0]		= x;
	m[3][1]		= y;
	m[3][2]		= z;
}

void VWTransformMatrix::SetTranslation(const VWPoint3D& pt)
{
	m[3][0]		= pt.x;
	m[3][1]		= pt.y;
	m[3][2]		= pt.z;
}

void VWTransformMatrix::SetMatrix(const VWPoint3D& uVec, const VWPoint3D& vVec, const VWPoint3D& wVec, bool bTransponse /*=false*/)
{
	if ( ! bTransponse ) {
		m[0][0] = uVec.x;	m[0][1] = uVec.y;	m[0][2] = uVec.z;	//	- u(x, y, z) 
		m[1][0] = vVec.x;	m[1][1] = vVec.y;	m[1][2] = vVec.z;	//	- v(x, y, z)
		m[2][0] = wVec.x;	m[2][1] = wVec.y;	m[2][2] = wVec.z;	//	- w(x, y, z)
	}
	else {
		m[0][0] = uVec.x;	m[0][1] = vVec.x;	m[0][2] = wVec.x;	//	- u(x, y, z) 
		m[1][0] = uVec.y;	m[1][1] = vVec.y;	m[1][2] = wVec.y;	//	- v(x, y, z)
		m[2][0] = uVec.z;	m[2][1] = vVec.z;	m[2][2] = wVec.z;	//	- w(x, y, z)
	}

	m[3][0] = 0;		m[3][1] = 0;		m[3][2] = 0;		//  - offset(x,y,z)
}

void VWTransformMatrix::SetMatrix(const VWPoint3D& uVec, const VWPoint3D& vVec, const VWPoint3D& wVec, const VWPoint3D& offset, bool bTransponse /*=false*/)
{
	if ( ! bTransponse ) {
		m[0][0] = uVec.x;	m[0][1] = uVec.y;	m[0][2] = uVec.z;	//	- u(x, y, z) 
		m[1][0] = vVec.x;	m[1][1] = vVec.y;	m[1][2] = vVec.z;	//	- v(x, y, z)
		m[2][0] = wVec.x;	m[2][1] = wVec.y;	m[2][2] = wVec.z;	//	- w(x, y, z)
	}
	else {
		m[0][0] = uVec.x;	m[0][1] = vVec.x;	m[0][2] = wVec.x;	//	- u(x, y, z) 
		m[1][0] = uVec.y;	m[1][1] = vVec.y;	m[1][2] = wVec.y;	//	- v(x, y, z)
		m[2][0] = uVec.z;	m[2][1] = vVec.z;	m[2][2] = wVec.z;	//	- w(x, y, z)
	}

	m[3][0] = offset.x;	m[3][1] = offset.y;	m[3][2] = offset.z;	//  - offset(x,y,z)
}

void VWTransformMatrix::SetMatrix(const VWPoint3D& offset)
{
	this->SetIdentity();
	m[3][0] = offset.x;	m[3][1] = offset.y;	m[3][2] = offset.z;	//  - offset(x,y,z)
}

void VWTransformMatrix::SetMatrix(double scaleX, double scaleY, double scaleZ)
{
	m[0][0] = scaleX;	m[0][1] = 0;		m[0][2] = 0;		//	- u(x, y, z) 
	m[1][0] = 0;		m[1][1] = scaleY;	m[1][2] = 0;		//	- v(x, y, z)
	m[2][0] = 0;		m[2][1] = 0;		m[2][2] = scaleZ;	//	- w(x, y, z)
	m[3][0] = 0;		m[3][1] = 0;		m[3][2] = 0;		//  - offset(x,y,z)
}

VWPoint3D VWTransformMatrix::GetUVector() const
{
	return VWPoint3D( m[0][0], m[0][1], m[0][2] );
}

void VWTransformMatrix::SetUVector(double x, double y, double z)
{
	m[0][0] = x;	m[0][1] = y;	m[0][2] = z;		//	- u(x, y, z) 
}

void VWTransformMatrix::SetUVector(const VWPoint3D& uVec)
{
	m[0][0] = uVec.x;	m[0][1] = uVec.y;	m[0][2] = uVec.z;		//	- u(x, y, z) 
}

VWPoint3D VWTransformMatrix::GetVVector() const
{
	return VWPoint3D( m[1][0], m[1][1], m[1][2] );
}

void VWTransformMatrix::SetVVector(double x, double y, double z)
{
	m[1][0] = x;	m[1][1] = y;	m[1][2] = z;		//	- v(x, y, z)
}

void VWTransformMatrix::SetVVector(const VWPoint3D& vVec)
{
	m[1][0] = vVec.x;	m[1][1] = vVec.y;	m[1][2] = vVec.z;		//	- v(x, y, z)
}

VWPoint3D VWTransformMatrix::GetWVector() const
{
	return VWPoint3D( m[2][0], m[2][1], m[2][2] );
}

void VWTransformMatrix::SetWVector(double x, double y, double z)
{
	m[2][0] = x;	m[2][1] = y;	m[2][2] = z;		//  - offset(x,y,z)
}

void VWTransformMatrix::SetWVector(const VWPoint3D& wVec)
{
	m[2][0] = wVec.x;	m[2][1] = wVec.y;	m[2][2] = wVec.z;		//  - offset(x,y,z)
}

VWPoint3D VWTransformMatrix::GetOffset() const
{
	return VWPoint3D( m[3][0], m[3][1], m[3][2] );
}

void VWTransformMatrix::SetOffset(double x, double y, double z)
{
	m[3][0]		= x;
	m[3][1]		= y;
	m[3][2]		= z;
}

void VWTransformMatrix::SetOffset(const VWPoint3D& off)
{
	m[3][0]		= off.x;
	m[3][1]		= off.y;
	m[3][2]		= off.z;
}

VWTransformMatrix& VWTransformMatrix::Invert()
{
	double	pos = 0.0;
	double	neg = 0.0;
	
	double	temp;
	temp =   m[0][0] * m[1][1] * m[2][2];		(temp >= 0 ? pos : neg) += temp;
	temp =   m[0][1] * m[1][2] * m[2][0];		(temp >= 0 ? pos : neg) += temp;
	temp =   m[0][2] * m[1][0] * m[2][1];		(temp >= 0 ? pos : neg) += temp;
	temp = -(m[2][0] * m[1][1] * m[0][2]);		(temp >= 0 ? pos : neg) += temp;
	temp = -(m[2][1] * m[1][2] * m[0][0]);		(temp >= 0 ? pos : neg) += temp;
	temp = -(m[2][2] * m[1][0] * m[0][1]);		(temp >= 0 ? pos : neg) += temp;

	double	det = pos + neg;
	double	mag = pos - neg;
	
	if ( Abs(mag) < 0.00000001 ) {
		// Invert4by4_ByeBye();
		temp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = temp;
		temp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = temp;
		temp = m[2][1]; m[2][1] = m[1][2]; m[1][2] = temp;
	}
	else {
		temp = det / mag;
		if ( Abs(temp) < 0.00000001 ) { 
			// Invert4by4_ByeBye();
			temp = m[0][1]; m[0][1] = m[1][0]; m[1][0] = temp;
			temp = m[0][2]; m[0][2] = m[2][0]; m[2][0] = temp;
			temp = m[2][1]; m[2][1] = m[1][2]; m[1][2] = temp;
		}
		else {
			VWTransformMatrix	dest;
			dest.m[0][0] =  (  m[1][1] * m[2][2] - m[2][1] * m[1][2] ) / det;
			dest.m[0][1] = -(( m[0][1] * m[2][2] - m[2][1] * m[0][2] ) / det );
			dest.m[0][2] =  (  m[0][1] * m[1][2] - m[1][1] * m[0][2] ) / det;
			dest.m[1][0] = -(( m[1][0] * m[2][2] - m[2][0] * m[1][2] ) / det );
			dest.m[1][1] =  (  m[0][0] * m[2][2] - m[2][0] * m[0][2] ) / det;
			dest.m[1][2] = -(( m[0][0] * m[1][2] - m[1][0] * m[0][2] ) / det );
			dest.m[2][0] =  (  m[1][0] * m[2][1] - m[2][0] * m[1][1] ) / det;
			dest.m[2][1] = -(( m[0][0] * m[2][1] - m[2][0] * m[0][1] ) / det );
			dest.m[2][2] =  (  m[0][0] * m[1][1] - m[1][0] * m[0][1] ) / det;

			dest.m[3][0] = -(  m[3][0] * dest.m[0][0] + m[3][1] * dest.m[1][0] + m[3][2] * dest.m[2][0] );
			dest.m[3][1] = -(  m[3][0] * dest.m[0][1] + m[3][1] * dest.m[1][1] + m[3][2] * dest.m[2][1] );
			dest.m[3][2] = -(  m[3][0] * dest.m[0][2] + m[3][1] * dest.m[1][2] + m[3][2] * dest.m[2][2] );

			*this	= dest;
		}
	}

	return *this;
}

VWTransformMatrix VWTransformMatrix::Invert() const
{
	VWTransformMatrix	m	= * this;
	return m.Invert();
}

VWTransformMatrix VWTransformMatrix::GetInverted() const
{
	VWTransformMatrix	m	= * this;
	return m.Invert();
}

bool VWTransformMatrix::IsFlipped() const
{
	VWPoint3D	uVec	= VWPoint3D( m[0][0], m[0][1], m[0][2] );
	VWPoint3D	vVec	= VWPoint3D( m[1][0], m[1][1], m[1][2] );
	VWPoint3D	wVec	= VWPoint3D( m[2][0], m[2][1], m[2][2] );

	return uVec.DotProduct( vVec.Cross( wVec ) ) < 0.0001;
}

void VWTransformMatrix::FlipMatrix()
{
	VWPoint3D	uVec	= VWPoint3D( m[0][0], m[0][1], m[0][2] );
	VWPoint3D	vVec	= VWPoint3D( m[1][0], m[1][1], m[1][2] );

	this->SetUVector( vVec );
	this->SetVVector( uVec );
}

void VWTransformMatrix::TranslateAfter(double x, double y, double z)
{
	m[3][0] += x;
	m[3][1] += y;
	m[3][2] += z;
}

void VWTransformMatrix::TranslateAfter(const VWPoint3D& offset)
{
	this->TranslateAfter( offset.x, offset.y, offset.z );
}

void VWTransformMatrix::TranslateBefore(double x, double y, double z)
{
	m[3][0] += ( m[0][0] * x + m[1][0] * y + m[2][0] * z );
	m[3][1] += ( m[0][1] * x + m[1][1] * y + m[2][1] * z );
	m[3][2] += ( m[0][2] * x + m[1][2] * y + m[2][2] * z );
}

void VWTransformMatrix::TranslateBefore(const VWPoint3D& offset)
{
	this->TranslateBefore( offset.x, offset.y, offset.z );
}

void VWTransformMatrix::RotateXAfter(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );

	double m01	= m[0][1] * c - m[0][2] * s;
	double m11	= m[1][1] * c - m[1][2] * s;
	double m21	= m[2][1] * c - m[2][2] * s;
	double m31	= m[3][1] * c - m[3][2] * s;

	m[0][2]		= m[0][1] * s + m[0][2] * c;
	m[1][2]		= m[1][1] * s + m[1][2] * c;
	m[2][2]		= m[2][1] * s + m[2][2] * c;
	m[3][2]		= m[3][1] * s + m[3][2] * c;

	m[0][1]		= m01;
	m[1][1]		= m11;
	m[2][1]		= m21;
	m[3][1]		= m31;
}

void VWTransformMatrix::RotateYAfter(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );

	double m00	= m[0][0] * c + m[0][2] * s;
	double m10	= m[1][0] * c + m[1][2] * s;
	double m20	= m[2][0] * c + m[2][2] * s;
	double m30	= m[3][0] * c + m[3][2] * s;

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

	m[0][0]		= m00;
	m[1][0]		= m10;
	m[2][0]		= m20;
	m[3][0]		= m30;
}

void VWTransformMatrix::RotateZAfter(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );

	double m00	= m[0][0] * c - m[0][1] * s;
	double m10	= m[1][0] * c - m[1][1] * s;
	double m20	= m[2][0] * c - m[2][1] * s;
	double m30	= m[3][0] * c - m[3][1] * s;

	m[0][1]		= m[0][0] * s + m[0][1] * c;
	m[1][1]		= m[1][0] * s + m[1][1] * c;
	m[2][1]		= m[2][0] * s + m[2][1] * c;
	m[3][1]		= m[3][0] * s + m[3][1] * c;

	m[0][0]		= m00;
	m[1][0]		= m10;
	m[2][0]		= m20;
	m[3][0]		= m30;
}

void VWTransformMatrix::RotateXBefore(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );
	
	double m10	= m[1][0] * c + m[2][0] * s;
	double m11	= m[1][1] * c + m[2][1] * s;
	double m12	= m[1][2] * c + m[2][2] * s;

	double m20	= - m[1][0] * s + m[2][0] * c;
	double m21	= - m[1][1] * s + m[2][1] * c;
	double m22	= - m[1][2] * s + m[2][2] * c;

	m[1][0]		= m10;
	m[1][1]		= m11;
	m[1][2]		= m12;

	m[2][0]		= m20;
	m[2][1]		= m21;
	m[2][2]		= m22;
}

void VWTransformMatrix::RotateYBefore(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );

	double m00	= m[0][0] * c - m[2][0] * s;
	double m01	= m[0][1] * c - m[2][1] * s;
	double m02	= m[0][2] * c - m[2][2] * s;

	double m20	= m[0][0] * s + m[2][0] * c;
	double m21	= m[0][1] * s + m[2][1] * c;
	double m22	= m[0][2] * s + m[2][2] * c;

	m[0][0]	= m00;
	m[0][1]	= m01;
	m[0][2]	= m02;

	m[2][0]	= m20;
	m[2][1]	= m21;
	m[2][2]	= m22;
}

void VWTransformMatrix::RotateZBefore(double angleDeg)
{
	double ang	= angleDeg * kRadiansPerDegree;
	double c	= cos( ang );
	double s	= sin( ang );

	double m00	= m[0][0] * c + m[1][0] * s;
	double m01	= m[0][1] * c + m[1][1] * s;
	double m02	= m[0][2] * c + m[1][2] * s;

	m[1][0]		= - m[0][0] * s + m[1][0] * c;
	m[1][1]		= - m[0][1] * s + m[1][1] * c;
	m[1][2]		= - m[0][2] * s + m[1][2] * c;
	 
	m[0][0]		= m00;
	m[0][1]		= m01;
	m[0][2]		= m02;
}

void VWTransformMatrix::ScaleAfter(double scaleX, double scaleY, double scaleZ)
{
	m[0][0] *= scaleX; m[1][0] *= scaleX; m[2][0] *= scaleX; m[3][0] *= scaleX;
	m[0][1] *= scaleY; m[1][1] *= scaleY; m[2][1] *= scaleY; m[3][1] *= scaleY;
	m[0][2] *= scaleZ; m[1][2] *= scaleZ; m[2][2] *= scaleZ; m[3][2] *= scaleZ;
}

void VWTransformMatrix::RotateRoundPoint(const VWPoint3D& centerPt, double angXDeg, double angYDeg, double angZDeg)
{
	this->TranslateAfter( - centerPt.x, - centerPt.y, - centerPt.z );
	this->RotateXAfter( angXDeg );
	this->RotateYAfter( angYDeg );
	this->RotateZAfter( angZDeg );
	this->TranslateAfter( centerPt.x, centerPt.y, centerPt.z );
}

void VWTransformMatrix::ScaleWithCenter(const VWPoint3D& centerPt, double scaleX, double scaleY, double scaleZ)
{
	this->TranslateAfter( - centerPt.x, - centerPt.y, - centerPt.z );
	this->ScaleAfter( scaleX, scaleY, scaleZ );
	this->TranslateAfter( centerPt.x, centerPt.y, centerPt.z );
}

VWPoint2D VWTransformMatrix::PointTransform(const VWPoint2D& point) const
{
	VWPoint2D	res;
	res.x =	point.x * m[0][0] + point.y * m[1][0] + 0/*point.z*/ * m[2][0] + m[3][0];
	res.y =	point.x * m[0][1] + point.y * m[1][1] + 0/*point.z*/ * m[2][1] + m[3][1];
	//res.z =	point.x * m[0][2] + point.y * m[1][2] + 0/*point.z*/ * m[2][2] + m[3][2];
	return res;
}

VWPoint3D VWTransformMatrix::PointTransform(const VWPoint3D& point) const
{
	VWPoint3D	res;
	res.x =	point.x * m[0][0] + point.y * m[1][0] + point.z * m[2][0] + m[3][0];
	res.y =	point.x * m[0][1] + point.y * m[1][1] + point.z * m[2][1] + m[3][1];
	res.z =	point.x * m[0][2] + point.y * m[1][2] + point.z * m[2][2] + m[3][2];
	return res;
}

VWPoint2D VWTransformMatrix::InversePointTransform(const VWPoint2D& point) const
{
	// Linear algebra equivalent:
	// b = a MAT^{-1}

	VWPoint3D		dOffset;

	dOffset.x = point.x - m[3][0];
	dOffset.y = point.y - m[3][1];
	dOffset.z = 0/*point.z*/ - m[3][2];
	
	VWPoint2D		res;
	res.x =	dOffset.x * m[0][0] + dOffset.y * m[0][1] + dOffset.z * m[0][2];
	res.y =	dOffset.x * m[1][0] + dOffset.y * m[1][1] + dOffset.z * m[1][2];
	//res.z =	dOffset.x * m[2][0] + dOffset.y * m[2][1] + dOffset.z * m[2][2];
	return res;
}

VWPoint3D VWTransformMatrix::InversePointTransform(const VWPoint3D& point) const
{
	// Linear algebra equivalent:
	// b = a MAT^{-1}

	VWPoint3D		dOffset;

	dOffset.x = point.x - m[3][0];
	dOffset.y = point.y - m[3][1];
	dOffset.z = point.z - m[3][2];
	
	VWPoint3D		res;
	res.x =	dOffset.x * m[0][0] + dOffset.y * m[0][1] + dOffset.z * m[0][2];
	res.y =	dOffset.x * m[1][0] + dOffset.y * m[1][1] + dOffset.z * m[1][2];
	res.z =	dOffset.x * m[2][0] + dOffset.y * m[2][1] + dOffset.z * m[2][2];
	return res;
}

// operations

VWPoint2D VWTransformMatrix::operator*(const VWPoint2D& point) const
{
	return this->PointTransform( point );
}

VWPoint3D VWTransformMatrix::operator*(const VWPoint3D& point) const
{
	return this->PointTransform( point );
}

VWTransformMatrix VWTransformMatrix::operator*(const VWTransformMatrix& matrix) const
{
	VWTransformMatrix	m	= * this;
	return m *= matrix;
}

VWTransformMatrix& VWTransformMatrix::operator*=(const VWTransformMatrix& matrix)
{
	VWTransformMatrix	returnMat;	
	returnMat.m[0][0] = matrix.m[0][0] * this->m[0][0] + matrix.m[1][0] * this->m[0][1] + matrix.m[2][0] * this->m[0][2];
	returnMat.m[0][1] = matrix.m[0][1] * this->m[0][0] + matrix.m[1][1] * this->m[0][1] + matrix.m[2][1] * this->m[0][2];
	returnMat.m[0][2] = matrix.m[0][2] * this->m[0][0] + matrix.m[1][2] * this->m[0][1] + matrix.m[2][2] * this->m[0][2];
	returnMat.m[1][0] = matrix.m[0][0] * this->m[1][0] + matrix.m[1][0] * this->m[1][1] + matrix.m[2][0] * this->m[1][2];
	returnMat.m[1][1] = matrix.m[0][1] * this->m[1][0] + matrix.m[1][1] * this->m[1][1] + matrix.m[2][1] * this->m[1][2];
	returnMat.m[1][2] = matrix.m[0][2] * this->m[1][0] + matrix.m[1][2] * this->m[1][1] + matrix.m[2][2] * this->m[1][2];
	returnMat.m[2][0] = matrix.m[0][0] * this->m[2][0] + matrix.m[1][0] * this->m[2][1] + matrix.m[2][0] * this->m[2][2];
	returnMat.m[2][1] = matrix.m[0][1] * this->m[2][0] + matrix.m[1][1] * this->m[2][1] + matrix.m[2][1] * this->m[2][2];
	returnMat.m[2][2] = matrix.m[0][2] * this->m[2][0] + matrix.m[1][2] * this->m[2][1] + matrix.m[2][2] * this->m[2][2];

	returnMat.m[3][0] = matrix.m[0][0] * this->m[3][0] + matrix.m[1][0] * this->m[3][1] + matrix.m[2][0] * this->m[3][2] + matrix.m[3][0];
	returnMat.m[3][1] = matrix.m[0][1] * this->m[3][0] + matrix.m[1][1] * this->m[3][1] + matrix.m[2][1] * this->m[3][2] + matrix.m[3][1];
	returnMat.m[3][2] = matrix.m[0][2] * this->m[3][0] + matrix.m[1][2] * this->m[3][1] + matrix.m[2][2] * this->m[3][2] + matrix.m[3][2];

	*this = returnMat;
	return *this;
}

// compatibility with SDK
VWTransformMatrix::operator const TransformMatrix&() const
{
	// fool the compiler to allow us modify fMatrix
	TransformMatrix*	pMatrix	= (TransformMatrix*) & fMatrix;
	pMatrix->mat[0][0] = m[0][0]; pMatrix->mat[0][1] = m[0][1]; pMatrix->mat[0][2] = m[0][2];
	pMatrix->mat[1][0] = m[1][0]; pMatrix->mat[1][1] = m[1][1]; pMatrix->mat[1][2] = m[1][2];
	pMatrix->mat[2][0] = m[2][0]; pMatrix->mat[2][1] = m[2][1]; pMatrix->mat[2][2] = m[2][2];
	pMatrix->mat[3][0] = m[3][0]; pMatrix->mat[3][1] = m[3][1]; pMatrix->mat[3][2] = m[3][2];
	return (*pMatrix);
}

VWTransformMatrix::operator TransformMatrix&()
{
	fMatrix.mat[0][0] = m[0][0]; fMatrix.mat[0][1] = m[0][1]; fMatrix.mat[0][2] = m[0][2];
	fMatrix.mat[1][0] = m[1][0]; fMatrix.mat[1][1] = m[1][1]; fMatrix.mat[1][2] = m[1][2];
	fMatrix.mat[2][0] = m[2][0]; fMatrix.mat[2][1] = m[2][1]; fMatrix.mat[2][2] = m[2][2];
	fMatrix.mat[3][0] = m[3][0]; fMatrix.mat[3][1] = m[3][1]; fMatrix.mat[3][2] = m[3][2];
	return fMatrix;
}

// This fucntion calculates a transformation matrix (non afine)
// that transforms the first triangle (inPt1, inPt2, inPt3) into (transfPt1, transfPt2, transfPt3) triangle
bool VWTransformMatrix::CalcTransformFromOneTriToAnother(
											const VWPoint3D& inPt1, const VWPoint3D& inPt2, const VWPoint3D& inPt3,
											const VWPoint3D& transfPt1, const VWPoint3D& transfPt2, const VWPoint3D& transfPt3)
{
	class CSystemOfLinearEq
	{
	public:
		CSystemOfLinearEq()
		{
			m_arrB[0] = m_arrB[1] = m_arrB[2] = m_arrB[3] = 0;
			for(int i=0; i<4*4; i++)
				m_A[i] = 0;
		}
		
		void Reset()
		{
			m_arrB[0] = m_arrB[1] = m_arrB[2] = m_arrB[3] = 0;
			for(int i=0; i<4*4; i++)
				m_A[i] = 0;
		}

		void SetEquation(unsigned int _iEqNumber, const double *_pdEqCoefficients, double _dEqResult )
		{
			ASSERTN( kVStanev, _iEqNumber < 4);

			m_A[4*_iEqNumber+0]	= _pdEqCoefficients[0];
			m_A[4*_iEqNumber+1]	= _pdEqCoefficients[1];
			m_A[4*_iEqNumber+2]	= _pdEqCoefficients[2];
			m_A[4*_iEqNumber+3]	= _pdEqCoefficients[3];

			m_arrB[_iEqNumber] = _dEqResult;
		}

		bool FindLUDecompSolution( double *_pSolution, double dZeroEpsilon )
		{
			ASSERTN( kVStanev, _pSolution );
			int piIndex[ 4 ];

			double p;
			double tmp[ 4*4 ];
			if ( ! LUDecomposition( m_A, tmp, p, piIndex, dZeroEpsilon ) ) {
				return false;
			}

			this->LUBackSubstitution( tmp, piIndex, &m_arrB[0], _pSolution);

			return true;
		}

		int LUDecomposition(double* arrMatrix, double* _rResult, double &_rdEven_Odd, int *_pRowIndex, double dZeroEpsilon ) const
		{
			int		i, iMax, j, k;
			long double	dBig, dDum, dSum, dTemp;
			double Vec[4];

			
			_rdEven_Odd = 1.0;
			for( i=0; i<4*4; i++)
				_rResult[i] = arrMatrix[i];

			for( i=0; i<4; i++ )
			{
				dBig = 0.0;
				for( j=0; j<4; j++ )
					if( (dTemp = fabs( _rResult[4*i+j] )) > dBig )
						dBig = dTemp;
				
				if( fabs(dBig)< dZeroEpsilon )
					return 0;
				Vec[i] = 1.0/dBig;
			}

			for( j=0; j<4; j++ )
			{
				for ( i=0; i<j; i++)
				{
					dSum = _rResult[4*i+j];
					for( k=0; k<i; k++ )
						dSum -= _rResult[4*i+k] * _rResult[4*k+j];
					_rResult[4*i+j] = dSum;
				}
				dBig = 0.0;
				for( i=j; i<4; i++)
				{
					dSum = _rResult[4*i+j];
					for( k=0; k<j; k++ )
						dSum -= _rResult[4*i+k] * _rResult[4*k+j];
					_rResult[4*i+j] = dSum;
					if( ( dDum = Vec[i]*fabs(dSum) ) >= dBig )
					{
						dBig = dDum;
						iMax = i;
					}
				}

				if( j!=iMax )
				{
					for( k=0; k<4; k++ )
					{
						dDum = _rResult[4*iMax+k];
						_rResult[4*iMax+k] = _rResult[4*j+k];
						_rResult[4*j+k] = dDum;
					}
					_rdEven_Odd = -_rdEven_Odd;
					Vec[iMax] = Vec[j];
				}

				if( _pRowIndex )
					_pRowIndex[j]=iMax;

		//.....if pivot element is zero........................

				if( fabs(_rResult[4*j+j] ) < dZeroEpsilon )
					return 0;

				if( j!=(4-1) )
				{
					dDum = 1.0/_rResult[4*j+j];
					for( i=j+1; i<4; i++ )
						_rResult[4*i+j] *= dDum;
				}
			}
			return 1;
		}

		void LUBackSubstitution(double* arrMatrix, const int *_pcRowIndex, double *_pB, double *_pSolution ) const
		{
			ASSERTN( kVStanev, _pB );
			ASSERTN( kVStanev, _pSolution );

			int    i, ip, j, ii=-1;
			long double dSum;

			_pSolution[0]	= _pB[0];
			_pSolution[1]	= _pB[1];
			_pSolution[2]	= _pB[2];
			_pSolution[3]	= _pB[3];
			
			for( i=0; i<4; i++ )
			{
				ip = _pcRowIndex[i];
				dSum = _pB[ip];
				_pB[ip] = _pB[i];
				if( ii != -1 )
					for( j=ii; j<i; j++ )
						dSum -= arrMatrix[4*i+j]*_pB[j];
				else 
					if(dSum)
						ii=i;
				_pB[i] = dSum;
			}

			for(i=4-1; i>=0; i--)
			{
				dSum = _pB[i];
				for ( j=i+1; j<4; j++ )
					dSum -= arrMatrix[4*i+j]*_pB[j];
				_pB[i] = dSum / arrMatrix[4*i+i];
			}
			
			if( _pSolution )
			{
				VWFC::Tools::SwapVariables( _pSolution[0], _pB[0] );
				VWFC::Tools::SwapVariables( _pSolution[1], _pB[1] );
				VWFC::Tools::SwapVariables( _pSolution[2], _pB[2] );
				VWFC::Tools::SwapVariables( _pSolution[3], _pB[3] );
			}
		}
	private:
		double		m_A[4*4];//Coefficient matrix
		double		m_arrB[4];//Equation result vector.
	}	system;

	// Find fourth couple
	VWPoint3D	vecG4	= inPt1 + (inPt2 - inPt1).Cross( inPt3 - inPt1 );
	VWPoint3D	vecL4	= transfPt1 + (transfPt2 - transfPt1).Cross( transfPt3 - transfPt1 );

	const VWPoint3D*	arrGV[4] = { & inPt1, & inPt2, & inPt3, & vecG4 };
	const VWPoint3D*	arrLV[4] = { & transfPt1, & transfPt2, & transfPt3, & vecL4 };

	this->SetIdentity();

	double dTmpVec[4];	
	dTmpVec[3] = 1.0;
	for (int i=0; i<3; i++)	{
		// find the i-th row of the transformation matrix
		system.Reset();
		for( int j=0; j<4; j++ )
		{
			dTmpVec[0] = arrGV[j]->x;
			dTmpVec[1] = arrGV[j]->y;
			dTmpVec[2] = arrGV[j]->z;
			double	result;
			if ( i == 0 )		result = arrLV[j]->x;
			else if ( i == 1 )	result = arrLV[j]->y;
			else if ( i == 2 )	result = arrLV[j]->z;
			system.SetEquation( j, dTmpVec, result );
		}

		double	_solution[4];
		if ( !system.FindLUDecompSolution( _solution, 1.192092896e-07F ) )
		{
			return false; // Some points coincide - transormation undefined
		}

		this->m[0][i]	= _solution[0];
		this->m[1][i]	= _solution[1];
		this->m[2][i]	= _solution[2];
		this->m[3][i]	= _solution[3];
	}

	return true;
} // CalcTransformFromOneTriToAnother

