#ifndef _MATRIX_H_
#define _MATRIX_H_

/* class Mat2, Mat3, Mat4, and Mat<N>
 *
 * General square matrix classes. The entries are stored in the member M,
 * in row-major format.
 *
 * Originally based off Matthew Fisher's Matrix4 class (now it's Mat4 here).
 *
 * Last modified: 2008-02-10
 */

#include <vDEC/include/Vector.h>
#include <vDEC/include/Point.h>

class Mat4{
private:
	real M[4][4];
public:
	/// Initializes the matrix to all zeroes.
	inline Mat4();
	/// Copies elements in from data in row-major dense format.
	inline Mat4(const real *data);
	inline Mat4(const Mat4 &mat);

	inline Mat4& operator = (const Mat4 &mat);

	inline Mat4& operator+=(const Mat4& mat);
	inline Mat4& operator-=(const Mat4& mat);
	inline Mat4& operator*=(const real &s);
	inline Mat4& operator*=(const Mat4 &mat);
	inline Mat4& operator/=(const real &s);

	/// Indexes the element in the i-th row and j-th column.
	inline real& operator()(unsigned int i, unsigned int j);
	/// Indexes the element in the i-th row and j-th column.
	inline const real& operator()(unsigned int i, unsigned int j) const;
	
	/// Returns the identity matrix.
	inline static const Mat4 Identity();
	/// Returns a matrix whose elements are the given vector along the diagonal.
	inline static Mat4 Diagonal(const Vec4 &d);
	/// Returns a scaling matrix for the given scale factors along the canonical basis directions.
	/// Each element of ScaleFactors is the scale factor in that direction.
	inline static Mat4 Scaling(const Vec3 &ScaleFactors);
	/// Returns a uniform scaling matrix.
	inline static Mat4 Scaling(const real &Scale);
	/// Returns a translation matrix for the given vector.
	inline static Mat4 Translation(const Vec3 &displacement);

	/// Returns a rotation matrix about a given axis for a given angle.
	inline static Mat4 Rotation(const Vec3 &Axis, real Angle);
	/// Returns a rotation matrix about a given axis (defined by Center and Axis), and a given angle.
	inline static Mat4 Rotation(const Vec3 &Axis, real Angle, const Vec3 &Center);

	/// Returns a transformation matrix to move the camera such that it is located at Eye, the central view target is At, and the up vector is Up.
	inline static Mat4 LookAt(const Vec3 &Eye, const Vec3 &At, const Vec3 &Up);
	/// Returns an orthogonal projection matrix.
	inline static Mat4 Orthogonal(real Width, real Height, real ZNear, real ZFar);
	/// Returns a perspective transform matrix.
	inline static Mat4 Perspective(real Width, real Height, real ZNear, real ZFar);
	/// Returns a perspective transform matrix given a field of view angle in radians.
	inline static Mat4 PerspectiveFov(real FOV, real Aspect, real ZNear, real ZFar);
	/// Returns a transformation matrix to make all vectors facing v1 face v2 instead.
	inline static Mat4 Face(const Vec3 &v1, const Vec3 &v2);
	/// Returns a viewport matrix.
	inline static Mat4 Viewport(real Width, real Height);

	/// Inverts the current matrix in place by LU factorization.
	inline Mat4& Invert();
	/// Transposes the current matrix in place.
	inline Mat4& Transpose();
	/// Returns the determinant matrix by a direct formula.
	inline real Determinant();
	
	/// Indexes a certain row of the matrix.
	inline real* operator [] (unsigned int Row);
	/// Indexes a certain row of the matrix.
	inline const real* operator [] (unsigned int Row) const;

	/// Returns a pointer to the data array (packed in dense row-major format). Suitable for use in glLoadMatrix().
	inline const real* Data() const;
	/// Returns a pointer to the data array (packed in dense row-major format). Suitable for use in glLoadMatrix().
	inline real* MutableData();

	/// Performs an affine transform of the point P (extends it to (x, y, z, 1), multiplies by this matrix, and returns the result.
	inline Pt3 AffineTransform(const Pt3 &P) const;
	/// Performs an affine transform of the vector V (extends it to (x, y, z, 0), multiplies by this matrix, and returns the result.
	inline Vec3 LinearTransform(const Vec3 &V) const;
	/// Performs an affine transform of the point P (extends it to (x, y, z, 1), multiplies by this matrix, renormalizes the resulting w component to be 1 and returns it.
	inline Pt3 AffineProjectionTransform(const Pt3 &P) const;
};

inline Mat4 operator * (const Mat4 &Left, const Mat4 &Right);
inline Mat4 operator * (const Mat4 &Left, const real &Right);
inline Mat4 operator * (const real &Left, const Mat4 &Right);
inline Mat4 operator + (const Mat4 &Left, const Mat4 &Right);
inline Mat4 operator - (const Mat4 &Left, const Mat4 &Right);
inline Vec4 operator * (const Vec4 &Right, const Mat4 &Left);

///////////////////////////////////////////////////////////////////////////////

class Mat3{
private:
	real M[3][3];

public:
	/// Initializes the matrix to all zeroes.
	inline Mat3();
	/// Copies elements in from data in row-major dense format.
	inline Mat3(const real *data);
	inline Mat3(const Mat3& mat);
	inline Mat3(const Vec3 &row1,const Vec3 &row2, const Vec3 &row3);

	/// Indexes the element in the i-th row and j-th column.
	inline real& operator()(unsigned int i, unsigned int j);
	/// Indexes the element in the i-th row and j-th column.
	inline real  operator()(unsigned int i, unsigned int j) const;
	/// Extracts the i-th column as a vector.
	inline Vec3 Col(unsigned int i) const;
	/// Extracts the i-th row as a vector.
	inline Vec3 Row(unsigned int i) const;

	/// Indexes a certain row of the matrix.
	inline real* operator [] (unsigned int Row);
	/// Indexes a certain row of the matrix.
	inline const real* operator [] (unsigned int Row) const;

