/* -----------------------------------------------------------------------------
	GSFramework
	Copyright 2001-2013 Emmanuel Julien. All Rights Reserved.
----------------------------------------------------------------------------- */


		#include <cmath>
		#include "math/matrix3.h"
		#include "math/matrix4.h"
		#include "metafile/nml.h"

		using namespace GS;
		using namespace GS::Math;

		Matrix3 Matrix3::static_identity;


//------------------------------------------------------------------------------
bool				Matrix3::Inverse(Matrix3 &i) const
{
	// Covariants.
	i.m[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
	i.m[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
	i.m[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];
	i.m[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
	i.m[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
	i.m[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];
	i.m[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
	i.m[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
	i.m[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];

	float k = m[0][0] * i.m[0][0] + m[0][1] * i.m[1][0] + m[0][2] * i.m[2][0];
	if	(!k)
		return false;

	k = 1.f / k;
	i.m[0][0] *= k; i.m[0][1] *= k; i.m[0][2] *= k;
	i.m[1][0] *= k; i.m[1][1] *= k; i.m[1][2] *= k;
	i.m[2][0] *= k; i.m[2][1] *= k; i.m[2][2] *= k;
	return true;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::VectorMatrix(const Vector4 &v)
{	return Matrix3(v.x, 0, 0, v.y, 0, 0, v.z, 0, 0);	}
Matrix3				Matrix3::CrossProductMatrix(const Vector4 &v)
{	return Matrix3(0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0);	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::Normalized() const
{
	Vector4 x(GetRow(0)), y(GetRow(1)), z(GetRow(2));

	Matrix3 m;
	m.SetRow(0, x.Normalized());
	m.SetRow(1, y.Normalized());
	m.SetRow(2, z.Normalized());
	return m;
}
Matrix3				Matrix3::AsOrthonormalBase() const
{
	Vector4 x(GetRow(0)), y(GetRow(1));

	Matrix3 m;
	x = x.Normalized();
	m.SetRow(0, x);
	Vector4 z(x.Cross(y).Normalized());
	m.SetRow(2, z);
	y = z.Cross(x).Normalized();
	m.SetRow(1, y);
	return m;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Vector4				Matrix3::AsEuler(rOrder rorder) const
{
	Vector4 euler(0, 0, 0);

	switch (rorder)
	{
		case rOrder_ZYX:
			euler.y = ASin(-m[2][0]);
			euler.z = atan2(m[1][0], m[0][0]);
			euler.x = atan2(m[2][1], m[2][2]);
			break;

		case rOrder_XZY:
			euler.z = ASin(-m[0][1]);
			euler.x = atan2(m[2][1], m[1][1]);
			euler.y = atan2(m[0][2], m[0][0]);
			break;

		case rOrder_XYZ:
			euler.y = ASin(m[0][2]);
			euler.x = atan2(-m[1][2], m[2][2]);
			euler.z = atan2(-m[0][1], m[0][0]);
			break;

		case rOrder_YZX:
			euler.z = ASin(m[1][0]);
			euler.x = atan2(-m[1][2], m[1][1]);
			euler.y = atan2(-m[2][0], m[0][0]);
			break;

		default:
		case rOrder_YXZ:	// Engine default.
			euler.x = ASin(-m[1][2]);
			euler.y = atan2(m[0][2], m[2][2]);
			euler.z = atan2(m[1][0], m[1][1]);
			break;

		case rOrder_ZXY:	// MAX default.
			euler.x = ASin(m[2][1]);
			euler.y = atan2(-m[2][0], m[2][2]);
			euler.z = atan2(-m[0][1], m[1][1]);
			break;

		case rOrder_XY:
			euler.y = ACos(m[0][0]);
			euler.x = ACos(m[1][1]);
			euler.z = 0;
			break;
	}
	return euler;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::FromEuler(const Vector4 &euler, rOrder rorder)
{	return Matrix3::FromEuler(euler.x, euler.y, euler.z, rorder);	}
Matrix3				Matrix3::FromEuler(float x, float y, float z, rOrder rorder)
{
	float cx = Cos(x), cy = Cos(y), cz = Cos(z),
		  sx = Sin(x), sy = Sin(y), sz = Sin(z);

	switch (rorder)
	{
		case rOrder_XZY:
			return Matrix3	(	cy * cz,					sx * sy + cx * cy * sz,			-cx * sy + cy * sx * sz,
								-sz,							cx * cz,						cz * sx,
								cz * sy,					-cy * sx + cx * sy * sz,			cx * cy + sx * sy * sz	);

		case rOrder_ZYX:
			return Matrix3	(	cy * cz,					cy * sz,						-sy,
								cz * sx * sy - cx * sz,		cx * cz + sx * sy * sz,			cy * sx,
								cx  *cz * sy + sx * sz,		-cz * sx + cx * sy * sz,			cx * cy					);

		case rOrder_XYZ:
			return Matrix3	(	cy * cz,					cz * sx * sy + cx * sz,			-cx * cz * sy + sx * sz,
								-cy * sz,					cx * cz - sx * sy * sz,			cz * sx + cx * sy * sz,
								sy,							-cy * sx,						cx * cy					);

		case rOrder_ZXY:
			return Matrix3	(	cy * cz - sx * sy * sz,		cz * sx * sy + cy * sz,			-cx * sy,
								-cx * sz,					cx * cz,						sx,
								cz * sy + cy * sx * sz,		-cy * cz * sx + sy * sz,			cx * cy					);

		case rOrder_YZX:
			return Matrix3	(	cy * cz,					sz,								-cz * sy,
								sx * sy - cx * cy * sz,		cx * cz,						cy * sx + cx * sy * sz,
								cx * sy + cy * sx * sz,		-cz * sx,						cx * cy - sx * sy * sz	);

		case rOrder_YXZ:
			return Matrix3	(	cy * cz + sx * sy * sz,		cx * sz,						-cz * sy + cy * sx * sz,
								cz * sx * sy - cy * sz,		cx * cz,						cy * cz * sx + sy * sz,
								cx * sy,					-sx,								cx * cy					);

		case rOrder_XY:
			return Matrix3	(	cy,							sx * sy,						-cx * sy,
								0,							cx,								sx,
								sy,							-cy * sx,						cx * cy					);
	}
	return Matrix3::IdentityMatrix();
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::TranslationMatrix(const Vector4 &t)
{	return Matrix3(1, 0, 0, 0, 1, 0, t.x, t.y, 1);		}
Matrix3				Matrix3::TranslationMatrix(const Vector2 &t)
{	return Matrix3(1, 0, 0, 0, 1, 0, t.x, t.y, 1);		}
Matrix3				Matrix3::ScaleMatrix(const Vector4 &s)
{	return Matrix3(s.x, 0, 0, 0, s.y, 0, 0, 0, s.z);	}
Matrix3				Matrix3::ScaleMatrix(const Vector2 &s)
{	return Matrix3(s.x, 0, 0, 0, s.y, 0, 0, 0, 1);	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::RotationMatrixXAxis(float a)
{	return Matrix3(1, 0, 0, 0, Cos(a), Sin(a), 0, -Sin(a), Cos(a));	}
Matrix3				Matrix3::RotationMatrixYAxis(float a)
{	return Matrix3(Cos(a), 0, -Sin(a), 0, 1, 0, Sin(a), 0, Cos(a));	}
Matrix3				Matrix3::RotationMatrixZAxis(float a)
{	return Matrix3(Cos(a), Sin(a), 0, -Sin(a), Cos(a), 0, 0, 0, 1);	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
void				Matrix3::SetRow(uint n, const Vector4 &row)
{	m[0][n] = row.x; m[1][n] = row.y; m[2][n] = row.z;	}
void				Matrix3::SetColumn(uint n, const Vector4 &col)
{	m[n][0] = col.x; m[n][1] = col.y; m[n][2] = col.z;	}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
Matrix3				Matrix3::FromOrthonormalBasis(const Vector4 &w, const Vector4 *v)
{
	Matrix3 mtx;

	float l = w.Len();
	if	(!l)
		return Matrix3::IdentityMatrix();

	Vector4 wn = w / l, u;

	if	(!v)
	{
		if	(!EqualZero(wn.x) || !EqualZero(wn.z))
		{
			u.Set(wn.z, 0, -wn.x);		// Cross with up = {0,1,0}.
			u = u.Normalized();
		}
		else
			u.Set(-1, 0, 0);

		Vector4 c(wn.Cross(u));
		mtx.SetRow(1, c);
	}
	else
	{
		Vector4 vn(v->Normalized());
		mtx.SetRow(1, vn);
		u = vn.Cross(wn);
	}

	mtx.SetRow(0, u);
	mtx.SetRow(2, wn);
	return mtx;
}
Matrix3				Matrix3::FromMatrix4(const Matrix4 &mtx)
{
	return Matrix3(
						mtx.m[0][0], mtx.m[1][0], mtx.m[2][0],
						mtx.m[0][1], mtx.m[1][1], mtx.m[2][1],
						mtx.m[0][2], mtx.m[1][2], mtx.m[2][2]
					);
}
//------------------------------------------------------------------------------

//-----------------------------------------------------------------------------
void				Matrix3::Apply(Vector4 *o, const Vector4 *v, uint n) const
//-----------------------------------------------------------------------------
{
	for	(uint c = 0; c < n; c++)
	{
		float x = v->x, y = v->y, z = v->z;
		o->x = x * m[0][0] + y * m[0][1] + z * m[0][2];
		o->y = x * m[1][0] + y * m[1][1] + z * m[1][2];
		o->z = x * m[2][0] + y * m[2][1] + z * m[2][2];
		o->w = 1;
		o++; v++;
	}
}

//------------------------------------------------------------------------------
void				Matrix3::Set
					(
						float m00, float m10, float m20,
						float m01, float m11, float m21,
						float m02, float m12, float m22
					)
{
	m[0][0] = m00; m[1][0] = m10; m[2][0] = m20;
	m[0][1] = m01; m[1][1] = m11; m[2][1] = m21;
	m[0][2] = m02; m[1][2] = m12; m[2][2] = m22;
}
void				Matrix3::Set(const Vector4 &u, const Vector4 &v, const Vector4 &w)
{
	m[0][0] = u.x; m[1][0] = u.y; m[2][0] = u.z;
	m[0][1] = v.x; m[1][1] = v.y; m[2][1] = v.z;
	m[0][2] = w.x; m[1][2] = w.y; m[2][2] = w.z;
}
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
NML::Tag			*Matrix3::AsMetaTag(const char *id) const
{
	NML::Tag *root = new NML::Tag(id ? id : "Mtx3");
	root->AddChild(GetRow(0).AsMetaTag("R0"));
	root->AddChild(GetRow(1).AsMetaTag("R1"));
	root->AddChild(GetRow(2).AsMetaTag("R2"));
	return root;
}
bool				Matrix3::FromMetaTag(NML::Tag &tag)
{
	NML::Tag *t;
	Vector4	R;

	List <NML::Tag *> ::Iterator i(tag.GetTags().GetRoot());

	t = i.ObjectPtr();
	if	(!t) return false;
	R.FromMetaTag(*t); SetRow(0, R);
	++i;

	t = i.ObjectPtr();
	if	(!t) return false;
	R.FromMetaTag(*t); SetRow(1, R);
	++i;

	t = i.ObjectPtr();
	if	(!t) return false;
	R.FromMetaTag(*t); SetRow(2, R);

	return true;
}
//------------------------------------------------------------------------------