//	Darkstar Engine  (c) UnderWorld Interactive Studio (all coding by Svarog    2007)
//
//      Данный файл является частью Darkstar Engine. Любое использование кода файла
//      без согласия компании, является нарушением закона об авторском праве (в слу-
//      чае если не оговорено обратное).
//
//      Имя файла: matr.inl
//      Краткое описание: работа с матрицами
//

namespace math
{
	namespace _private
	{
		// Зактрытая функция. Создаёт матрицу поворота из любой матрицы.
		template <typename T> inline void _ApplyRotate(T& t, const float ang, const Vect3& vec)
		{
			float c = cosf(ang);
			float s = sinf(ang);
			float c1 = 1 - c;
			float xx = vec.x*vec.x;
			float yy = vec.y*vec.y;
			float zz = vec.z*vec.z;
			float xz = vec.x*vec.z;
			float yz = vec.y*vec.z;
			float xy = vec.x*vec.y;

			t.row0.x = c + c1*xx;
			t.row0.y = c1*xy - s*vec.z;
			t.row0.z = c1*xz + s*vec.y;

			t.row1.x = c1*xy + s*vec.z;
			t.row1.y = c + c1*yy;
			t.row1.z = c1*yz - s*vec.x;

			t.row2.x = c1*xz - s*vec.y;
			t.row2.y = c1*yz + s*vec.x;
			t.row2.z = c + c1*zz;
		};

		// сначала вращение вокруг Оx потом Oy потом Oz
		template <typename T> inline void _ApplyRotate(T& m, const Vect3& angles)
		{
			float sr = sinf(angles.x);
			float sp = sinf(angles.y);
			float sy = sinf(angles.z);
			float cr = cosf(angles.x);
			float cp = cosf(angles.y);
			float cy = cosf(angles.z);

			float srsp = sr*sp;
			float crsp = cr*sp;

			m.row0.x = cp*cy;
			m.row0.y = -sy*cr + cy*srsp;
			m.row0.z = sr*sy + crsp*cy;

			m.row1.x = sy*cp;
			m.row1.y = cr*cy + srsp*sy;
			m.row1.z = -cy*sr + crsp*sy;

			m.row2.x = -sp;
			m.row2.y = cp*sr;
			m.row2.z = cr*cp;
		};
	};

	// Matr3

	inline Matr3::Matr3()
	{

	};

	inline Matr3::Matr3(const Matr3& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
	};

	inline Matr3::Matr3(const float a00, const float a01, const float a02, const float a10, const float a11, const float a12, const float a20, const float a21, const float a22)
	{
		_a00 = a00;
		_a01 = a01;
		_a02 = a02;
		
		_a10 = a10;
		_a11 = a11;
		_a12 = a12;

		_a20 = a20;
		_a21 = a21;
		_a22 = a22;
	};

	inline Matr3::Matr3(const Vect3& row0, const Vect3& row1, const Vect3& row2)
	{
		this->row0 = row0;
		this->row1 = row1;
		this->row2 = row2;
	};

	inline Matr3::Matr3(const Vect3* rows)
	{
		this->rows[0] = rows[0];
		this->rows[1] = rows[1];
		this->rows[2] = rows[2];
		this->rows[3] = rows[3];
	};

	inline Matr3::Matr3(const Matr34& m)
	{
		row0 = Vect3(m.row0);
		row1 = Vect3(m.row1);
		row2 = Vect3(m.row2);
	};

	inline Matr3::Matr3(const Matr4& m)
	{
		row0 = Vect3(m.row0);
		row1 = Vect3(m.row1);
		row2 = Vect3(m.row2);
	};

	inline Matr3::~Matr3()
	{

	};

	inline Matr3& Matr3::operator =(const Matr3& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
		return *this;
	};

	inline const Vect3& Matr3::operator [](const unsigned int i) const
	{
		return rows[i];
	};

	inline Vect3& Matr3::operator [](const unsigned int i)
	{
		return rows[i];
	};

	inline Matr3 Matr3::operator +(const Matr3& m) const
	{
		return Matr3(row0 + m.row0, row1 + m.row1, row2 + m.row2);
	};

	inline const Matr3& Matr3::operator +=(const Matr3& m)
	{
		row0 += m.row0;
		row1 += m.row1;
		row2 += m.row2;
		return *this;
	};

	inline Matr3 Matr3::operator -(const Matr3& m) const
	{
		return Matr3(row0 - m.row0, row1 - m.row1, row2 - m.row2);
	};

	inline const Matr3& Matr3::operator -=(const Matr3& m)
	{
		row0 -= m.row0;
		row1 -= m.row1;
		row2 -= m.row2;
		return *this;
	};

	inline Matr3 Matr3::operator *(const Matr3& m) const
	{
		return Matr3(Vect3(dot(row0, Vect3(m.row0.x, m.row1.x, m.row2.x)),
						dot(row0, Vect3(m.row0.y, m.row1.y, m.row2.y)),
						dot(row0, Vect3(m.row0.z, m.row1.z, m.row2.z))),
					Vect3(dot(row1, Vect3(m.row0.x, m.row1.x, m.row2.x)),
						dot(row1, Vect3(m.row0.y, m.row1.y, m.row2.y)),
						dot(row1, Vect3(m.row0.z, m.row1.z, m.row2.z))),
					Vect3(dot(row2, Vect3(m.row0.x, m.row1.x, m.row2.x)),
						dot(row2, Vect3(m.row0.y, m.row1.y, m.row2.y)),
						dot(row2, Vect3(m.row0.z, m.row1.z, m.row2.z))));
	};