	inline Mat3& operator=(const Mat3& mat);

	inline Mat3& operator+=(const Mat3& mat);
	inline Mat3& operator-=(const Mat3& mat);
	inline Mat3& operator*=(const real &s);
	inline Mat3& operator/=(const real &s);

	/// Returns the determinant by a direct formula.
	inline real Determinant() const;
	/// Returns the sum of the diagonal elements.
	inline real Trace() const;
	/// Performs an in place transpose.
	inline Mat3& Transpose();
	/// Performs an in place invert by LU factorization.
	inline Mat3& Invert();

	/// Returns the identity matrix.
	inline static const Mat3 Identity();

	/// Returns a matrix whose elements are the given vector along the diagonal.
	inline static Mat3 Diagonal(const Vec3 &d);

	/// Returns a pointer to the data array (packed in dense row-major format).
	inline const real* Data() const;
	/// Returns a pointer to the data array (packed in dense row-major format).
	inline real* MutableData();
};

inline Mat3 operator+(const Mat3& Left, const Mat3& Right);
inline Mat3 operator-(const Mat3& Left, const Mat3& Right);
inline Mat3 operator-(const Mat3& M); /// unary negation
inline Mat3 operator*(const real &s, const Mat3& M);
inline Mat3 operator*(const Mat3& M, const real &s);
inline Mat3 operator/(const Mat3& M, const real &s);
inline Vec3 operator*(const Mat3& M, const Vec3& V);
inline Mat3 operator*(const Mat3& Left, const Mat3& Right);
inline Mat3 OuterProduct(const Vec3& ColVec, const Vec3& RowVec);

///////////////////////////////////////////////////////////////////////////////

class Mat2{
private:
	real M[2][2];
public:
	/// Initializes the matrix to all zeroes.
	inline Mat2();
	/// Copies elements in from data in row-major dense format.
	inline Mat2(const real* data);
	inline Mat2(const Mat2& mat);
	inline Mat2(const Vec2 &row1, const Vec2 &row2);

	/// Indexes the element in the i-th row and j-th column.
	inline real& operator()(unsigned int i, unsigned int j);
	/// Indexes the element in the i-th row and j-th column.
	inline real  operator()(unsigned int i, unsigned int j) const;
	/// Extracts the i-th column as a vector.
	inline Vec2 Col(unsigned int i) const;
	/// Extracts the i-th row as a vector.
	inline Vec2 Row(unsigned int i) const;

	/// Indexes a certain row of the matrix.
	inline real* operator [] (unsigned int Row);
	/// Indexes a certain row of the matrix.
	inline const real* operator [] (unsigned int Row) const;

	inline Mat2& operator=(const Mat2& mat);

	inline Mat2& operator+=(const Mat2& mat);
	inline Mat2& operator-=(const Mat2& mat);
	inline Mat2& operator*=(const real &s);
	inline Mat2& operator/=(const real &s);

	/// Returns the determinant by a direct formula.
	inline real Determinant() const;
	/// Returns the sum of the diagonal elements.
	inline real Trace() const;
	/// Performs an in place transpose.
	inline Mat2& Transpose();
	/// Performs an in place inversion by a direct formula.
	inline Mat2& Invert();

	/// Returns the identity matrix.
	inline static const Mat2 Identity();
	/// Returns a matrix whose elements are the given vector along the diagonal.
	inline static Mat2 Diagonal(const Vec2 &d);
	
	/// Returns a pointer to the data array (packed in dense row-major format).
	inline const real* Data() const;
	/// Returns a pointer to the data array (packed in dense row-major format).
	inline real* MutableData();
};

inline Mat2 operator+(const Mat2& Left, const Mat2& Right);
inline Mat2 operator-(const Mat2& Left, const Mat2& Right);
inline Mat2 operator-(const Mat2& M); // unary negation
inline Mat2 operator*(const real &s, const Mat2& M);
inline Mat2 operator*(const Mat2& M, const real &s);
inline Mat2 operator/(const Mat2& M, const real &s);
inline Vec2 operator*(const Mat2& M, const Vec2& V);
inline Mat2 operator*(const Mat2& Left, const Mat2& Right);
inline Mat2 OuterProduct(const Vec2& ColVec, const Vec2& RowVec);

////////////////////////////////////////////////////////////////

template <unsigned int N>
class Mat{
private:
	real M[N][N];

public:
	Mat(){
		for(unsigned int i = 0; i < N; ++i){
			for(unsigned int j = 0; j < N; ++j){
				M[i][j] = 0;
			}
		}
	}
	Mat(const real *d){ // assumes row major data
		for(unsigned int i = 0; i < N; ++i){
			for(unsigned int j = 0; j < N; ++j){
				M[i][j] = d[i*N+j];
			}
		}
	}
	Mat(const Mat& mat){
		for(unsigned int i = 0; i < N; ++i){
			for(unsigned int j = 0; j < N; ++j){
				M[i][j] = mat.M[i][j];
			}
		}
	}

	real& operator()(unsigned int i, unsigned int j){ return M[i][j]; }
	real  operator()(unsigned int i, unsigned int j) const{ return M[i][j]; }

	// Assignment methods
	//
	Mat<N>& operator=(const Mat<N>& mat){
		if(this != &M){
			for(unsigned int i = 0; i < N; ++i){
				for(unsigned int j = 0; j < N; ++j){
					M[i][j] = mat.M[i][j];
				}
			}
		}
		return *this;
	}

	Mat<N>& operator+=(const Mat<N>& Right){
		for(unsigned int i = 0; i < 3; i++){
			for(unsigned int j = 0; j < 3; j++){
				M[i][j] += Right.M[i][j];
			}
		}
		return *this;
	}
	Mat<N>& operator-=(const Mat<N>& Right){
		for(unsigned int i = 0; i < 3; i++){
			for(unsigned int j = 0; j < 3; j++){
				M[i][j] -= Right.M[i][j];
			}
		}
		return *this;
	}
	Mat<N>& operator*=(const real &s){
		for(unsigned int i = 0; i < 3; i++){
			for(unsigned int j = 0; j < 3; j++){
				M[i][j] *= s;
			}
		}
		return *this;
	}
	Mat<N>& operator/=(const real &s){
		for(unsigned int i = 0; i < 3; i++){
			for(unsigned int j = 0; j < 3; j++){
				M[i][j] /= s;
			}
		}
		return *this;
	}

