#pragma once

#include "vector3.h"
#include "vector4.h"

#include <limits>



namespace lm
{


//! 4次正方行列
/*
 
 インデックスは数学の行列と一致
 [ (0,0) , (0,1) , (0,2) , (0,3) ]
 [ (1,0) , (1,1) , (1,2) , (1,3) ]
 [ (2,0) , (2,1) , (2,2) , (2,3) ]
 [ (3,0) , (3,1) , (3,2) , (3,3) ]

 通しインデックスは
 [ 0  ,  1 ,  2 ,  3 ]
 [ 4  ,  5 ,  6 ,  7 ]
 [ 8  ,  9 , 10 , 11 ]
 [ 12 , 13 , 14 , 15 ]

 積の結合方向は

 [ax] = [bx by bz bw] * [ m00 m01 m02 m03 ]
 [ay]                   [ m10 m11 m12 m13 ]
 [az]                   [ m20 m21 m22 m23 ]
 [aw]                   [ m30 m31 m32 m33 ]

 [ax] = [ m00 m01 m02 m03 ] * [bx]
 [ay]   [ m10 m11 m12 m13 ]   [by]
 [az]   [ m20 m21 m22 m23 ]   [bz]
 [aw]   [ m30 m31 m32 m33 ]   [bw]

 ベクトルの行列変換の正方向は
 v' = v * m;

*/
template<typename T>
class matrix4
{
public:
	enum
	{
		WIDTH        =  4 , //!< 行,列の幅
		NUM_ELEMENTS = 16 , //!< 全要素数
	};

public:
	T m00 , m01 , m02 , m03 ,
	  m10 , m11 , m12 , m13 ,
	  m20 , m21 , m22 , m23 ,
	  m30 , m31 , m32 , m33 ;

public:
	matrix4(void)
		: m00(0) , m01(0) , m02(0) , m03(0)
		, m10(0) , m11(0) , m12(0) , m13(0)
		, m20(0) , m21(0) , m22(0) , m23(0)
		, m30(0) , m31(0) , m32(0) , m33(0) 
	{}

	matrix4(const T* _v)
		: m00(_v[ 0]) , m01(_v[ 1]) , m02(_v[ 2]) , m03(_v[ 3])
		, m10(_v[ 4]) , m11(_v[ 5]) , m12(_v[ 6]) , m13(_v[ 7])
		, m20(_v[ 8]) , m21(_v[ 9]) , m22(_v[10]) , m23(_v[11])
		, m30(_v[12]) , m31(_v[13]) , m32(_v[14]) , m33(_v[15])
	{}

	matrix4(const matrix4<T>& m)
		: m00(m.m00) , m01(m.m01) , m02(m.m02) , m03(m.m03)
		, m10(m.m10) , m11(m.m11) , m12(m.m12) , m13(m.m13)
		, m20(m.m20) , m21(m.m21) , m22(m.m22) , m23(m.m23)
		, m30(m.m30) , m31(m.m31) , m32(m.m32) , m33(m.m33)
	{}


	// バッファの先頭を直接参照
	T*       v(void)       { return &m00; }
	const T* v(void) const { return &m00; }


	// 通しインデックスで要素取得
	      T& operator[](size_t idx);
	const T& operator[](size_t idx) const;
	      T& operator()(size_t idx);
	const T& operator()(size_t idx) const;

	      T& at(size_t idx);
	const T& at(size_t idx) const;


	// row(行) , col(列) のインデックスで要素取得
	      T& operator()(size_t idx_row, size_t idx_col);
	const T& operator()(size_t idx_row, size_t idx_col) const;

	      T& at(size_t idx_row, size_t idx_col);
	const T& at(size_t idx_row, size_t idx_col) const;


	void set( const matrix4<T>& m );
	void set( const T* _v );
	void set( const T& _v00 , const T& _v01 , const T& _v02 , const T& _v03 ,
	          const T& _v10 , const T& _v11 , const T& _v12 , const T& _v13 ,
	          const T& _v20 , const T& _v21 , const T& _v22 , const T& _v23 ,
	          const T& _v30 , const T& _v31 , const T& _v32 , const T& _v33 );

	matrix4<T>& operator=( const matrix4<T>& m );

	void get( matrix4<T>& m ) const;
	void get( T* v ) const;

	//! 全要素に同じ値をセットする
	void fill( const T& _v );

	//! 単位行列化
	void set_identity(void);
	//! 零行列化
	void set_zero(void);

	//! 単位行列生成
	static const matrix4<T>& get_identity(void);
	//! 零行列生成
	static const matrix4<T>& get_zero(void);

