module util.matrix;
private import util.vector;
private import std.math;
private import util.cloneable;
public class Matrix:Cloneable{
	public double[4][4] m;
	public this(){
		for(int i = m.length - 1;0 <= i;i --){
			for(int j = m[i].length - 1;0 <= j;j --){
				if( i == j)m[i][j] = 1;
				else m[i][j] = 0;
			}
		}
	}
	public this(Matrix t){
		for(int i = m.length - 1;0 <= i;i --){
			for(int j = m[i].length - 1;0 <= j;j --){
				m[i][j] = t.m[i][j];
			}
		}
	}
	public Object clone(){
		return new Matrix(this);
	}
	public Matrix opMul(Matrix t) {
    Matrix result = new Matrix();
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				result.m[i][j] = m[i][0] * t.m[0][j] +  m[i][1] * t.m[1][j] 
					+ m[i][2] * t.m[2][j] + m[i][3] * t.m[3][j];
			}
		}
		return result;
  }
	public Matrix opMul(double mul) {
    Matrix result = new Matrix();
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				result.m[i][j] = m[i][j] * mul;
			}
		}
		return result;
  }
	public void opMulAssign(Matrix t) {
		double[4][4] temp;
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				temp[i][j]  = m[i][0] * t.m[0][j] +  m[i][1] * t.m[1][j] 
					+ m[i][2] * t.m[2][j] + m[i][3] * t.m[3][j];
			}
		}
		for(int i = 0;i < m.length;i ++){
			for(int j = 0;j < m[i].length;j ++){
				m[i][j] = temp[i][j];
			}
		}
	}
}
public Matrix matZero(){
	Matrix result = new Matrix();
	for(int i = 0;i < 4;i ++){
		for(int j = 0;j < 4;j ++){
			result.m[i][j] = 0;
		}
	}
	return result;
}
public Matrix matRotate(double deg ,double x, double y ,double z){
	Vector3 u = vec3Normalize(new Vector3(x ,y ,z));
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	double[4][4] UU;
	UU[0][0] = u.x * u.x;UU[0][1] = u.x * u.y;UU[0][2] = u.x * u.z;UU[0][3] =  0;
	UU[1][0] = u.y * u.x;UU[1][1] = u.y * u.y;UU[1][2] = u.y * u.z;UU[1][3] =  0;
	UU[2][0] = u.z * u.x;UU[2][1] = u.z * u.y;UU[2][2] = u.z * u.z;UU[2][3] =  0;
	UU[3][0] =  0;			 UU[3][1] =  0;				UU[3][2] =  0;			 UU[3][3] =  0;
	double[4][4] T ;
	T[0][0] = cos + (1 - cos) * UU[0][0]; T[0][1] = (1 - cos) * UU[0][1] - u.z * sin; T[0][2] = (1 - cos) * UU[0][2] + u.y * sin;T[0][3] = 0;
	T[1][0] = (1 - cos) * UU[1][0] + u.z * sin; T[1][1] = cos + (1 - cos) * UU[1][1]; T[1][2] = (1 - cos) * UU[1][2] - u.x * sin;T[1][3] = 0;
	T[2][0] = (1 - cos) * UU[2][0] - u.y * sin; T[2][1] = (1 - cos) * UU[2][1] + u.x * sin; T[2][2] = cos + (1 - cos) * UU[2][2];T[2][3] = 0;
	T[3][0] = 0																; T[3][1] = 0																; T[3][2] = 0													;T[3][3] = 1;
	for(int i = 0;i < 4;i ++){
		for(int j = 0;j < 4;j ++){
			result.m[i][j] = T[i][j];
		}
	}
	return result;
}
public Matrix matRotateX(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = 1;   result.m[0][1] = 0;    result.m[0][2] = 0;   	result.m[0][3] = 0;
	result.m[1][0] = 0;   result.m[1][1] = cos;  result.m[1][2] = sin; 	result.m[1][3] = 0;
	result.m[2][0] = 0;   result.m[2][1] = -sin; result.m[2][2] = cos; 	result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0;   	result.m[3][3] = 1;
	return result;
}