	inline const Matr3& Matr3::operator *=(const Matr3& m)
	{
		Matr3 old = *this;

		row0.x = dot(old.row0, Vect3(m.row0.x, m.row1.x, m.row2.x));
		row0.y = dot(old.row0, Vect3(m.row0.y, m.row1.y, m.row2.y));
		row0.z = dot(old.row0, Vect3(m.row0.z, m.row1.z, m.row2.z));

		row1.x = dot(old.row1, Vect3(m.row0.x, m.row1.x, m.row2.x));
		row1.y = dot(old.row1, Vect3(m.row0.y, m.row1.y, m.row2.y));
		row1.z = dot(old.row1, Vect3(m.row0.z, m.row1.z, m.row2.z));

		row2.x = dot(old.row2, Vect3(m.row0.x, m.row1.x, m.row2.x));
		row2.y = dot(old.row2, Vect3(m.row0.y, m.row1.y, m.row2.y));
		row2.z = dot(old.row2, Vect3(m.row0.z, m.row1.z, m.row2.z));
		
		return *this;
	};

	inline Matr3 Matr3::operator *(const float f) const
	{
		return Matr3(row0*f, row1*f, row2*f);
	};

	inline const Matr3& Matr3::operator *=(const float f)
	{
		row0 *= f;
		row1 *= f;
		row2 *= f;
		return *this;
	};

	inline Matr3 Matr3::operator /(const float f) const
	{
		float f0 = 1/f;
		return Matr3(row0*f0, row1*f0, row2*f0);
	};

	inline const Matr3& Matr3::operator /=(const float f)
	{
		float f0 = 1/f;
		row0 *= f0;
		row1 *= f0;
		row2 *= f0;
		return *this;
	};

	inline Matr3 Matr3::operator -() const
	{
		return Matr3(-row0, -row1, -row2);
	};

	inline bool Matr3::operator ==(const Matr3& m) const
	{
		return (row0 == m.row0 && row1 == m.row1 && row2 == m.row2);
	};

	inline bool Matr3::operator !=(const Matr3& m) const
	{
		return (row0 != m.row0 || row1 != m.row1 || row2 != m.row2);
	};

	inline const Matr3& Matr3::transpose()
	{
		std::swap(_a[1], _a[3]);
		std::swap(_a[2], _a[6]);
		std::swap(_a[5], _a[7]);
		return *this;
	};

	inline Matr3 Matr3::getTranspose() const
	{
		return Matr3(_a00, _a10, _a20, _a01, _a11, _a21, _a02, _a12, _a22);
	};

	inline Matr3 Matr3::getInverse() const
	{
		float d = 1/det();
		
		return Matr3(Vect3((row1.y*row2.z-row1.z*row2.y)*d,-(row0.y*row2.z-row0.z*row2.y)*d,
			(row0.y*row1.z-row0.z*row1.y)*d), 
					Vect3(-(row1.x*row2.z-row1.z*row2.x)*d,
						(row0.x*row2.z-row0.z*row2.x)*d, -(row0.x*row1.z-row0.z*row1.x)*d), 
					Vect3((row1.x*row2.y-row1.y*row2.x)*d,
			-(row0.x*row2.y-row0.y*row2.x)*d, (row0.x*row1.y-row0.y*row1.x)*d));
	};

	inline Matr3 Matr3::getFastInverse() const
	{
		return getTranspose();
	};

	inline const Matr3& Matr3::invert()
	{
		Matr3 old = *this;
		float d = 1/det();

		row0 = Vect3((old.row1.y*old.row2.z-old.row1.z*old.row2.y)*d,-(old.row0.y*old.row2.z-old.row0.z*old.row2.y)*d,
			(old.row0.y*old.row1.z-old.row0.z*old.row1.y)*d);
		row1 = Vect3(-(old.row1.x*old.row2.z-old.row1.z*old.row2.x)*d,
			(old.row0.x*old.row2.z-old.row0.z*old.row2.x)*d, -(old.row0.x*old.row1.z-old.row0.z*old.row1.x)*d);
		row2 = Vect3((old.row1.x*old.row2.y-old.row1.y*old.row2.x)*d,
			-(old.row0.x*old.row2.y-old.row0.y*old.row2.x)*d, (old.row0.x*old.row1.y-old.row0.y*old.row1.x)*d);
		
		return *this;
	};

	inline const Matr3& Matr3::fastInvert()
	{
		return transpose();
	};

	inline float Matr3::det() const
	{
		return (_a00*(_a11*_a22 - _a12* _a21) - _a01*(_a10*_a22 - _a12*_a20) + _a02*(_a10*_a21 - _a11*_a20));
	};

	inline const Matr3& Matr3::setIdentity()
	{
		*this = matr3_sing;
		return *this;
	};

	inline const Matr3& Matr3::ApplyScaling(const float u_s)
	{
		*this *= Matr3(Vect3(u_s, 0, 0), Vect3(0, u_s, 0), Vect3(0, 0, u_s));
		return *this;
	};

	inline const Matr3& Matr3::ApplyScaling(const Vect3& v)
	{
		*this *= Matr3(Vect3(v.x, 0, 0), Vect3(0, v.y, 0), Vect3(0, 0, v.z));
		return *this;
	};

	inline const Matr3& Matr3::ApplyRotation(const Vect3& v, const float ang)
	{
		Matr3 m;
		_private::_ApplyRotate(m, ang, v);
		*this *= m;
		return *this;
	};

	inline const Matr3& Matr3::ApplyRotation(const Vect3& angles)
	{
		Matr3 m;
		_private::_ApplyRotate(m, angles);
		*this *= m;
		return *this;
	};

	inline const Matr3& Matr3::setOnlyScaling(const float u_s)
	{
		row0 = Vect3(u_s, 0, 0);
		row1 = Vect3(0, u_s, 0);
		row2 = Vect3(0, 0, u_s);
		return *this;
	};