	real Determinant() const{
		for(unsigned int i = 1; i < N; ++i){
			M[0][i] /= M[0][0];
		}

		// LU decomposition
		for(unsigned int i = 1; i < N; ++i){
			for(unsigned int j = i; j < N; ++j){
				real sum = 0.0;
				for(unsigned int k = 0; k < i; ++k){
					sum += M[j][k] * M[k][i];
				}
				M[j][i] -= sum;
			}
			if(i == N-1) continue;
			for(unsigned int j = i+1; j < N; ++j){
				real sum = 0.0;
				for(unsigned int k = 0; k < i; ++k){
					sum += M[i][k]*M[k][j];
				}
				M[i][j] = (M[i][j]-sum) / M[i][i];
			}
		}
		real prod = (real)1.0;
		for(unsigned int i = 0; i < N; ++i){
			prod *= M[i][i];
		}
		return prod;
	}
	real Trace() const{
		real sum = 0;
		for(unsigned int i = 0; i < N; ++i){
			sum += M[i][i];
		}
		return sum;
	}
	Mat<N>& Transpose(){
		for(unsigned int i = 0; i < N; i++){
			for(unsigned int j = i+1; j < N; j++){
				std::swap(M[i][j], M[j][i]);
			}
		}
		return *this;
	}
	Mat<N>& Invert(){
		//Inversion by LU decomposition

		for(unsigned int i = 1; i < N; ++i){
			M[0][i] /= M[0][0];
		}

		for(unsigned int i = 1; i < N; ++i){
			for(unsigned int j = i; j < N; ++j){
				real sum = 0.0;
				for(unsigned int k = 0; k < i; ++k){
					sum += M[j][k] * M[k][i];
				}
				M[j][i] -= sum;
			}
			if(i == N-1) continue;
			for(unsigned int j = i+1; j < N; ++j){
				real sum = 0.0;
				for(unsigned int k = 0; k < i; ++k){
					sum += M[i][k]*M[k][j];
				}
				M[i][j] = (M[i][j]-sum) / M[i][i];
			}
		}

		for(unsigned int i = 0; i < N; ++i){ // invert L
			for(unsigned int j = i; j < N; ++j){
				real x = 1.0;
				if(i != j){
					x = 0.0;
					for(unsigned int k = i; k < j; ++k){
						x -= M[j][k]*M[k][i];
					}
				}
				M[j][i] = x / M[j][j];
			}
		}
		for(unsigned int i = 0; i < N; ++i){ // invert U
			for(unsigned int j = i; j < N; ++j){
				if(i == j){ continue; }
				real sum = 0.0;
				for(unsigned int k = i; k < j; ++k){
					sum += M[k][j]*( (i==k) ? 1.0f : M[i][k] );
				}
				M[i][j] = -sum;
			}
		}
		for(unsigned int i = 0; i < N; ++i){ // final inversion
			for(unsigned int j = 0; j < N; ++j){
				real sum = 0.0;
				for(unsigned int k = ((i>j)?i:j); k < N; k++ ){
					sum += ((j==k)?1.0f:M[j][k])*M[k][i];
				}
				M[j][i] = sum;
			}
		}
		return *this;
	}

	static Mat<N> Diagonal(const Vec<N> &d){
		Mat<N> ret;
		for(unsigned int i = 0; i < N; ++i){
			ret(i,i) = d[i];
		}
		return ret;
	}
	static Mat<N> Identity(){
		Mat<N> ret;
		for(unsigned int i = 0; i < N; ++i){
			ret(i,i) = 1;
		}
		return ret;
	}

	const real* Data() const{ return (const real*)M; }
	real* MutableData(){ return (real*)M; }
};

template <unsigned int N>
Mat<N> operator+(const Mat<N>& Left, const Mat<N>& Right){
	Mat<N> ret(Left);
	return (ret += Right);
}

template <unsigned int N>
Mat<N> operator-(const Mat<N>& Left, const Mat<N>& Right){
	Mat<N> ret(Left);
	return (ret -= Right);
}

template <unsigned int N>
Mat<N> operator-(const Mat<N>& M){ // unary negation
	Mat<N> ret(M);
	return (ret *= -1);
}

template <unsigned int N>
Mat<N> operator*(const real &s, const Mat<N>& M){
	Mat<N> ret(M);
	return (ret *= s);
}

template <unsigned int N>
Mat<N> operator*(const Mat<N>& M, const real &s){
	Mat<N> ret(M);
	return (ret *= s);
}

template <unsigned int N>
Mat<N> operator/(const Mat<N>& M, const real &s){
	Mat<N> ret(M);
	return (ret /= s);
}

template <unsigned int N>
Vec<N> operator*(const Mat<N>& M, const Vec<N>& V){
	Vec<N> ret;
	for(unsigned int i = 0; i < N; ++i){
		ret[i] = 0;
		for(unsigned int j = 0; j < N; ++j){
			ret[i] += M(i,j) * V[j];
		}
	}
	return ret;
}

template <unsigned int N>
Mat<N> operator*(const Mat<N> Left, const Mat<N>& Right){
	Mat<N> ret;
	for(unsigned int i = 0; i < N; ++i){
		for(unsigned int j = 0; j < N; ++j){
			ret(i,j) = 0;
			for(unsigned int k = 0; k < N; ++k){
				ret(i,j) += Left(i, k) * Right(k, j);
			}
		}
	}
	return ret;
}

/********************** Implementation for Mat4 **********************/

Mat4::Mat4(){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] = 0;
		}
	}
}