	//! 転置
	void transpose(void);
	matrix4<T> get_transpose(void) const;

	//! 逆行列
	void invert(void);
	matrix4<T> get_invert(void) const;

	T get_det(void) const;

	//! 平行移動行列
	static matrix4<T> get_translate( const vector3<T>& v );
	static matrix4<T> get_translate( const T& x , const T& y , const T& z );
	void set_translate( const vector3<T>& v );
	void set_translate( const T& x , const T& y , const T& z );
	void translate( const vector3<T>& v );
	void translate( const T& x , const T& y , const T& z );

	//! 回転行列
	static matrix4<T> get_rotate( const vector3<T>& axis , const T& angle );
	static matrix4<T> get_rotate_x( const T& angle );
	static matrix4<T> get_rotate_y( const T& angle );
	static matrix4<T> get_rotate_z( const T& angle );
	void set_rotate( const vector3<T>& axis , const T& angle );
	void set_rotate_x( const T& angle );
	void set_rotate_y( const T& angle );
	void set_rotate_z( const T& angle );
	void rotate( const vector3<T>& axis , const T& angle );
	void rotate_x( const T& angle );
	void rotate_y( const T& angle );
	void rotate_z( const T& angle );

	//! スケール行列
	static matrix4<T> get_scale( const T& scale_xyz );
	static matrix4<T> get_scale( const vector3<T>& scale_xyz );
	static matrix4<T> get_scale( const T& scale_x , const T& scale_y , const T& scale_z );
	void set_scale( const T& scale_xyz );
	void set_scale( const vector3<T>& scale_xyz );
	void set_scale( const T& scale_x , const T& scale_y , const T& scale_z );
	void scale( const T& scale_xyz );
	void scale( const vector3<T>& scale_xyz );
	void scale( const T& scale_x , const T& scale_y , const T& scale_z );


	// 指定した行(横方向)の要素取得 [i,0][i,1][i,2][i,3]
	void set_row( size_t row_idx , const T& v0 , const T& v1 , const T& v2 , const T& v3 );
	void set_row( size_t row_idx , const vector3<T>& v );
	void set_row( size_t row_idx , const vector4<T>& v );
	void get_row( size_t row_idx , T& v0 , T& v1 , T& v2 ) const;
	void get_row( size_t row_idx , T& v0 , T& v1 , T& v2 , T& v3 ) const;
	void get_row( size_t row_idx , vector3<T>& v ) const;
	void get_row( size_t row_idx , vector4<T>& v ) const;

	// 指定した列(縦方向)の要素取得 [0,i][1,i][2,i][3,i]
	void set_col( size_t col_idx , const T& v0 , const T& v1 , const T& v2 , const T& v3 );
	void set_col( size_t col_idx , const vector3<T>& v );
	void set_col( size_t col_idx , const vector4<T>& v );
	void get_col( size_t col_idx , T& v0 , T& v1 , T& v2 ) const;
	void get_col( size_t col_idx , T& v0 , T& v1 , T& v2 , T& v3 ) const;
	void get_col( size_t col_idx , vector3<T>& v ) const;
	void get_col( size_t col_idx , vector4<T>& v ) const;


	//! 指定した基底行列の, ローカルからグローバルへの直交変換行列化
	// vec_global = vec_local * m
	void set_ortho_trans( const vector3<T>& ex , const vector3<T>& ey , const vector3<T>& ez );
	//! 指定した基底行列の, グローバルからローカルへの直交変換行列化
	// vec_local = vec_global * m
	void set_ortho_trans_rev( const vector3<T>& ex , const vector3<T>& ey , const vector3<T>& ez );


	bool equals( const matrix4<T>& m ) const;
	bool equals( const T* _v ) const;
	bool operator==( const matrix4<T>& m ) const;
	bool operator!=( const matrix4<T>& m ) const;


	matrix4<T>& operator+=( const matrix4<T>& m );
	matrix4<T>& operator-=( const matrix4<T>& m );
	matrix4<T>& operator*=( const T& val );
	matrix4<T>& operator/=( const T& val );