	inline const Matr3& Matr3::setOnlyScaling(const Vect3& v)
	{
		row0 = Vect3(v.x, 0, 0);
		row1 = Vect3(0, v.y, 0);
		row2 = Vect3(0, 0, v.z);
		return *this;
	};

	inline const Matr3& Matr3::setOnlyRotation(const Vect3& v, const float ang)
	{
		_private::_ApplyRotate(*this, ang, v);
		return *this;
	};

	inline const Matr3& Matr3::setOnlyRotation(const Vect3& angles)
	{
		_private::_ApplyRotate(*this, angles);
		return *this;
	};

	inline bool Matr3::getIsOrthogonalMatrix() const
	{
		return (float_equal(dot(row0, row1), 0) && (float_equal(dot(row1, row2), 0)) && (float_equal(dot(row0, row2), 0)));
	};

	// Matr4

	inline Matr4::Matr4()
	{

	};

	inline Matr4::Matr4(const Matr4& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
		row3 = m.row3;
	};

	inline Matr4::Matr4(const float a00, const float a01, const float a02, const float a03, const float a10, const float a11, const float a12, const float a13, const float a20, const float a21, const float a22, const float a23, const float a30, const float a31, const float a32, const float a33)
	{
		_a00 = a00;
		_a01 = a01;
		_a02 = a02;
		_a03 = a03;

		_a10 = a10;
		_a11 = a11;
		_a12 = a12;
		_a13 = a13;

		_a20 = a20;
		_a21 = a21;
		_a22 = a22;
		_a23 = a23;

		_a30 = a30;
		_a31 = a31;
		_a32 = a32;
		_a33 = a33;
	};

	inline Matr4::Matr4(const Vect4& row0, const Vect4& row1, const Vect4& row2, const Vect4& row3)
	{
		this->row0 = row0;
		this->row1 = row1;
		this->row2 = row2;
		this->row3 = row3;
	};

	inline Matr4::Matr4(const Vect4* rows)
	{
		this->rows[0] = rows[0];
		this->rows[1] = rows[1];
		this->rows[2] = rows[2];
		this->rows[3] = rows[3];
	};

	inline Matr4::Matr4(const Matr34& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
	};

	inline Matr4::Matr4(const Matr3& m)
	{
		row0 = Vect4(m.row0);
		row0.w = 0;
		row1 = Vect4(m.row1);
		row1.w = 0;
		row2 = Vect4(m.row2);
		row2.w = 0;
		row3 = vect4_sing_w;
	};

	inline Matr4::Matr4(const Matr3& rot, const Vect3& trans, const Vect3& scale)
	{
		*this = Matr4(rot);
		this->ApplyTranslation(trans);
		this->ApplyScaling(scale);
	};

	inline Matr4::Matr4(const Matr3& rot, const Vect3& trans, const float scale)
	{
		*this = Matr4(rot);
		this->ApplyTranslation(trans);
		this->ApplyScaling(scale);
	};

	inline Matr4::~Matr4()
	{

	};

	inline Matr4& Matr4::operator =(const Matr4& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
		row3 = m.row3;
		return *this;
	};

	inline const Vect4& Matr4::operator [](const unsigned int i) const
	{
		return rows[i];
	};

	inline Vect4& Matr4::operator [](const unsigned int i)
	{
		return rows[i];
	};

	inline Matr4 Matr4::operator +(const Matr4& m) const
	{
		return Matr4(row0 + m.row0, row1 + m.row1, row2 + m.row2, row3 + m.row3);
	};

	inline const Matr4& Matr4::operator +=(const Matr4& m)
	{
		row0 += m.row0;
		row1 += m.row1;
		row2 += m.row2;
		row3 += m.row3;
		return *this;
	};

	inline Matr4 Matr4::operator -(const Matr4& m) const
	{
		return Matr4(row0 - m.row0, row1 - m.row1, row2 - m.row2, row3 - m.row3);
	};

	inline const Matr4& Matr4::operator -=(const Matr4& m)
	{
		row0 -= m.row0;
		row1 -= m.row1;
		row2 -= m.row2;
		row3 -= m.row3;
		return *this;
	};