Mat4::Mat4(const real *data){
	unsigned int k = 0;
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] = data[k++];
		}
	}
}

Mat4::Mat4(const Mat4 &mat){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] = mat.M[i][j];
		}
	}
}

Mat4& Mat4::operator = (const Mat4 &mat){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] = mat.M[i][j];
		}
	}
	return *this;
}

Mat4& Mat4::operator+=(const Mat4& mat){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] += mat.M[i][j];
		}
	}
	return *this;
}

Mat4& Mat4::operator-=(const Mat4& mat){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] -= mat.M[i][j];
		}
	}
	return *this;
}

Mat4& Mat4::operator*=(const real &s){
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] += s;
		}
	}
	return *this;
}

Mat4& Mat4::operator*=(const Mat4 &mat){
	Mat4 ret;
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			ret(i,j) = (real)0.0;
			for(unsigned int k = 0; k < 4; ++k){
				ret(i,j) += M[i][k] * mat(k, j);
			}
		}
	}
	*this = ret;
	return *this;
}

Mat4& Mat4::operator/=(const real &s){
	real f = (real)1/s;
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			M[i][j] *= f;
		}
	}
	return *this;
}

real& Mat4::operator()(unsigned int i, unsigned int j){ return M[i][j]; }
const real& Mat4::operator()(unsigned int i, unsigned int j) const{ return M[i][j]; }

const Mat4 Mat4::Identity(){
	static const real identity_data_mat4[16] = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
	static const Mat4 I(identity_data_mat4);
	return I;
}

Mat4 Mat4::Diagonal(const Vec4 &d){
	Mat4 ret;
	ret(0,0) = d.x;
	ret(1,1) = d.y;
	ret(2,2) = d.z;
	ret(3,3) = d.w;
	return ret;
}

Mat4 Mat4::Scaling(const Vec3 &ScaleFactors){
	Mat4 ret;
	ret(0,0) = ScaleFactors.x;
	ret(1,0) = (real)0.0;
	ret(2,0) = (real)0.0;
	ret(3,0) = (real)0.0;

	ret(0,1) = (real)0.0;
	ret(1,1) = ScaleFactors.y;
	ret(2,1) = (real)0.0;
	ret(3,1) = (real)0.0;

	ret(0,2) = (real)0.0;
	ret(1,2) = (real)0.0;
	ret(2,2) = ScaleFactors.z;
	ret(3,2) = (real)0.0;

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)0.0;
	ret(3,3) = (real)1.0;
	return ret;
}

Mat4 Mat4::Scaling(const real &Scale){
	return Scaling(Vec3(Scale, Scale, Scale));
}

Mat4 Mat4::Translation(const Vec3 &displacement){
	Mat4 ret;
	ret(0,0) = (real)1.0;
	ret(1,0) = (real)0.0;
	ret(2,0) = (real)0.0;
	ret(3,0) = displacement.x;

	ret(0,1) = (real)0.0;
	ret(1,1) = (real)1.0;
	ret(2,1) = (real)0.0;
	ret(3,1) = displacement.y;

	ret(0,2) = (real)0.0;
	ret(1,2) = (real)0.0;
	ret(2,2) = (real)1.0;
	ret(3,2) = displacement.z;

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)0.0;
	ret(3,3) = (real)1.0;
	return ret;
}

Mat4 Mat4::Rotation(const Vec3 &Axis, real Angle){
	Mat4 ret;
	real c = VDEC_COS(Angle);
	real s = VDEC_SIN(Angle);
	real t = 1.0f - c;

	Vec3 Axis2 = Axis;
	Axis2.Normalize();
	real x = Axis2.x;
	real y = Axis2.y;
	real z = Axis2.z;

	ret(0,0) = (real)1.0 + t*(x*x -(real) 1.0);
	ret(0,1) = z*s + t*x*y;
	ret(0,2) = -y*s + t*x*z;
	ret(0,3) = (real)0.0;

	ret(1,0) = -z*s + t*x*y;
	ret(1,1) = (real)1.0 + t*(y*y - (real)1.0);
	ret(1,2) = x*s + t*y*z;
	ret(1,3) = (real)0.0;

	ret(2,0) = y*s + t*x*z;
	ret(2,1) = -x*s + t*y*z;
	ret(2,2) = (real)1.0 + t*(z*z - (real)1.0);
	ret(2,3) = (real)0.0;

	ret(3,0) = (real)0.0;
	ret(3,1) = (real)0.0;
	ret(3,2) = (real)0.0;
	ret(3,3) = (real)1.0;
	return ret;
}

Mat4 Mat4::Rotation(const Vec3 &Axis, real Angle, const Vec3 &Center){
	return Mat4::Translation(-Center) * Mat4::Rotation(Axis, Angle) * Mat4::Translation(Center);
}

Mat4 Mat4::LookAt(const Vec3 &Eye, const Vec3 &At, const Vec3 &Up){
	Mat4 ret;
	Vec3 ZAxis(Eye - At);
	Vec3 XAxis(Vec3::Cross(Up,ZAxis));
	XAxis.Normalize();
	Vec3 YAxis(Vec3::Cross(ZAxis,XAxis));

	ret(0,0) = XAxis.x;
	ret(1,0) = XAxis.y;
	ret(2,0) = XAxis.z;
	ret(3,0) = -Vec3::Dot(XAxis,Eye);

	ret(0,1) = YAxis.x;
	ret(1,1) = YAxis.y;
	ret(2,1) = YAxis.z;
	ret(3,1) = -Vec3::Dot(YAxis,Eye);

	ret(0,2) = ZAxis.x;
	ret(1,2) = ZAxis.y;
	ret(2,2) = ZAxis.z;
	ret(3,2) = -Vec3::Dot(ZAxis,Eye);

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)0.0;
	ret(3,3) = (real)1.0;
	return ret;
}