	matrix4<T>& operator*=( const matrix4<T>& m );
};

typedef matrix4<float>  matrix4f;
typedef matrix4<double> matrix4d;



// global methods

template<typename T>
matrix4<T> operator+( const matrix4<T>& m1 , const matrix4<T>& m2 );
template<typename T>
matrix4<T> operator-( const matrix4<T>& m1 , const matrix4<T>& m2 );
template<typename T>
matrix4<T> operator*( const matrix4<T>& m1 , const matrix4<T>& m2 );

template<typename T>
matrix4<T> operator*( const matrix4<T>& m1 , const T& val );
template<typename T>
matrix4<T> operator*( const T& val , const matrix4<T>& m1 );

template<typename T>
matrix4<T> operator/( const matrix4<T>& m1 , const T& val );

// vector3 との積
// ※ { v *= m } == { v = v * m } != { v = m * v }
template<typename T>
vector3<T>& operator*=( vector3<T>& v , const matrix4<T>& m );
template<typename T>
vector3<T> operator*( const vector3<T>& v , const matrix4<T>& m );

template<typename T>
vector3<T> operator*( const matrix4<T>& m , const vector3<T>& v );


template<typename T>
vector4<T>& operator*=( vector4<T>& v , const matrix4<T>& m );
template<typename T>
vector4<T> operator*( const vector4<T>& v , const matrix4<T>& m );

template<typename T>
vector4<T> operator*( const matrix4<T>& m , const vector4<T>& v );



// matrix4 implementation 

template<typename T> inline
T& matrix4<T>::operator[](size_t idx)
{
	return at(idx);
}
template<typename T> inline
const T& matrix4<T>::operator[](size_t idx) const
{
	return at(idx);
}
template<typename T> inline
T& matrix4<T>::operator()(size_t idx)
{
	return at(idx);
}
template<typename T> inline
const T& matrix4<T>::operator()(size_t idx) const 
{
	return at(idx);
}
template<typename T> inline
T& matrix4<T>::operator()(size_t idx_row, size_t idx_col)
{
	return at( idx_row , idx_col );
}
template<typename T> inline
const T& matrix4<T>::operator()(size_t idx_row, size_t idx_col) const
{
	return at( idx_row , idx_col );
}

template<typename T> inline
T& matrix4<T>::at(size_t idx)
{
	return v()[idx];
}

template<typename T> inline
const T& matrix4<T>::at(size_t idx) const
{
	return v()[idx];
}

template<typename T> inline
T& matrix4<T>::at(size_t idx_row, size_t idx_col)
{
	return v()[ idx_col + idx_row * WIDTH ];
}

template<typename T> inline
const T& matrix4<T>::at(size_t idx_row, size_t idx_col) const
{
	return v()[ idx_col + idx_row * WIDTH ];
}


template<typename T> inline
void matrix4<T>::set( const matrix4<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = m.at(i);
}

template<typename T> inline
void matrix4<T>::set( const T* _v )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = _v[i];
}

template<typename T> inline
void matrix4<T>::set( const T& _v00 , const T& _v01 , const T& _v02 , const T& _v03 ,
                      const T& _v10 , const T& _v11 , const T& _v12 , const T& _v13 ,
                      const T& _v20 , const T& _v21 , const T& _v22 , const T& _v23 ,
                      const T& _v30 , const T& _v31 , const T& _v32 , const T& _v33 )
{
	m00 = _v00;   m01 = _v01;   m02 = _v02;   m03 = _v03;
	m10 = _v10;   m11 = _v11;   m12 = _v12;   m13 = _v13;
	m20 = _v20;   m21 = _v21;   m22 = _v22;   m23 = _v23;
	m30 = _v30;   m31 = _v31;   m32 = _v32;   m33 = _v33;
}

template<typename T> inline
matrix4<T>& matrix4<T>::operator=( const matrix4<T>& m )
{
	set(m);
	return *this;
}


template<typename T> inline
void matrix4<T>::fill( const T& _v )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		this->at(i) = _v;
}


template<typename T> inline
void matrix4<T>::set_identity(void)
{
	set( get_identity() );
}

template<typename T> inline
void matrix4<T>::set_zero(void)
{
	set( get_zero() );
}

template<typename T> inline
const matrix4<T>& matrix4<T>::get_identity(void)
{
	static const T v[NUM_ELEMENTS]
		= { T(1) , T(0) , T(0) , T(0) ,
		    T(0) , T(1) , T(0) , T(0) ,
		    T(0) , T(0) , T(1) , T(0) ,
		    T(0) , T(0) , T(0) , T(1) };

	static const matrix4<T> m(v);
	return m;
}

template<typename T> inline
const matrix4<T>& matrix4<T>::get_zero(void)
{
	static const T v[NUM_ELEMENTS]
		= { T(0) , T(0) , T(0) , T(0) ,
		    T(0) , T(0) , T(0) , T(0) ,
		    T(0) , T(0) , T(0) , T(0) ,
		    T(0) , T(0) , T(0) , T(0) };

	static const matrix4<T> m(v);
	return m;
}