	inline Matr4 Matr4::operator *(const Matr4& m) const 
	{
		return Matr4(dot(row0, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x)), 
					 dot(row0, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y)),
					 dot(row0, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z)),
					 dot(row0, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w)),

					 dot(row1, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x)), 
					 dot(row1, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y)),
					 dot(row1, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z)),
					 dot(row1, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w)),

					 dot(row2, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x)), 
					 dot(row2, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y)),
					 dot(row2, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z)),
					 dot(row2, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w)),

					 dot(row3, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x)), 
					 dot(row3, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y)),
					 dot(row3, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z)),
					 dot(row3, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w)));
	};

	inline const Matr4& Matr4::operator *=(const Matr4& m)
	{
		Matr4 old = *this;

		row0.x = dot(old.row0, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x));
		row0.y = dot(old.row0, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y));
		row0.z = dot(old.row0, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z));
		row0.w = dot(old.row0, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w));

		row1.x = dot(old.row1, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x));
		row1.y = dot(old.row1, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y));
		row1.z = dot(old.row1, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z));
		row1.w = dot(old.row1, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w));

		row2.x = dot(old.row2, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x));
		row2.y = dot(old.row2, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y));
		row2.z = dot(old.row2, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z));
		row2.w = dot(old.row2, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w));

		row3.x = dot(old.row3, Vect4(m.row0.x, m.row1.x, m.row2.x, m.row3.x));
		row3.y = dot(old.row3, Vect4(m.row0.y, m.row1.y, m.row2.y, m.row3.y));
		row3.z = dot(old.row3, Vect4(m.row0.z, m.row1.z, m.row2.z, m.row3.z));
		row3.w = dot(old.row3, Vect4(m.row0.w, m.row1.w, m.row2.w, m.row3.w));

		return *this;
	};

	inline Matr4 Matr4::operator *(const float f) const
	{
		return Matr4(row0*f, row1*f, row2*f, row3*f);
	};

	inline const Matr4& Matr4::operator *=(const float f)
	{
		row0 *= f;
		row1 *= f;
		row2 *= f;
		row3 *= f;
		return *this;
	};

	inline Matr4 Matr4::operator /(const float f) const
	{
		float f0 = 1/f;
		return Matr4(row0*f0, row1*f0, row2*f0, row3*f0);
	};

	inline const Matr4& Matr4::operator /=(const float f)
	{
		float f0 = 1/f;
		row0 *= f0;
		row1 *= f0;
		row2 *= f0;
		row3 *= f0;
		return *this;
	};

	inline Matr4 Matr4::operator -() const
	{
		return Matr4(-row0, -row1, -row2, -row3);
	};

	inline bool Matr4::operator ==(const Matr4& m) const
	{
		return ((row0 == m.row0) && (row1 == m.row1) && (row2 == m.row2) && (row3 == m.row3));
	};

	inline bool Matr4::operator !=(const Matr4& m) const
	{
		return ((row0 != m.row0) || (row1 != m.row1) || (row2 != m.row2) || (row3 != m.row3));
	};

	inline const Matr4& Matr4::transpose()
	{
		std::swap(_a01, _a10);
		std::swap(_a02, _a20);
		std::swap(_a03, _a30);
		std::swap(_a12, _a21);
		std::swap(_a13, _a31);
		std::swap(_a23, _a32);
		return *this;
	};

	inline Matr4 Matr4::getTranspose() const
	{
		return Matr4(_a00, _a10, _a20, _a30, 
					 _a01, _a11, _a21, _a31,
					 _a02, _a12, _a22, _a32, 
					 _a03, _a13, _a23, _a33);
	};

	inline Matr4 Matr4::getInverse() const
	{
		float mdet = 1/det();
		return Matr4(Vect4((row1.y*row2.z*row3.w+row2.y*row3.z*row1.w+row3.y*row1.z*row2.w-
			row1.y*row3.z*row2.w-row2.y*row1.z*row3.w-row3.y*row2.z*row1.w)*mdet,
			-(row0.y*row2.z*row3.w+row2.y*row3.z*row0.w+row3.y*row0.z*row2.w-
			row0.y*row3.z*row2.w-row2.y*row0.z*row3.w-row3.y*row2.z*row0.w)*mdet,
			(row0.y*row1.z*row3.w+row1.y*row3.z*row0.w+row3.y*row0.z*row1.w-
			row0.y*row3.z*row1.w-row1.y*row0.z*row3.w-row3.y*row1.z*row0.w)*mdet,
			-(row0.y*row1.z*row2.w+row1.y*row2.z*row0.w+row2.y*row0.z*row1.w-
			row0.y*row2.z*row1.w-row1.y*row0.z*row2.w-row2.y*row1.z*row0.w)*mdet),
			Vect4(-(row1.x*row2.z*row3.w+row2.x*row3.z*row1.w+row3.x*row1.z*row2.w-
			row1.x*row3.z*row2.w-row2.x*row1.z*row3.w-row3.x*row2.z*row1.w)*mdet,
			(row0.x*row2.z*row3.w+row2.x*row3.z*row0.w+row3.x*row0.z*row2.w-
			row0.x*row3.z*row2.w-row2.x*row0.z*row3.w-row3.x*row2.z*row0.w)*mdet,
			-(row0.x*row1.z*row3.w+row1.x*row3.z*row0.w+row3.x*row0.z*row1.w-
			row0.x*row3.z*row1.w-row1.x*row0.z*row3.w-row3.x*row1.z*row0.w)*mdet,
			(row0.x*row1.z*row2.w+row1.x*row2.z*row0.w+row2.x*row0.z*row1.w-
			row0.x*row2.z*row1.w-row1.x*row0.z*row2.w-row2.x*row1.z*row0.w)*mdet),
			Vect4((row1.x*row2.y*row3.w+row2.x*row3.y*row1.w+row3.x*row1.y*row2.w-
			row1.x*row3.y*row2.w-row2.x*row1.y*row3.w-row3.x*row2.y*row1.w)*mdet,
			-(row0.x*row2.y*row3.w+row2.x*row3.y*row0.w+row3.x*row0.y*row2.w-
			row0.x*row3.y*row2.w-row2.x*row0.y*row3.w-row3.x*row2.y*row0.w)*mdet,
			(row0.x*row1.y*row3.w+row1.x*row3.y*row0.w+row3.x*row0.y*row1.w-
			row0.x*row3.y*row1.w-row1.x*row0.y*row3.w-row3.x*row1.y*row0.w)*mdet,
			-(row0.x*row1.y*row2.w+row1.x*row2.y*row0.w+row2.x*row0.y*row1.w-
			row0.x*row2.y*row1.w-row1.x*row0.y*row2.w-row2.x*row1.y*row0.w)*mdet),
			Vect4(-(row1.x*row2.y*row3.z+row2.x*row3.y*row1.z+row3.x*row1.y*row2.z-
			row1.x*row3.y*row2.z-row2.x*row1.y*row3.z-row3.x*row2.y*row1.z)*mdet,
			(row0.x*row2.y*row3.z+row2.x*row3.y*row0.z+row3.x*row0.y*row2.z-
			row0.x*row3.y*row2.z-row2.x*row0.y*row3.z-row3.x*row2.y*row0.z)*mdet,
			-(row0.x*row1.y*row3.z+row1.x*row3.y*row0.z+row3.x*row0.y*row1.z-
			row0.x*row3.y*row1.z-row1.x*row0.y*row3.z-row3.x*row1.y*row0.z)*mdet,
			(row0.x*row1.y*row2.z+row1.x*row2.y*row0.z+row2.x*row0.y*row1.z-
			row0.x*row2.y*row1.z-row1.x*row0.y*row2.z-row2.x*row1.y*row0.z)*mdet));
	};

	inline Matr4 Matr4::getFastInverse() const
	{
		Vect3 trans(row0.w, row1.w, row2.w);
		return Matr4(_a00, _a10, _a20, -dot(trans, Vect3(_a00, _a10, _a20)),
					 _a01, _a11, _a21, -dot(trans, Vect3(_a01, _a11, _a21)),
					 _a02, _a12, _a22, -dot(trans, Vect3(_a02, _a12, _a22)),
					 0, 0, 0, 1);
	};

	inline const Matr4& Matr4::fastInvert()
	{
		Matr4 old = *this;
		Vect3 trans(row0.w, row1.w, row2.w);

		row0 = Vect4(old._a00, old._a10, old._a20, -dot(trans, Vect3(old._a00, old._a10, old._a20)));
		row1 = Vect4(old._a01, old._a11, old._a21, -dot(trans, Vect3(old._a01, old._a11, old._a21)));
		row2 = Vect4(old._a02, old._a12, old._a22, -dot(trans, Vect3(old._a02, old._a12, old._a22)));
		row3 = vect4_sing_w;
		return *this;
	};
	
	inline const Matr4& Matr4::invert()
	{
		Matr4 old = *this;
		float mdet = 1/det();

		row0 = Vect4((old.row1.y*old.row2.z*old.row3.w+old.row2.y*old.row3.z*old.row1.w+old.row3.y*old.row1.z*old.row2.w-
			old.row1.y*old.row3.z*old.row2.w-old.row2.y*old.row1.z*old.row3.w-old.row3.y*old.row2.z*old.row1.w)*mdet,
			-(old.row0.y*old.row2.z*old.row3.w+old.row2.y*old.row3.z*old.row0.w+old.row3.y*old.row0.z*old.row2.w-
			old.row0.y*old.row3.z*old.row2.w-old.row2.y*old.row0.z*old.row3.w-old.row3.y*old.row2.z*old.row0.w)*mdet,
			(old.row0.y*old.row1.z*old.row3.w+old.row1.y*old.row3.z*old.row0.w+old.row3.y*old.row0.z*old.row1.w-
			old.row0.y*old.row3.z*old.row1.w-old.row1.y*old.row0.z*old.row3.w-old.row3.y*old.row1.z*old.row0.w)*mdet,
			-(old.row0.y*old.row1.z*old.row2.w+old.row1.y*old.row2.z*old.row0.w+old.row2.y*old.row0.z*old.row1.w-
			old.row0.y*old.row2.z*old.row1.w-old.row1.y*old.row0.z*old.row2.w-old.row2.y*old.row1.z*old.row0.w)*mdet);
		row1 = Vect4(-(old.row1.x*old.row2.z*old.row3.w+old.row2.x*old.row3.z*old.row1.w+old.row3.x*old.row1.z*old.row2.w-
			old.row1.x*old.row3.z*old.row2.w-old.row2.x*old.row1.z*old.row3.w-old.row3.x*old.row2.z*old.row1.w)*mdet,
			(old.row0.x*old.row2.z*old.row3.w+old.row2.x*old.row3.z*old.row0.w+old.row3.x*old.row0.z*old.row2.w-
			old.row0.x*old.row3.z*old.row2.w-old.row2.x*old.row0.z*old.row3.w-old.row3.x*old.row2.z*old.row0.w)*mdet,
			-(old.row0.x*old.row1.z*old.row3.w+old.row1.x*old.row3.z*old.row0.w+old.row3.x*old.row0.z*old.row1.w-
			old.row0.x*old.row3.z*old.row1.w-old.row1.x*old.row0.z*old.row3.w-old.row3.x*old.row1.z*old.row0.w)*mdet,
			(old.row0.x*old.row1.z*old.row2.w+old.row1.x*old.row2.z*old.row0.w+old.row2.x*old.row0.z*old.row1.w-
			old.row0.x*old.row2.z*old.row1.w-old.row1.x*old.row0.z*old.row2.w-old.row2.x*old.row1.z*old.row0.w)*mdet);
		row2 = Vect4((old.row1.x*old.row2.y*old.row3.w+old.row2.x*old.row3.y*old.row1.w+old.row3.x*old.row1.y*old.row2.w-
			old.row1.x*old.row3.y*old.row2.w-old.row2.x*old.row1.y*old.row3.w-old.row3.x*old.row2.y*old.row1.w)*mdet,
			-(old.row0.x*old.row2.y*old.row3.w+old.row2.x*old.row3.y*old.row0.w+old.row3.x*old.row0.y*old.row2.w-
			old.row0.x*old.row3.y*old.row2.w-old.row2.x*old.row0.y*old.row3.w-old.row3.x*old.row2.y*old.row0.w)*mdet,
			(old.row0.x*old.row1.y*old.row3.w+old.row1.x*old.row3.y*old.row0.w+old.row3.x*old.row0.y*old.row1.w-
			old.row0.x*old.row3.y*old.row1.w-old.row1.x*old.row0.y*old.row3.w-old.row3.x*old.row1.y*old.row0.w)*mdet,
			-(old.row0.x*old.row1.y*old.row2.w+old.row1.x*old.row2.y*old.row0.w+old.row2.x*old.row0.y*old.row1.w-
			old.row0.x*old.row2.y*old.row1.w-old.row1.x*old.row0.y*old.row2.w-old.row2.x*old.row1.y*old.row0.w)*mdet);
		row3 = Vect4(-(old.row1.x*old.row2.y*old.row3.z+old.row2.x*old.row3.y*old.row1.z+old.row3.x*old.row1.y*old.row2.z-
			old.row1.x*old.row3.y*old.row2.z-old.row2.x*old.row1.y*old.row3.z-old.row3.x*old.row2.y*old.row1.z)*mdet,
			(old.row0.x*old.row2.y*old.row3.z+old.row2.x*old.row3.y*old.row0.z+old.row3.x*old.row0.y*old.row2.z-
			old.row0.x*old.row3.y*old.row2.z-old.row2.x*old.row0.y*old.row3.z-old.row3.x*old.row2.y*old.row0.z)*mdet,
			-(old.row0.x*old.row1.y*old.row3.z+old.row1.x*old.row3.y*old.row0.z+old.row3.x*old.row0.y*old.row1.z-
			old.row0.x*old.row3.y*old.row1.z-old.row1.x*old.row0.y*old.row3.z-old.row3.x*old.row1.y*old.row0.z)*mdet,
			(old.row0.x*old.row1.y*old.row2.z+old.row1.x*old.row2.y*old.row0.z+old.row2.x*old.row0.y*old.row1.z-
			old.row0.x*old.row2.y*old.row1.z-old.row1.x*old.row0.y*old.row2.z-old.row2.x*old.row1.y*old.row0.z)*mdet);
		return *this;
	};

	inline float Matr4::det() const
	{
		return (_a00*(_a11*(_a22*_a33 - _a23*_a32) - _a12*(_a21*_a33 - _a23*_a31) + _a13*(_a21*_a32 - _a22*_a31))
			   -_a01*(_a10*(_a22*_a33 - _a23*_a32) - _a12*(_a20*_a33 - _a23*_a30) + _a13*(_a20*_a32 - _a22*_a30))
			   +_a02*(_a10*(_a21*_a33 - _a23*_a31) - _a11*(_a20*_a33 - _a23*_a30) + _a13*(_a20*_a31 - _a21*_a30))
			   -_a03*(_a10*(_a21*_a32 - _a22*_a31) - _a11*(_a20*_a32 - _a22*_a30) + _a12*(_a20*_a31 - _a21*_a30)));
	};

	inline const Matr4& Matr4::setIdentity()
	{
		row0 = vect4_sing_x;
		row1 = vect4_sing_y;
		row2 = vect4_sing_z;
		row3 = vect4_sing_w;
		return *this;
	};

	inline const Matr4& Matr4::setTranslation(const Vect3& v)
	{
		row0.w = v.x;
		row1.w = v.y;
		row2.w = v.z;
		row3.w = 1;
		return *this;
	};

	inline const Matr4& Matr4::setScaling(const float f)
	{
		row0 = Vect4(f, 0, 0, row0.w);
		row1 = Vect4(0, f, 0, row1.w);
		row2 = Vect4(0, 0, f, row2.w);
		return *this;
	};

	inline const Matr4& Matr4::setScaling(const Vect3& v)
	{
		row0 = Vect4(v.x, 0, 0, row0.w);
		row1 = Vect4(0, v.y, 0, row1.w);
		row2 = Vect4(0, 0, v.z, row2.w);
		return *this;
	};

	inline const Matr4& Matr4::setRotation(const Vect3& v)
	{
		_private::_ApplyRotate(*this, v);
		return *this;
	};

	inline const Matr4& Matr4::setRotation(const Vect3& v, const float ang)
	{
		_private::_ApplyRotate(*this, ang, v);
		return *this;
	};

	inline const Matr4& Matr4::setRotation(const Matr3& m)
	{
		row0 = Vect4(m.row0.x, m.row0.y, m.row0.z, row0.w);
		row1 = Vect4(m.row1.x, m.row1.y, m.row1.z, row1.w);
		row2 = Vect4(m.row2.x, m.row2.y, m.row2.z, row2.w);
		return *this;
	};

	inline const Matr4& Matr4::ApplyTranslation(const Vect3& v)
	{
		*this *= Matr4().setOnlyTranslation(v);
		return *this;
	};

	inline const Matr4& Matr4::ApplyScaling(const float f)
	{
		*this *= Matr4().setOnlyScaling(f);
		return *this;
	};

	inline const Matr4& Matr4::ApplyScaling(const Vect3& v)
	{
		*this *= Matr4().setOnlyScaling(v);
		return *this;
	};

	inline const Matr4& Matr4::ApplyRotation(const Vect3& v)
	{
		*this *= Matr4().setOnlyRotation(v);
		return *this;
	};

	inline const Matr4& Matr4::ApplyRotation(const Vect3& v, const float ang)
	{
		*this *= Matr4().setOnlyRotation(v, ang);
		return *this;
	};

	inline const Matr4& Matr4::ApplyRotation(const Matr3& m)
	{
		*this *= Matr4().setOnlyRotation(m);
		return *this;
	};

	inline const Matr4& Matr4::setOnlyTranslation(const Vect3& v)
	{
		*this = matr4_sing;
		this->row0.w = v.x;
		this->row1.w = v.y;
		this->row2.w = v.z;
		return *this;
	};

	inline const Matr4& Matr4::setOnlyScaling(const float f)
	{
		*this = matr4_sing;
		this->row0.x = f;
		this->row1.y = f;
		this->row2.z = f;
		return *this;
	};

	inline const Matr4& Matr4::setOnlyScaling(const Vect3& v)
	{
		*this = matr4_sing;
		this->row0.x = v.x;
		this->row1.y = v.y;
		this->row2.z = v.z;
		return *this;
	};

	inline const Matr4& Matr4::setOnlyRotation(const Vect3& v)
	{
		*this = matr4_sing;
		_private::_ApplyRotate(*this, v);
		return *this;
	};

	inline const Matr4& Matr4::setOnlyRotation(const Vect3& v, const float ang)
	{
		*this = matr4_sing;
		_private::_ApplyRotate(*this, ang, v);
		return *this;
	};

	inline const Matr4& Matr4::setOnlyRotation(const Matr3& m)
	{
		*this = Matr4(m);
		return *this;
	};

	inline bool Matr4::getIsOrthogonalMatrix() const
	{
		return (float_equal(dot(row0, row1), 0) && float_equal(dot(row0, row2), 0) && 
				float_equal(dot(row0, row3), 0) && float_equal(dot(row1, row2), 0) &&
				float_equal(dot(row1, row3), 0) && float_equal(dot(row2, row3), 0));
	};

	// Matr34

	inline Matr34::Matr34()
	{

	};

	inline Matr34::Matr34(const Matr34& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
	};

	inline Matr34::Matr34(const float a00, const float a01, const float a02, const float a03, const float a10, const float a11, const float a12, const float a13, const float a20, const float a21, const float a22, const float a23)
	{
		_a00 = a00;
		_a01 = a01;
		_a02 = a02;
		_a03 = a03;

		_a10 = a10;
		_a11 = a11;
		_a12 = a12;
		_a13 = a13;

		_a20 = a20;
		_a21 = a21;
		_a22 = a22;
		_a23 = a23;
	};

	inline Matr34::Matr34(const Vect4& row0, const Vect4& row1, const Vect4& row2)
	{
		this->row0 = row0;
		this->row1 = row1;
		this->row2 = row2;
	};

	inline Matr34::Matr34(const Vect4* rows)
	{
		row0 = rows[0];
		row1 = rows[1];
		row2 = rows[2];
	};

	inline Matr34::Matr34(const Matr3& m)
	{
		row0 = Vect4(m.row0.x, m.row0.y, m.row0.z, 0);
		row1 = Vect4(m.row1.x, m.row1.y, m.row1.z, 0);
		row2 = Vect4(m.row2.x, m.row2.y, m.row2.z, 0);
	};

	inline Matr34::Matr34(const Matr4& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
	};

	inline Matr34::Matr34(const Matr3& m, const Vect3& trans, const Vect3& scale)
	{
		*this = Matr34(m);
		this->ApplyTranslation(trans);
		this->ApplyScaling(scale);
	};

	inline Matr34::Matr34(const Matr3& m, const Vect3& trans, const float f)
	{
		*this = Matr34(m);
		this->ApplyScaling(f);
		this->ApplyTranslation(trans);
	};

	inline Matr34::~Matr34()
	{

	};

	inline Matr34& Matr34::operator =(const Matr34& m)
	{
		row0 = m.row0;
		row1 = m.row1;
		row2 = m.row2;
		return *this;
	};

	inline const Vect4& Matr34::operator [](const unsigned int i) const
	{
		return rows[i];
	};

	inline Vect4& Matr34::operator [](const unsigned int i)
	{
		return rows[i];
	};

	inline Matr34 Matr34::operator +(const Matr34& m) const
	{
		return Matr34(row0 + m.row0, row1 + m.row1, row2 + m.row2);
	};

	inline const Matr34& Matr34::operator +=(const Matr34& m)
	{
		row0 += m.row0;
		row1 += m.row1;
		row2 += m.row2;
		return *this;
	};

	inline Matr34 Matr34::operator -(const Matr34& m) const
	{
		return Matr34(row0 - m.row0, row1 - m.row1, row2 - m.row2);
	};

	inline const Matr34& Matr34::operator -=(const Matr34& m)
	{
		row0 -= m.row0;
		row1 -= m.row1;
		row2 -= m.row2;
		return *this;
	};

	inline Matr34 Matr34::operator *(const Matr34& m) const
	{
		return Matr34(dot(row0, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					  dot(row0, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					  dot(row0, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					  dot(row0, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)),

					  dot(row1, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					  dot(row1, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					  dot(row1, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					  dot(row1, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)),

					  dot(row2, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					  dot(row2, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					  dot(row2, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					  dot(row2, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)));
	};

	inline const Matr34& Matr34::operator *=(const Matr34& m) 
	{
		Matr34 old = *this;

		row0 = Vect4(dot(old.row0, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					dot(old.row0, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					dot(old.row0, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					dot(old.row0, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)));

		row1 = Vect4(dot(old.row1, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					dot(old.row1, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					dot(old.row1, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					dot(old.row1, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)));

		row2 = Vect4(dot(old.row2, Vect4(m.row0.x, m.row1.x, m.row2.x, 0)),
					dot(old.row2, Vect4(m.row0.y, m.row1.y, m.row2.y, 0)),
					dot(old.row2, Vect4(m.row0.z, m.row1.z, m.row2.z, 0)),
					dot(old.row2, Vect4(m.row0.w, m.row1.w, m.row2.w, 1)));
		return *this;
	};

	inline Matr34 Matr34::operator *(const float f) const
	{
		return Matr34(row0*f, row1*f, row2*f);
	};

	inline const Matr34& Matr34::operator *=(const float f)
	{
		row0 *= f;
		row1 *= f;
		row2 *= f;
		return *this;
	};

	inline Matr34 Matr34::operator /(const float f) const
	{
		float f0 = 1/f;
		return Matr34(row0*f0, row1*f0, row2*f0);
	};

	inline const Matr34& Matr34::operator /=(const float f)
	{
		float f0 = 1/f;
		row0 *= f0;
		row1 *= f0;
		row2 *= f0;
		return *this;
	};

	inline Matr34 Matr34::operator -() const
	{
		return Matr34(-row0, -row1, -row2);
	};

	inline bool Matr34::operator ==(const Matr34& m) const
	{
		return ((row0 == m.row0) && (row1 == m.row1) && (row2 == m.row2));
	};

	inline bool Matr34::operator !=(const Matr34& m) const
	{
		return ((row0 != m.row0) || (row1 != m.row1) || (row2 != m.row2));
	};

	inline Matr34 Matr34::getInverse() const
	{
		return Matr34(Matr4(*this).getInverse());
	};

	inline Matr34 Matr34::getFastInverse() const
	{
		return Matr34(Matr4(*this).getFastInverse());
	};

	inline const Matr34& Matr34::invert()
	{
		*this = Matr34(Matr4(*this).getInverse());
		return *this;
	};

	inline const Matr34& Matr34::fastInvert()
	{
		*this = Matr34(Matr4(*this).getFastInverse());
		return *this;
	};

	inline const Matr34& Matr34::setIdentity()
	{
		row0 = vect4_sing_x;
		row1 = vect4_sing_y;
		row2 = vect4_sing_z;
		return *this;
	};

	inline const Matr34& Matr34::setTranslation(const Vect3& v)
	{
		row0.w = v.x;
		row1.w = v.y;
		row2.w = v.z;
		return *this;
	};

	inline const Matr34& Matr34::setScaling(const float f)
	{
		row0 = Vect4(f, 0, 0, row0.w);
		row1 = Vect4(0, f, 0, row1.w);
		row2 = Vect4(0, 0, f, row2.w);
		return *this;
	};

	inline const Matr34& Matr34::setScaling(const Vect3& v)
	{
		row0 = Vect4(v.x, 0, 0, row0.w);
		row1 = Vect4(0, v.y, 0, row1.w);
		row2 = Vect4(0, 0, v.z, row2.w);
		return *this;
	};

	inline const Matr34& Matr34::setRotation(const Vect3& v)
	{
		_private::_ApplyRotate(*this, v);
		return *this;
	};

	inline const Matr34& Matr34::setRotation(const Vect3& v, const float ang)
	{
		_private::_ApplyRotate(*this, ang, v);
		return *this;
	};

	inline const Matr34& Matr34::setRotation(const Matr3& m)
	{
		row0 = Vect4(m.row0.x, m.row0.y, m.row0.z, row0.w);
		row1 = Vect4(m.row1.x, m.row1.y, m.row1.z, row1.w);
		row2 = Vect4(m.row2.x, m.row2.y, m.row2.z, row2.w);
		return *this;
	};

	inline const Matr34& Matr34::ApplyTranslation(const Vect3& v)
	{
		*this *= Matr34().setOnlyTranslation(v);
		return *this;
	};

	inline const Matr34& Matr34::ApplyScaling(const float f)
	{
		*this *= Matr34().setOnlyScaling(f);
		return *this;
	};

	inline const Matr34& Matr34::ApplyScaling(const Vect3& v)
	{
		*this *= Matr34().setOnlyScaling(v);
		return *this;
	};

	inline const Matr34& Matr34::ApplyRotation(const Vect3& v)
	{
		*this *= Matr34().setOnlyRotation(v);
		return *this;
	};

	inline const Matr34& Matr34::ApplyRotation(const Vect3& v, const float ang)
	{
		*this *= Matr34().setOnlyRotation(v, ang);
		return *this;
	};

	inline const Matr34& Matr34::ApplyRotation(const Matr3& m)
	{
		*this *= Matr34().setOnlyRotation(m);
		return *this;
	};

	inline const Matr34& Matr34::setOnlyTranslation(const Vect3& v)
	{
		*this = matr34_sing;
		row0.w = v.x;
		row1.w = v.y;
		row2.w = v.z;
		return *this;
	};

	inline const Matr34& Matr34::setOnlyScaling(const float f)
	{
		*this = matr34_sing;
		row0.x = f;
		row1.y = f;
		row2.z = f;
		return *this;
	};

	inline const Matr34& Matr34::setOnlyScaling(const Vect3& v)
	{
		*this = matr34_sing;
		row0.x = v.x;
		row1.y = v.y;
		row2.z = v.z;
		return *this;
	};

	inline const Matr34& Matr34::setOnlyRotation(const Vect3& v)
	{
		*this = matr34_sing;
		_private::_ApplyRotate(*this, v);
		return *this;
	};

	inline const Matr34& Matr34::setOnlyRotation(const Vect3& v, const float ang)
	{
		*this = matr34_sing;
		_private::_ApplyRotate(*this, ang, v);
		return *this;
	};

	inline const Matr34& Matr34::setOnlyRotation(const Matr3& m)
	{
		*this = Matr34(m);
		return *this;
	};

	// friend operators

	inline Vect3 operator *(const Matr3& m, const Vect3& v)
	{
		return Vect3(dot(m.row0, v), dot(m.row1, v), dot(m.row2, v));
	};

	inline Vect4 operator *(const Matr4& m, const Vect4& v)
	{
		return Vect4(dot(m.row0, v), dot(m.row1, v), dot(m.row2, v), dot(m.row3, v));
	};

	inline Vect4 operator *(const Matr4& m, const Vect3& v)
	{
		Vect4 v4 = Vect4(v);
		return Vect4(dot(m.row0, v4), dot(m.row1, v4), dot(m.row2, v4), dot(m.row3, v4));
	};

	inline Vect3 operator *(const Matr34& m, const Vect4& v)
	{
		return Vect3(dot(m.row0, v), dot(m.row1, v), dot(m.row2, v));
	};

	inline Vect3 operator *(const Matr34& m, const Vect3 &v)
	{
		Vect4 v4 = Vect4(v);
		return Vect3(dot(m.row0, v4), dot(m.row1, v4), dot(m.row2, v4));
	};
};