Mat4 Mat4::Orthogonal(real Width, real Height, real ZNear, real ZFar){
	Mat4 ret;
	ret(0,0) = (real)2.0 / Width;
	ret(1,0) = (real)0.0;
	ret(2,0) = (real)0.0;
	ret(3,0) = (real)0.0;

	ret(0,1) = (real)0.0;
	ret(1,1) = (real)2.0 / Height;
	ret(2,1) = (real)0.0;
	ret(3,1) = (real)0.0;

	ret(0,2) = (real)0.0;
	ret(1,2) = (real)0.0;
	ret(2,2) = (real)1.0 / (ZNear - ZFar);
	ret(3,2) = ZNear / (ZNear - ZFar);

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)0.0;
	ret(3,3) = (real)1.0;
	return ret;
}

Mat4 Mat4::Perspective(real Width, real Height, real ZNear, real ZFar){
	Mat4 ret;
	ret(0,0) = (real)2.0 * ZNear / Width;
	ret(1,0) = (real)0.0;
	ret(2,0) = (real)0.0;
	ret(3,0) = (real)0.0;

	ret(0,1) = (real)0.0;
	ret(1,1) = (real)2.0 * ZNear / Height;
	ret(2,1) = (real)0.0;
	ret(3,1) = (real)0.0;

	ret(0,2) = (real)0.0;
	ret(1,2) = (real)0.0;
	ret(2,2) = ZFar / (ZNear - ZFar);
	ret(3,2) = ZFar * ZNear / (ZNear - ZFar);

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)-1.0;
	ret(3,3) = (real)0.0;
	return ret;
}

Mat4 Mat4::PerspectiveFov(real FOV, real Aspect, real ZNear, real ZFar){
	Mat4 ret;
	real Width = (real)1.0/VDEC_TAN((real)0.5*FOV), Height = (real)Aspect/VDEC_TAN((real)0.5*FOV);

	ret(0,0) = Width;
	ret(1,0) = (real)0.0;
	ret(2,0) = (real)0.0;
	ret(3,0) = (real)0.0;

	ret(0,1) = (real)0.0;
	ret(1,1) = Height;
	ret(2,1) = (real)0.0;
	ret(3,1) = (real)0.0;

	ret(0,2) = (real)0.0;
	ret(1,2) = (real)0.0;
	ret(2,2) = ZFar / (ZNear - ZFar);
	ret(3,2) = ZFar * ZNear / (ZNear - ZFar);

	ret(0,3) = (real)0.0;
	ret(1,3) = (real)0.0;
	ret(2,3) = (real)-1.0;
	ret(3,3) = (real)0.0;
	return ret;
}

Mat4 Mat4::Face(const Vec3 &_v1, const Vec3 &_v2){
	//Rotate about the cross product of the two vectors by the angle between the two vectors
	Vec3 v1(_v1), v2(_v2);
	real Angle;
	v1.Normalize();
	v2.Normalize();
	Vec3 Axis(Vec3::Cross(v1, v2));
	real DotProduct = Vec3::Dot(v1,v2);
	if(DotProduct <= -1.0f){
		return Mat4::Scaling(-1);
	}
	if(DotProduct >= 1.0f){
		return Mat4::Identity();
	}
	Angle = VDEC_ACOS(DotProduct);

	if(Angle == 0.0f || Axis.Length() == 0.0f)
		return Mat4::Identity();
	else
		return Mat4::Rotation(Axis,Angle);
}

Mat4 Mat4::Viewport(real Width, real Height){
	Mat4 ret;
	ret(0,0) = Width;
	ret(1,0) = 0.0f;
	ret(2,0) = 0.0f;
	ret(3,0) = 0.0f;

	ret(0,1) = 0.0f;
	ret(1,1) = Height;
	ret(2,1) = 0.0f;
	ret(3,1) = 0.0f;

	ret(0,2) = 0.0f;
	ret(1,2) = 0.0f;
	ret(2,2) = 1.0f;
	ret(3,2) = 0.0f;

	ret(0,3) = 0.0f;
	ret(1,3) = 0.0f;
	ret(2,3) = 0.0f;
	ret(3,3) = 1.0f;
	return ret;
}

Mat4& Mat4::Invert(){
	//Inversion by LU decomposition
	for(unsigned int i = 1; i < 4; ++i){
		M[0][i] /= M[0][0];
	}

	for(unsigned int i = 1; i < 4; ++i){
		for(unsigned int j = i; j < 4; ++j){
			real sum = 0.0;
			for(unsigned int k = 0; k < i; ++k){
				sum += M[j][k] * M[k][i];
			}
			M[j][i] -= sum;
		}
		if(i == 4-1) continue;
		for(unsigned int j = i+1; j < 4; ++j){
			real sum = 0.0;
			for(unsigned int k = 0; k < i; ++k){
				sum += M[i][k]*M[k][j];
			}
			M[i][j] = (M[i][j]-sum) / M[i][i];
		}
	}

	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = i; j < 4; ++j){
			real x = 1.0;
			if(i != j){
				x = 0.0;
				for(unsigned int k = i; k < j; ++k){
					x -= M[j][k]*M[k][i];
				}
			}
			M[j][i] = x / M[j][j];
		}
	}
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = i; j < 4; ++j){
			if(i == j){ continue; }
			real sum = 0.0;
			for(unsigned int k = i; k < j; ++k){
				sum += M[k][j]*( (i==k) ? 1.0f : M[i][k] );
			}
			M[i][j] = -sum;
		}
	}
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			real sum = 0.0;
			for(unsigned int k = ((i>j)?i:j); k < 4; k++ ){
				sum += ((j==k)?1.0f:M[j][k])*M[k][i];
			}
			M[j][i] = sum;
		}
	}
	return *this;
}

Mat4& Mat4::Transpose(){
	for(unsigned int i = 0; i < 4; i++){
		for(unsigned int j = i+1; j < 4; j++){
			std::swap(M[i][j], M[j][i]);
		}
	}
	return *this;
}

