public class Matrix3D {

	double data[][] = new double[4][4];

	public void set(int i, int j, double value) { data[i][j] = value; } // SET ONE VALUE
	public double get(int i, int j) { return data[i][j]; }              // GET ONE VALUE

	public Matrix3D(){
	
	double data[][] = new double[4][4];
	
	}
	public void identity() {
		for (int i=0 ; i < 4 ; i++)
			for (int j = 0 ; j < 4 ; j++)
				if (i == j) set(i,j,1);
				else set(i,j,0);
	}

	public void invert() {
		Matrix3D tmpInv = new Matrix3D();	
		tmpInv.identity();
		MatrixInverter.invert(data,tmpInv.data);
		copy(tmpInv);

	}

	public void transpose() {
	double tmp[][]=new double[4][4];
	for (int i=0;i<4;i++)
		for (int j=0;j<4;j++)
			tmp[i][j]=get(j,i);

	for (int i=0;i<4;i++)
		for (int j=0;j<4;j++)
			set(i,j,tmp[i][j]);
	}


	public void translationMatrix(double a, double b, double c) {
		identity();
		data[0][3] = a;
		data[1][3] = b;
		data[2][3] = c;
	}

	public void genRotationMatrix(double theta,int src, int dst){
		identity();
		set(src,src,Math.cos(theta));
		set(dst,dst,Math.cos(theta));
		set(src,dst,-Math.sin(theta));
		set(dst,src,Math.sin(theta));
	}

	public void xRotationMatrix(double theta){
		genRotationMatrix(theta,1,2);
	}

	public void yRotationMatrix(double theta){
		genRotationMatrix(theta,2,0);
	}

	public void zRotationMatrix(double theta){
		genRotationMatrix(theta,0,1);
	}

	public void scaleMatrix(double x, double y, double z){
		identity();
		set(0,0,x);
		set(1,1,y);
		set(2,2,z);
	}

	public void copy(Matrix3D src) {
		for (int i = 0 ; i < 4 ; i++)
			for (int j = 0 ; j < 4 ; j++)
				set(i,j, src.get(i,j));
	}


	void multiplyLeft(Matrix3D M) {
		//multiply on the left
		Matrix3D temp = new Matrix3D();
		temp.copy(this);      // FIRST COPY MY ORIGINAL DATA TO A TEMPORARY MATRIX
		for (int i = 0 ; i < 4 ; i++)
			for (int j = 0 ; j < 4 ; j++) {
				double sum = 0;
				for (int k = 0 ; k < 4 ; k++)
					sum += temp.get(k,j) * M.get(i,k);
				data[i][j] = sum;  // THEN COMPUTE NEW DATA VALUES FOR ME
			}
	}

	void multiplyRight(Matrix3D M) {
		Matrix3D temp = new Matrix3D();
		temp.copy(this);      // FIRST COPY MY ORIGINAL DATA TO A TEMPORARY MATRIX
		for (int i = 0 ; i < 4 ; i++)
			for (int j = 0 ; j < 4 ; j++) {
				double sum = 0;
				for (int k = 0 ; k < 4 ; k++)
					sum += temp.get(i,k) * M.get(k,j);
				data[i][j] = sum;  // THEN COMPUTE NEW DATA VALUES FOR ME
			}
	}
	public void translate(double a, double b, double c){
		Matrix3D temp = new Matrix3D();
		temp.translationMatrix(a,b,c);
		multiplyLeft(temp);
	}
	public void rotateX(double theta){
		Matrix3D temp = new Matrix3D();
		temp.xRotationMatrix(theta);
		multiplyLeft(temp);
	}
	public void rotateY(double theta){
		Matrix3D temp = new Matrix3D();
		temp.yRotationMatrix(theta);
		multiplyLeft(temp);
	}
	public void rotateZ(double theta){
		Matrix3D temp = new Matrix3D();
		temp.zRotationMatrix(theta);
		multiplyLeft(temp);
	}
	public void scale(double a, double b, double c){
		Matrix3D temp = new Matrix3D();
		temp.scaleMatrix(a,b,c);
		multiplyLeft(temp);
	}

	public void translateRight(double a, double b, double c){
		Matrix3D temp = new Matrix3D();
		temp.translationMatrix(a,b,c);
		multiplyRight(temp);
	}
	public void rotateXRight(double theta){
		Matrix3D temp = new Matrix3D();
		temp.xRotationMatrix(theta);
		multiplyRight(temp);
	}
	public void rotateYRight(double theta){
		Matrix3D temp = new Matrix3D();
		temp.yRotationMatrix(theta);
		multiplyRight(temp);
	}
	public void rotateZRight(double theta){
		Matrix3D temp = new Matrix3D();
		temp.zRotationMatrix(theta);
		multiplyRight(temp);
	}
	public void scaleRight(double a, double b, double c){
		Matrix3D temp = new Matrix3D();
		temp.scaleMatrix(a,b,c);
		multiplyRight(temp);
	}

	public void transform(double src[], double dst[]) {
		for (int i = 0 ; i < 4 ; i++)
			dst[i] = data[i][0] * src[0] + data[i][1] * src[1] + data[i][2] * src[2] + data[i][3]*src[3];
	}


	public void transformN(double src[], double dst[]) {
		Matrix3D transpInv=new Matrix3D();
		transpInv.copy(this);
		transpInv.invert();
		transpInv.transpose();
		for (int i = 0 ; i < 4 ; i++)
			dst[i] = transpInv.data[i][0] * src[0] + transpInv.data[i][1] * src[1] + transpInv.data[i][2] * src[2] + transpInv.data[i][3]*src[3];
	}

	public void transformBak(double src[], double dst[]) {
		for (int i = 0 ; i < 3 ; i++)
			dst[i] = data[i][0] * src[0] + data[i][1] * src[1] + data[i][2] * src[2] + data[i][3];
	}


}