template<typename T> inline
void matrix4<T>::transpose(void)
{
	(std::swap)( m01 , m10 );
	(std::swap)( m02 , m20 );
	(std::swap)( m03 , m30 );
	(std::swap)( m12 , m21 );
	(std::swap)( m13 , m31 );
	(std::swap)( m23 , m32 );
}

template<typename T> inline
matrix4<T> matrix4<T>::get_transpose(void) const
{
	matrix4<T> m = (*this);
	m.transpose();
	return m;
}

template<typename T> inline
T matrix4<T>::get_det(void) const
{
	T a0 = (at(0,0)*at(1,1) - at(0,1)*at(1,0));
	T a1 = (at(0,0)*at(1,2) - at(0,2)*at(1,0));
	T a2 = (at(0,0)*at(1,3) - at(0,3)*at(1,0));
	T a3 = (at(0,1)*at(1,2) - at(0,2)*at(1,1));
	T a4 = (at(0,1)*at(1,3) - at(0,3)*at(1,1));
	T a5 = (at(0,2)*at(1,3) - at(0,3)*at(1,2));
	T b0 = (at(2,0)*at(3,1) - at(2,1)*at(3,0));
	T b1 = (at(2,0)*at(3,2) - at(2,2)*at(3,0));
	T b2 = (at(2,0)*at(3,3) - at(2,3)*at(3,0));
	T b3 = (at(2,1)*at(3,2) - at(2,2)*at(3,1));
	T b4 = (at(2,1)*at(3,3) - at(2,3)*at(3,1));
	T b5 = (at(2,2)*at(3,3) - at(2,3)*at(3,2));

	T det = + a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
	return det;
}