real Mat4::Determinant(){
	return
		M[0][3] * M[1][2] * M[2][1] * M[3][0] - M[0][2] * M[1][3] * M[2][1] * M[3][0] - M[0][3] * M[1][1] * M[2][2] * M[3][0] + 
		M[0][1] * M[1][3] * M[2][2] * M[3][0] + M[0][2] * M[1][1] * M[2][3] * M[3][0] - M[0][1] * M[1][2] * M[2][3] * M[3][0] - 
		M[0][3] * M[1][2] * M[2][0] * M[3][1] + M[0][2] * M[1][3] * M[2][0] * M[3][1] + M[0][3] * M[1][0] * M[2][2] * M[3][1] - 
		M[0][0] * M[1][3] * M[2][2] * M[3][1] - M[0][2] * M[1][0] * M[2][3] * M[3][1] + M[0][0] * M[1][2] * M[2][3] * M[3][1] + 
		M[0][3] * M[1][1] * M[2][0] * M[3][2] - M[0][1] * M[1][3] * M[2][0] * M[3][2] - M[0][3] * M[1][0] * M[2][1] * M[3][2] + 
		M[0][0] * M[1][3] * M[2][1] * M[3][2] + M[0][1] * M[1][0] * M[2][3] * M[3][2] - M[0][0] * M[1][1] * M[2][3] * M[3][2] - 
		M[0][2] * M[1][1] * M[2][0] * M[3][3] + M[0][1] * M[1][2] * M[2][0] * M[3][3] + M[0][2] * M[1][0] * M[2][1] * M[3][3] - 
		M[0][0] * M[1][2] * M[2][1] * M[3][3] - M[0][1] * M[1][0] * M[2][2] * M[3][3] + M[0][0] * M[1][1] * M[2][2] * M[3][3];
}

real* Mat4::operator [] (unsigned int Row){ return M[Row]; }
const real* Mat4::operator [] (unsigned int Row) const{ return M[Row]; }

const real* Mat4::Data() const{ return (const real*)M; }
real* Mat4::MutableData(){ return (real*)M; }

Mat4 operator * (const Mat4 &Left, const Mat4 &Right){
	Mat4 ret;
	for(unsigned int i = 0; i < 4; ++i){
		for(unsigned int j = 0; j < 4; ++j){
			ret(i,j) = (real)0.0;
			for(unsigned int k = 0; k < 4; ++k){
				ret(i,j) += Left(i,k) * Right(k, j);
			}
		}
	}
	return ret;
}

Mat4 operator * (const Mat4 &Left, const real &Right){
	Mat4 Return;
	int i, i2;
	for(i=0; i<4; i++)
		for(i2=0; i2<4; i2++)
			Return[i][i2] = Left[i][i2]*Right;
	return Return;
}

Mat4 operator * (const real &Left, const Mat4 &Right){
	Mat4 Return;
	int i, i2;
	for(i=0; i<4; i++)
		for(i2=0; i2<4; i2++)
			Return[i][i2] = Right[i][i2]*Left;
	return Return;
}

Mat4 operator + (const Mat4 &Left, const Mat4 &Right){
	Mat4 Return;
	int i, i2;
	for(i=0; i<4; i++)
		for(i2=0; i2<4; i2++)
			Return[i][i2] = Left[i][i2] + Right[i][i2];
	return Return;
}

Mat4 operator - (const Mat4 &Left, const Mat4 &Right){
	Mat4 Return;
	int i, i2;
	for(i=0; i<4; i++)
		for(i2=0; i2<4; i2++)
			Return[i][i2] = Left[i][i2] - Right[i][i2];
	return Return;
}

Vec4 operator * (const Vec4 &Right, const Mat4 &Left){
	return Vec4(Right.x * Left[0][0] + Right.y * Left[1][0] + Right.z * Left[2][0] + Right.w * Left[3][0],
				Right.x * Left[0][1] + Right.y * Left[1][1] + Right.z * Left[2][1] + Right.w * Left[3][1],
				Right.x * Left[0][2] + Right.y * Left[1][2] + Right.z * Left[2][2] + Right.w * Left[3][2],
				Right.x * Left[0][3] + Right.y * Left[1][3] + Right.z * Left[2][3] + Right.w * Left[3][3]);
}

Pt3 Mat4::AffineTransform(const Pt3 &P) const{
	Vec4 Extended(P.e[0], P.e[1], P.e[2], (real)1);
	Extended = Extended * (*this);
	return Pt3(Extended.x, Extended.y, Extended.z);
}

Vec3 Mat4::LinearTransform(const Vec3 &V) const{
	Vec4 Extended(V.x, V.y, V.z, 0);
	Extended = Extended * (*this);
	return Vec3(Extended.x, Extended.y, Extended.z);
}

Pt3 Mat4::AffineProjectionTransform(const Pt3 &P) const{
	Vec4 Extended(P.e[0], P.e[1], P.e[2], (real)1);
	Extended = Extended * (*this);

	if(0 == Extended.w){
		return Pt3(Extended.x, Extended.y, Extended.z);
	}else{
		Pt3 ret(Extended.x / Extended.w, Extended.y / Extended.w, Extended.z / Extended.w);
		if(Extended.w < 0){
			return -ret;
		}else{
			return ret;
		}
	}
}


/********************** Implementation for Mat3 **********************/

Mat3::Mat3(){
	M[0][0] = M[0][1] = M[0][2] = M[1][0] = M[1][1] = M[1][2] = M[2][0] = M[2][1] = M[2][2] = (real)0.0;
}

Mat3::Mat3(const real *data){
	M[0][0] = data[0];
	M[0][1] = data[1];
	M[0][2] = data[2];
	M[1][0] = data[3];
	M[1][1] = data[4];
	M[1][2] = data[5];
	M[2][0] = data[6];
	M[2][1] = data[7];
	M[2][2] = data[8];
}

Mat3::Mat3(const Mat3& mat){
	for(unsigned int i = 0; i < 3; ++i){
		for(unsigned int j = 0; j < 3; ++j){
			M[i][j] = mat.M[i][j];
		}
	}
}