public Matrix matRotateY(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = cos; result.m[0][1] = 0;    result.m[0][2] = -sin;	result.m[0][3] = 0;
	result.m[1][0] = 0;   result.m[1][1] = 1;  	 result.m[1][2] = 0;		result.m[1][3] = 0;
	result.m[2][0] = sin; result.m[2][1] = 0; 	 result.m[2][2] = cos; 	result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0;   	result.m[3][3] = 1;
	return result;
}
public Matrix matRotateZ(double deg){
	Matrix result = new Matrix();
	double sin = sin(deg * PI / 180.0);
	double cos = cos(deg * PI / 180.0);
	result.m[0][0] = cos; result.m[0][1] = sin; result.m[0][2] = 0; result.m[0][3] = 0;
	result.m[1][0] = -sin; result.m[1][1] = cos;  result.m[1][2] = 0; result.m[1][3] = 0;
	result.m[2][0] = 0;   result.m[2][1] = 0; 	 result.m[2][2] = 1; result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0; result.m[3][3] = 1;
	return result;
}
public Matrix matRotateXYZ(double degx ,double degy ,double degz){
	Matrix result = new Matrix();
	double sx = sin(degx * PI / 180.0);
	double sy = sin(degy * PI / 180.0);
	double sz = sin(degz * PI / 180.0);
	double cx = cos(degx * PI / 180.0);
	double cy = cos(degy * PI / 180.0);
	double cz = cos(degz * PI / 180.0);
	result.m[0][0] = cy*cz; result.m[0][1] = cy*sz; result.m[0][2] = -sy; result.m[0][3] = 0;
	result.m[1][0] = sx*sy*cz-cx*sz; result.m[1][1] = sx*sy*sz+cx*cz;  result.m[1][2] = sx*cy; result.m[1][3] = 0;
	result.m[2][0] = cx*sy*cz+sx*sz;   result.m[2][1] = cx*sy*sz-sx*cz; 	 result.m[2][2] = cx*cy; result.m[2][3] = 0;
	result.m[3][0] = 0;   result.m[3][1] = 0;    result.m[3][2] = 0; result.m[3][3] = 1;
	return result;
}
public Matrix matTranslate(double x,double y,double z){
	Matrix result = new Matrix();
	result.m[0][0] = 1; result.m[0][1] = 0;  result.m[0][2] = 0; 	result.m[0][3] = 0;
	result.m[1][0] = 0;	result.m[1][1] = 1;  result.m[1][2] = 0; 	result.m[1][3] = 0;
	result.m[2][0] = 0; result.m[2][1] = 0;  result.m[2][2] = 1; 	result.m[2][3] = 0;
	result.m[3][0] = x; result.m[3][1] = y;  result.m[3][2] = z;  result.m[3][3] = 1;
	return result;
}
public Matrix matScale(double sx,double sy,double sz){
	Matrix result = new Matrix();
	result.m[0][0] = sx;result.m[0][1] = 0;  result.m[0][2] = 0; 	result.m[0][3] = 0;
	result.m[1][0] = 0;	result.m[1][1] = sy; result.m[1][2] = 0; 	result.m[1][3] = 0;
	result.m[2][0] = 0; result.m[2][1] = 0;  result.m[2][2] = sz; result.m[2][3] = 0;
	result.m[3][0] = 0; result.m[3][1] = 0;  result.m[3][2] = 0;  result.m[3][3] = 1;
	return result;
}
public Vector3 vec3translate(Vector3 v,Matrix t){
	Vector3 result;
	double[4] v4 ,r4;
	v4[0] = v.x;v4[1] = v.y;v4[2] = v.z;v4[3] = 1;
	for(int i = 0;i < 4;i ++){
		r4[i] = 0;
		for(int j = 0;j < 4;j ++){
			r4[i] += v4[j] * t.m[j][i];
		}
	}
	result = new Vector3(r4[0] ,r4[1] ,r4[2]);
	return result;
}