template<typename T> inline
void matrix4<T>::invert(void)
{
	matrix4<T> inv_mat;

	T a0 = (at(0,0)*at(1,1) - at(0,1)*at(1,0));
	T a1 = (at(0,0)*at(1,2) - at(0,2)*at(1,0));
	T a2 = (at(0,0)*at(1,3) - at(0,3)*at(1,0));
	T a3 = (at(0,1)*at(1,2) - at(0,2)*at(1,1));
	T a4 = (at(0,1)*at(1,3) - at(0,3)*at(1,1));
	T a5 = (at(0,2)*at(1,3) - at(0,3)*at(1,2));
	T b0 = (at(2,0)*at(3,1) - at(2,1)*at(3,0));
	T b1 = (at(2,0)*at(3,2) - at(2,2)*at(3,0));
	T b2 = (at(2,0)*at(3,3) - at(2,3)*at(3,0));
	T b3 = (at(2,1)*at(3,2) - at(2,2)*at(3,1));
	T b4 = (at(2,1)*at(3,3) - at(2,3)*at(3,1));
	T b5 = (at(2,2)*at(3,3) - at(2,3)*at(3,2));

	T det = + a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
	if (det <= (std::numeric_limits<T>::min)())
	{
		(*this) = inv_mat;
		return;
	}

	inv_mat(0,0) = + at(1,1)*b5 - at(1,2)*b4 + at(1,3)*b3;
	inv_mat(0,1) = - at(0,1)*b5 + at(0,2)*b4 - at(0,3)*b3;
	inv_mat(0,2) = + at(3,1)*a5 - at(3,2)*a4 + at(3,3)*a3;
	inv_mat(0,3) = - at(2,1)*a5 + at(2,2)*a4 - at(2,3)*a3;
	inv_mat(1,0) = - at(1,0)*b5 + at(1,2)*b2 - at(1,3)*b1;
	inv_mat(1,1) = + at(0,0)*b5 - at(0,2)*b2 + at(0,3)*b1;
	inv_mat(1,2) = - at(3,0)*a5 + at(3,2)*a2 - at(3,3)*a1;
	inv_mat(1,3) = + at(2,0)*a5 - at(2,2)*a2 + at(2,3)*a1;
	inv_mat(2,0) = + at(1,0)*b4 - at(1,1)*b2 + at(1,3)*b0;
	inv_mat(2,1) = - at(0,0)*b4 + at(0,1)*b2 - at(0,3)*b0;
	inv_mat(2,2) = + at(3,0)*a4 - at(3,1)*a2 + at(3,3)*a0;
	inv_mat(2,3) = - at(2,0)*a4 + at(2,1)*a2 - at(2,3)*a0;
	inv_mat(3,0) = - at(1,0)*b3 + at(1,1)*b1 - at(1,2)*b0;
	inv_mat(3,1) = + at(0,0)*b3 - at(0,1)*b1 + at(0,2)*b0;
	inv_mat(3,2) = - at(3,0)*a3 + at(3,1)*a1 - at(3,2)*a0;
	inv_mat(3,3) = + at(2,0)*a3 - at(2,1)*a1 + at(2,2)*a0;

	T det_inv = T(1) / det;

	inv_mat *= det_inv;

	(*this) = inv_mat;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_invert(void) const
{
	matrix4<T> m = (*this);
	m.invert();
	return m;
}


template<typename T> inline
matrix4<T> matrix4<T>::get_translate( const vector3<T>& v )
{
	matrix4<T> m;
	m.set_translate(v);
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_translate( const T& x , const T& y , const T& z )
{
	matrix4<T> m;
	m.set_translate(x, y, z);
	return m;
}

template<typename T> inline
void matrix4<T>::set_translate( const vector3<T>& v )
{
	set_translate(v.x, v.y, v.z);
}

template<typename T> inline
void matrix4<T>::set_translate( const T& x , const T& y , const T& z )
{
	set( 1.0f , 0.0f , 0.0f , 0.0f ,
	     0.0f , 1.0f , 0.0f , 0.0f ,
	     0.0f , 0.0f , 1.0f , 0.0f ,
	     x    , y    , z    , 1.0f );
}

template<typename T> inline
void matrix4<T>::translate( const vector3<T>& v )
{
	(*this) *= get_translate(v);
}

template<typename T> inline
void matrix4<T>::translate( const T& x , const T& y , const T& z )
{
	(*this) *= get_translate(x, y, z);
}


template<typename T> inline
matrix4<T> matrix4<T>::get_rotate( const vector3<T>& axis , const T& angle )
{
	matrix4<T> m;
	m.set_rotate(axis, angle);
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_rotate_x( const T& angle )
{
	matrix4<T> m;
	m.set_rotate_x(angle);
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_rotate_y( const T& angle )
{
	matrix4<T> m;
	m.set_rotate_y(angle);
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_rotate_z( const T& angle )
{
	matrix4<T> m;
	m.set_rotate_z(angle);
	return m;
}

template<typename T> inline
void matrix4<T>::set_rotate( const vector3<T>& axis , const T& angle )
{
	T c = cos(angle)  , s = sin(angle);
	T mc = T(1) - c ;
	const T& ax = axis.x ;  const T& ay = axis.y ;  const T& az = axis.z ;
	set( ( ax * ax * mc + c      ) , ( ay * ax * mc + az * s ) , ( az * ax * mc - ay * s ) , 0.0f ,
	     ( ax * ay * mc - az * s ) , ( ay * ay * mc + c      ) , ( az * ay * mc + ax * s ) , 0.0f ,
	     ( ax * az * mc + ay * s ) , ( ay * az * mc - ax * s ) , ( az * az * mc + c      ) , 0.0f ,
	     0.0f                      , 0.0f                      , 0.0f                      , 1.0f );
}

template<typename T> inline
void matrix4<T>::set_rotate_x( const T& angle )
{
	T c = cos(angle)  , s = sin(angle);
	set( 1.0f , 0.0f , 0.0f , 0.0f ,
	     0.0f , c    , s    , 0.0f ,
	     0.0f , -s   , c    , 0.0f ,
	     0.0f , 0.0f , 0.0f , 1.0f );
}

template<typename T> inline
void matrix4<T>::set_rotate_y( const T& angle )
{
	T c = cos(angle)  , s = sin(angle);
	set( c    , 0.0f , -s   , 0.0f ,
	     0.0f , 1.0f , 0.0f , 0.0f ,
	     s    , 0.0f , c    , 0.0f ,
	     0.0f , 0.0f , 0.0f , 1.0f );
}

template<typename T> inline
void matrix4<T>::set_rotate_z( const T& angle )
{
	T c = cos(angle)  , s = sin(angle);
	set( c    , s    , 0.0f , 0.0f ,
	     -s   , c    , 0.0f , 0.0f ,
	     0.0f , 0.0f , 1.0f , 0.0f ,
	     0.0f , 0.0f , 0.0f , 1.0f );
}


template<typename T> inline
void matrix4<T>::rotate( const vector3<T>& axis , const T& angle )
{
	(*this) *= get_rotate(axis, angle);
}

template<typename T> inline
void matrix4<T>::rotate_x( const T& angle )
{
	(*this) *= get_rotate_x(angle);
}

template<typename T> inline
void matrix4<T>::rotate_y( const T& angle )
{
	(*this) *= get_rotate_y(angle);
}

template<typename T> inline
void matrix4<T>::rotate_z( const T& angle )
{
	(*this) *= get_rotate_z(angle);
}


template<typename T> inline
matrix4<T> matrix4<T>::get_scale( const T& scale_xyz )
{
	matrix4<T> m;
	m.set_scale( scale_xyz );
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_scale( const vector3<T>& scale_xyz )
{
	matrix4<T> m;
	m.set_scale( scale_xyz );
	return m;
}

template<typename T> inline
matrix4<T> matrix4<T>::get_scale( const T& scale_x , const T& scale_y , const T& scale_z )
{
	matrix4<T> m;
	m.set_scale( scale_x , scale_y , scale_z );
	return m;
}

template<typename T> inline
void matrix4<T>::set_scale( const T& scale_xyz )
{
	set_scale(scale_xyz, scale_xyz, scale_xyz);
}

template<typename T> inline
void matrix4<T>::set_scale( const vector3<T>& scale_xyz )
{
	set_scale(scale_xyz.x, scale_xyz.y, scale_xyz.z);
}

template<typename T> inline
void matrix4<T>::set_scale( const T& scale_x , const T& scale_y , const T& scale_z )
{
	set( scale_x , 0.0f    , 0.0f    , 0.0f ,
	     0.0f    , scale_y , 0.0f    , 0.0f ,
	     0.0f    , 0.0f    , scale_z , 0.0f ,
	     0.0f    , 0.0f    , 0.0f    , 1.0f );
}

template<typename T> inline
void matrix4<T>::scale( const T& scale_xyz )
{
	(*this) *= get_scale(scale_xyz);
}

template<typename T> inline
void matrix4<T>::scale( const vector3<T>& scale_xyz )
{
	(*this) *= get_scale(scale_xyz);
}

template<typename T> inline
void matrix4<T>::scale( const T& scale_x , const T& scale_y , const T& scale_z )
{
	(*this) *= get_scale(scale_x, scale_y, scale_z);
}


template<typename T> inline
void matrix4<T>::set_row( size_t row_idx , const T& v0 , const T& v1 , const T& v2 , const T& v3 )
{
	at( row_idx , 0 ) = v0;
	at( row_idx , 1 ) = v1;
	at( row_idx , 2 ) = v2;
	at( row_idx , 3 ) = v3;
}

template<typename T> inline
void matrix4<T>::set_row( size_t row_idx , const vector3<T>& v )
{
	set_row( row_idx , v.x , v.y , v.z , T(0) );
}

template<typename T> inline
void matrix4<T>::set_row( size_t row_idx , const vector4<T>& v )
{
	set_row( row_idx , v.x , v.y , v.z , v.w );
}

template<typename T> inline
void matrix4<T>::get_row( size_t row_idx , T& v0 , T& v1 , T& v2 ) const
{
	v0 = at( row_idx , 0 );
	v1 = at( row_idx , 1 );
	v2 = at( row_idx , 2 );
}

template<typename T> inline
void matrix4<T>::get_row( size_t row_idx , T& v0 , T& v1 , T& v2 , T& v3 ) const
{
	v0 = at( row_idx , 0 );
	v1 = at( row_idx , 1 );
	v2 = at( row_idx , 2 );
	v3 = at( row_idx , 3 );
}

template<typename T> inline
void matrix4<T>::get_row( size_t row_idx , vector3<T>& v ) const
{
	get_row( row_idx , v.x , v.y , v.z );
}

template<typename T> inline
void matrix4<T>::get_row( size_t row_idx , vector4<T>& v ) const
{
	get_row( row_idx , v.x , v.y , v.z , v.w );
}


template<typename T> inline
void matrix4<T>::set_col( size_t col_idx , const T& v0 , const T& v1 , const T& v2 , const T& v3 )
{
	at( 0 , col_idx ) = v0;
	at( 1 , col_idx ) = v1;
	at( 2 , col_idx ) = v2;
	at( 3 , col_idx ) = v3;
}

template<typename T> inline
void matrix4<T>::set_col( size_t col_idx , const vector3<T>& v )
{
	set_col( col_idx , v.x , v.y , v.z , T(0) );
}

template<typename T> inline
void matrix4<T>::set_col( size_t col_idx , const vector4<T>& v )
{
	set_col( col_idx , v.x , v.y , v.z , v.w );
}

template<typename T> inline
void matrix4<T>::get_col( size_t col_idx , T& v0 , T& v1 , T& v2 ) const
{
	v0 = at( 0 , col_idx );
	v1 = at( 1 , col_idx );
	v2 = at( 2 , col_idx );
}

template<typename T> inline
void matrix4<T>::get_col( size_t col_idx , T& v0 , T& v1 , T& v2 , T& v3 ) const
{
	v0 = at( 0 , col_idx );
	v1 = at( 1 , col_idx );
	v2 = at( 2 , col_idx );
	v3 = at( 3 , col_idx );
}

template<typename T> inline
void matrix4<T>::get_col( size_t col_idx , vector3<T>& v ) const
{
	get_col( col_idx , v.x , v.y , v.z );
}

template<typename T> inline
void matrix4<T>::get_col( size_t col_idx , vector4<T>& v ) const
{
	get_col( col_idx , v.x , v.y , v.z , v.w );
}


template<typename T> inline
void matrix4<T>::set_ortho_trans( const vector3<T>& ex , const vector3<T>& ey , const vector3<T>& ez )
{
	set_col( 0 , ex.x , ex.y , ex.z , T(0) );
	set_col( 1 , ey.x , ey.y , ey.z , T(0) );
	set_col( 2 , ez.x , ez.y , ez.z , T(0) );
	set_col( 3 , T(0) , T(0) , T(0) , T(1) );
}

template<typename T> inline
void matrix4<T>::set_ortho_trans_rev( const vector3<T>& ex , const vector3<T>& ey , const vector3<T>& ez )
{
	set_col( 0 , ex.x , ey.x , ex.z , T(0) );
	set_col( 1 , ex.y , ey.y , ey.z , T(0) );
	set_col( 2 , ex.z , ey.z , ez.z , T(0) );
	set_col( 3 , T(0) , T(0) , T(0) , T(1) );
}


template<typename T> inline
bool matrix4<T>::equals( const matrix4<T>& m ) const
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		if( m.at(i) != at(i) ) return false;
	return true;
}

template<typename T> inline
bool matrix4<T>::equals( const T* _v ) const
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		if( m[i] != at(i) ) return false;
	return true;
}

template<typename T> inline
bool matrix4<T>::operator==( const matrix4<T>& m ) const
{
	return equals( m );
}

template<typename T> inline
bool matrix4<T>::operator!=( const matrix4<T>& m ) const
{
	return !equals( m );
}


template<typename T> inline
matrix4<T>& matrix4<T>::operator+=( const matrix4<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) += m[i];

	return *this;
}

template<typename T> inline
matrix4<T>& matrix4<T>::operator-=( const matrix4<T>& m )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) -= m[i];

	return *this;
}

template<typename T> inline
matrix4<T>& matrix4<T>::operator*=( const T& val )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) *= val;

	return *this;
}