Mat3::Mat3(const Vec3 &row1,const Vec3 &row2, const Vec3 &row3){
	M[0][0] = row1.x;
	M[0][1] = row1.y;
	M[0][2] = row1.z;
	M[1][0] = row2.x;
	M[1][1] = row2.y;
	M[1][2] = row2.z;
	M[2][0] = row3.x;
	M[2][1] = row3.y;
	M[2][2] = row3.z;
}

real& Mat3::operator()(unsigned int i, unsigned int j){ return M[i][j]; }
real  Mat3::operator()(unsigned int i, unsigned int j) const{ return M[i][j]; }
Vec3 Mat3::Col(unsigned int i) const{ return Vec3(M[0][i],M[1][i],M[2][i]); }
Vec3 Mat3::Row(unsigned int i) const{ return Vec3(M[i][0],M[i][1],M[i][2]); }

real* Mat3::operator [] (unsigned int Row){ return M[Row]; }
const real* Mat3::operator [] (unsigned int Row) const{ return M[Row]; }

Mat3& Mat3::operator=(const Mat3& mat){
	if(this != &mat){
		for(unsigned int i = 0; i < 3; ++i){
			for(unsigned int j = 0; j < 3; ++j){
				M[i][j] = mat.M[i][j];
			}
		}
	}
	return *this;
}

Mat3& Mat3::operator+=(const Mat3& mat){
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			M[i][j] += mat.M[i][j];
		}
	}
	return *this;
}

Mat3& Mat3::operator-=(const Mat3& mat){
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			M[i][j] -= mat.M[i][j];
		}
	}
	return *this;
}

Mat3& Mat3::operator*=(const real &s){
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			M[i][j] *= s;
		}
	}
	return *this;
}

Mat3& Mat3::operator/=(const real &s){
	real f = (real)1/s;
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			M[i][j] *= f;
		}
	}
	return *this;
}

real Mat3::Determinant() const{
	return M[0][0] * (M[1][1] * M[2][2] - M[2][1] * M[1][2])
		 - M[0][1] * (M[1][0] * M[2][2] - M[2][0] * M[1][2])
		 + M[0][2] * (M[1][0] * M[2][1] - M[2][0] * M[1][1]);
}

real Mat3::Trace() const{ return M[0][0] + M[1][1] + M[2][2]; }

Mat3& Mat3::Transpose(){ 
	std::swap(M[0][1], M[1][0]);
	std::swap(M[0][2], M[2][0]);
	std::swap(M[2][1], M[1][2]);
	return *this;
}

Mat3& Mat3::Invert(){
	//Inversion by LU decomposition
	for(unsigned int i = 1; i < 3; ++i){
		M[0][i] /= M[0][0];
	}

	for(unsigned int i = 1; i < 3; ++i){
		for(unsigned int j = i; j < 3; ++j){
			real sum = 0.0;
			for(unsigned int k = 0; k < i; ++k){
				sum += M[j][k] * M[k][i];
			}
			M[j][i] -= sum;
		}
		if(i == 3-1) continue;
		for(unsigned int j = i+1; j < 3; ++j){
			real sum = 0.0;
			for(unsigned int k = 0; k < i; ++k){
				sum += M[i][k]*M[k][j];
			}
			M[i][j] = (M[i][j]-sum) / M[i][i];
		}
	}

	for(unsigned int i = 0; i < 3; ++i){
		for(unsigned int j = i; j < 3; ++j){
			real x = 1.0;
			if(i != j){
				x = 0.0;
				for(unsigned int k = i; k < j; ++k){
					x -= M[j][k]*M[k][i];
				}
			}
			M[j][i] = x / M[j][j];
		}
	}
	for(unsigned int i = 0; i < 3; ++i){
		for(unsigned int j = i; j < 3; ++j){
			if(i == j){ continue; }
			real sum = 0.0;
			for(unsigned int k = i; k < j; ++k){
				sum += M[k][j]*( (i==k) ? 1.0f : M[i][k] );
			}
			M[i][j] = -sum;
		}
	}
	for(unsigned int i = 0; i < 3; ++i){
		for(unsigned int j = 0; j < 3; ++j){
			real sum = 0.0;
			for(unsigned int k = ((i>j)?i:j); k < 3; k++ ){
				sum += ((j==k)?1.0f:M[j][k])*M[k][i];
			}
			M[j][i] = sum;
		}
	}
	return *this;
}

const Mat3 Mat3::Identity(){
	static const real identity_data_mat3[9] = {1,0,0,0,1,0,0,0,1};
	static const Mat3 I(identity_data_mat3);
	return I;
}

Mat3 Mat3::Diagonal(const Vec3 &d){
	Mat3 ret;
	ret(0,0) = d.x;
	ret(1,1) = d.y;
	ret(2,2) = d.z;
	return ret;
}

const real* Mat3::Data() const{
	return (const real*)M;
}
real* Mat3::MutableData(){
	return (real*)M;
}

Mat3 operator+(const Mat3& Left, const Mat3& Right){
	Mat3 ret(Left);
	ret += Right;
	return ret;
}

Mat3 operator-(const Mat3& Left, const Mat3& Right){
	Mat3 ret(Left);
	ret -= Right;
	return ret;
}

Mat3 operator-(const Mat3& M){
	Mat3 ret(M);
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			ret(i,j) = -ret(i,j);
		}
	}
	return ret;
}

Mat3 operator*(const real &s, const Mat3& M){
	Mat3 ret(M);
	ret *= s;
	return ret;
}

Mat3 operator*(const Mat3& M, const real &s){
	Mat3 ret(M);
	ret *= s;
	return ret;
}

Mat3 operator/(const Mat3& M, const real &s){
	Mat3 ret(M);
	ret /= s;
	return ret;
}

Vec3 operator*(const Mat3& mat, const Vec3& V){
	Vec3 ret(0,0,0);
	for(unsigned int j = 0; j < 3; j++){
		for(unsigned int i = 0; i < 3; i++){
			ret[i] += mat(i,j) * V[j];
		}
	}
	return ret;
}