template<typename T> inline
matrix4<T>& matrix4<T>::operator/=( const T& val )
{
	for( size_t i = 0 ; i < NUM_ELEMENTS ; ++i )
		at(i) /= val;

	return *this;
}

template<typename T> inline
matrix4<T>& matrix4<T>::operator*=( const matrix4<T>& m )
{
	matrix4<T> tmp;

	tmp.m00 = this->m00 * m.m00 + this->m01 * m.m10 + this->m02 * m.m20 + this->m03 * m.m30;
	tmp.m10 = this->m10 * m.m00 + this->m11 * m.m10 + this->m12 * m.m20 + this->m13 * m.m30;
	tmp.m20 = this->m20 * m.m00 + this->m21 * m.m10 + this->m22 * m.m20 + this->m23 * m.m30;
	tmp.m30 = this->m30 * m.m00 + this->m31 * m.m10 + this->m32 * m.m20 + this->m33 * m.m30;

	tmp.m01 = this->m00 * m.m01 + this->m01 * m.m11 + this->m02 * m.m21 + this->m03 * m.m31;
	tmp.m11 = this->m10 * m.m01 + this->m11 * m.m11 + this->m12 * m.m21 + this->m13 * m.m31;
	tmp.m21 = this->m20 * m.m01 + this->m21 * m.m11 + this->m22 * m.m21 + this->m23 * m.m31;
	tmp.m31 = this->m30 * m.m01 + this->m31 * m.m11 + this->m32 * m.m21 + this->m33 * m.m31;

	tmp.m02 = this->m00 * m.m02 + this->m01 * m.m12 + this->m02 * m.m22 + this->m03 * m.m32;
	tmp.m12 = this->m10 * m.m02 + this->m11 * m.m12 + this->m12 * m.m22 + this->m13 * m.m32;
	tmp.m22 = this->m20 * m.m02 + this->m21 * m.m12 + this->m22 * m.m22 + this->m23 * m.m32;
	tmp.m32 = this->m30 * m.m02 + this->m31 * m.m12 + this->m32 * m.m22 + this->m33 * m.m32;

	tmp.m03 = this->m00 * m.m03 + this->m01 * m.m13 + this->m02 * m.m23 + this->m03 * m.m33;
	tmp.m13 = this->m10 * m.m03 + this->m11 * m.m13 + this->m12 * m.m23 + this->m13 * m.m33;
	tmp.m23 = this->m20 * m.m03 + this->m21 * m.m13 + this->m22 * m.m23 + this->m23 * m.m33;
	tmp.m33 = this->m30 * m.m03 + this->m31 * m.m13 + this->m32 * m.m23 + this->m33 * m.m33;

	(*this) = tmp;

	return *this;
}



// global method implements

template<typename T> inline
matrix4<T> operator+( const matrix4<T>& m1 , const matrix4<T>& m2 )
{
	matrix4<T> ret = m1;
	ret += m1;
	return ret;
}

template<typename T> inline
matrix4<T> operator-( const matrix4<T>& m1 , const matrix4<T>& m2 )
{
	matrix4<T> ret = m1;
	ret -= m1;
	return ret;
}

template<typename T> inline
matrix4<T> operator*( const matrix4<T>& m1 , const matrix4<T>& m2 )
{
	matrix4<T> ret = m1;
	ret *= m2;
	return ret;
}

template<typename T> inline
matrix4<T> operator*( const matrix4<T>& m1 , const T& val )
{
	matrix4<T> ret = m1;
	ret *= val;
	return ret;
}

template<typename T> inline
matrix4<T> operator*( const T& val , const matrix4<T>& m1 )
{
	matrix4<T> ret = m1;
	ret *= val;
	return ret;
}

template<typename T> inline
matrix4<T> operator/( const matrix4<T>& m1 , const T& val )
{
	matrix4<T> ret = m1;
	ret /= val;
	return ret;
}

template<typename T> inline
vector3<T>& operator*=( vector3<T>& v , const matrix4<T>& m )
{
	vector3<T> tmp = v;

	v.x = tmp.x * m.m00 + tmp.y * m.m10 + tmp.z * m.m20 + m.m30;
	v.y = tmp.x * m.m01 + tmp.y * m.m11 + tmp.z * m.m21 + m.m31;
	v.z = tmp.x * m.m02 + tmp.y * m.m12 + tmp.z * m.m22 + m.m32;

	// 同次座標->通常の3D座標への変換
	float w = tmp.x * m.m03 + tmp.y * m.m13 + tmp.z * m.m23 + m.m33;
	v /= w;

	return v;
}