Mat3 operator*(const Mat3& Left, const Mat3& Right){
	Mat3 ret;
	for(unsigned int i = 0; i < 3; i++){
		for(unsigned int j = 0; j < 3; j++){
			for(unsigned int k = 0; k < 3; k++){
				ret(i,k) += Left(i,j) * Right(j,k);
			}
		}
	}
	return ret;
}

Mat3 OuterProduct(const Vec3& ColVec, const Vec3& RowVec){
	Mat3 ret;
	for(unsigned int i = 0; i < 3; ++i){
		for(unsigned int j = 0; j < 3; ++j){
			ret(i,j) = RowVec[j] * ColVec[i];
		}
	}
	return ret;
}

/********************** Implementation for Mat2 **********************/

Mat2::Mat2(){
	M[0][0] = M[0][1] = M[1][0] = M[1][1] = (real)0.0;
}
Mat2::Mat2(const real* data){
	M[0][0] = data[0];
	M[0][1] = data[1];
	M[1][0] = data[2];
	M[1][1] = data[3];
}
Mat2::Mat2(const Mat2& mat){
	M[0][0] = mat.M[0][0];
	M[0][1] = mat.M[0][1];
	M[1][0] = mat.M[1][0];
	M[1][1] = mat.M[1][1];
}
Mat2::Mat2(const Vec2 &row1,const Vec2 &row2){
	M[0][0] = row1.x;
	M[0][1] = row1.y;
	M[1][0] = row2.x;
	M[1][1] = row2.y;
}

real& Mat2::operator()(unsigned int i, unsigned int j){ return M[i][j]; }
real  Mat2::operator()(unsigned int i, unsigned int j) const{ return M[i][j]; }
Vec2 Mat2::Col(unsigned int i) const{ return Vec2(M[0][i],M[1][i]); }
Vec2 Mat2::Row(unsigned int i) const{ return Vec2(M[i][0],M[i][1]); }

real* Mat2::operator [] (unsigned int Row){ return M[Row]; }
const real* Mat2::operator [] (unsigned int Row) const{ return M[Row]; }


Mat2& Mat2::operator=(const Mat2& mat){
	if(this != &mat){
		for(unsigned int i = 0; i < 2; i++){
			for(unsigned int j = 0; j < 2; j++){
				M[i][j] = mat.M[i][j];
			}
		}
	}
	return *this;
}

Mat2& Mat2::operator+=(const Mat2& mat){
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			M[i][j] += mat.M[i][j];
		}
	}
	return *this;
}
Mat2& Mat2::operator-=(const Mat2& mat){
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			M[i][j] -= mat.M[i][j];
		}
	}
	return *this;
}
Mat2& Mat2::operator*=(const real &s){
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			M[i][j] *= s;
		}
	}
	return *this;
}
Mat2& Mat2::operator/=(const real &s){
	real f = (real)1/s;
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			M[i][j] *= f;
		}
	}
	return *this;
}

real Mat2::Determinant() const{ return M[0][0] * M[1][1] - M[0][1] * M[1][0]; }
real Mat2::Trace() const{ return M[0][0] + M[1][1]; }
Mat2& Mat2::Transpose(){ std::swap(M[0][1], M[1][0]); return *this; }
Mat2& Mat2::Invert(){
	std::swap(M[0][1], M[1][0]);
	std::swap(M[0][0], M[1][1]);
	M[0][1] = -M[0][1];
	M[1][0] = -M[1][0];
	return (*this /= Determinant());
}

const Mat2 Mat2::Identity(){
	static const real identity_data_mat2[4] = {1,0,0,1};
	static const Mat2 I(identity_data_mat2);
	return I;
}

Mat2 Mat2::Diagonal(const Vec2 &d){
	Mat2 ret;
	ret(0,0) = d.x;
	ret(0,1) = ret(1,0) = (real)0.0;
	ret(1,1) = d.y;
	return ret;
}

const real* Mat2::Data() const{ return (const real*)M; }
real* Mat2::MutableData(){ return (real*)M; }

Mat2 operator+(const Mat2& Left, const Mat2& Right){
	Mat2 ret(Left);
	ret += Right;
	return ret;
}

Mat2 operator-(const Mat2& Left, const Mat2& Right){
	Mat2 ret(Left);
	ret -= Right;
	return ret;
}

Mat2 operator-(const Mat2& M){
	Mat2 ret(M);
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			ret(i,j) = -ret(i,j);
		}
	}
	return ret;
}

Mat2 operator*(const real &s, const Mat2& M){
	Mat2 ret(M);
	ret *= s;
	return ret;
}

Mat2 operator*(const Mat2& M, const real &s){
	Mat2 ret(M);
	ret *= s;
	return ret;
}

Mat2 operator/(const Mat2& M, const real &s){
	Mat2 ret(M);
	ret /= s;
	return ret;
}

Vec2 operator*(const Mat2& mat, const Vec2& V){
	Vec2 ret(0,0);
	for(unsigned int j = 0; j < 2; j++){
		for(unsigned int i = 0; i < 2; i++){
			ret[i] += mat(i,j) * V[j];
		}
	}
	return ret;
}

Mat2 operator*(const Mat2& Left, const Mat2& Right){
	Mat2 ret;
	for(unsigned int i = 0; i < 2; i++){
		for(unsigned int j = 0; j < 2; j++){
			for(unsigned int k = 0; k < 2; k++){
				ret(i,k) += Left(i,j) * Right(j,k);
			}
		}
	}
	return ret;
}

Mat2 OuterProduct(const Vec2& ColVec, const Vec2& RowVec){
	Mat2 ret;
	for(unsigned int i = 0; i < 2; ++i){
		for(unsigned int j = 0; j < 2; ++j){
			ret(i,j) = RowVec[j] * ColVec[i];
		}
	}
	return ret;
}

#endif