template<typename T> inline
vector3<T> operator*( const vector3<T>& v , const matrix4<T>& m )
{
	vector3<T> ret = v;
	ret *= m;
	return ret;
}

template<typename T> inline
vector3<T> operator*( const matrix4<T>& m , const vector3<T>& v )
{
	vector3<T> ret;

	ret.x = v.x * m.m00 + v.y * m.m01 + v.z * m.m02 + m.m03;
	ret.y = v.x * m.m10 + v.y * m.m11 + v.z * m.m12 + m.m13;
	ret.z = v.x * m.m20 + v.y * m.m21 + v.z * m.m22 + m.m23;

	// 同次座標->通常の3D座標への変換
	float w = v.x * m.m30 + v.y * m.m31 + v.z * m.m32 + m.m33;
	ret /= w;

	return ret;
}


template<typename T>
vector4<T>& operator*=( vector4<T>& v , const matrix4<T>& m )
{
	vector4<T> tmp = v;

	v.x = tmp.x * m.m00 + tmp.y * m.m10 + tmp.z * m.m20 + tmp.w * m.m30;
	v.y = tmp.x * m.m01 + tmp.y * m.m11 + tmp.z * m.m21 + tmp.w * m.m31;
	v.z = tmp.x * m.m02 + tmp.y * m.m12 + tmp.z * m.m22 + tmp.w * m.m32;
	v.w = tmp.x * m.m03 + tmp.y * m.m13 + tmp.z * m.m23 + tmp.w * m.m33;

	return v;
}

template<typename T>
vector4<T> operator*( const vector4<T>& v , const matrix4<T>& m )
{
	vector4<T> ret = v;
	ret *= m;
	return ret;
}

template<typename T>
vector4<T> operator*( const matrix4<T>& m , const vector4<T>& v )
{
	vector4<T> ret;

	ret.x = v.x * m.m00 + v.y * m.m01 + v.z * m.m02 + v.w * m.m03;
	ret.y = v.x * m.m10 + v.y * m.m11 + v.z * m.m12 + v.w * m.m13;
	ret.z = v.x * m.m20 + v.y * m.m21 + v.z * m.m22 + v.w * m.m23;
	ret.w = v.x * m.m30 + v.y * m.m31 + v.z * m.m32 + v.w * m.m33;

	return ret;
}


}
