public class Matrix3D {
	private double data[][] = new double[4][4];

	public Matrix3D() {
		identityMatrix();
	}

	public void set(int i, int j, double value) {
		data[i][j] = value;
	}

	public double get(int i, int j) {
		return data[i][j];
	}

	public void copy(Matrix3D M) {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				data[i][j] = M.get(i, j);
			}
		}
	}

	public void identityMatrix() {
		data[0][0] =  1; data[0][1] =  0; data[0][2] =  0; data[0][3] =  0;
		data[1][0] =  0; data[1][1] =  1; data[1][2] =  0; data[1][3] =  0;
		data[2][0] =  0; data[2][1] =  0; data[2][2] =  1; data[2][3] =  0;
		data[3][0] =  0; data[3][1] =  0; data[3][2] =  0; data[3][3] =  1;
	}

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

	public void xRotationMatrix(double theta) {
		double c = Math.cos(theta),
		       s = Math.sin(theta);

		data[0][0] =  1; data[0][1] =  0; data[0][2] =  0; data[0][3] =  0;
		data[1][0] =  0; data[1][1] =  c; data[1][2] = -s; data[1][3] =  0;
		data[2][0] =  0; data[2][1] =  s; data[2][2] =  c; data[2][3] =  0;
		data[3][0] =  0; data[3][1] =  0; data[3][2] =  0; data[3][3] =  1;
	}

	public void yRotationMatrix(double theta) {
		double c = Math.cos(theta),
		       s = Math.sin(theta);

		data[0][0] =  c; data[0][1] =  0; data[0][2] =  s; data[0][3] =  0;
		data[1][0] =  0; data[1][1] =  1; data[1][2] =  0; data[1][3] =  0;
		data[2][0] = -s; data[2][1] =  0; data[2][2] =  c; data[2][3] =  0;
		data[3][0] =  0; data[3][1] =  0; data[3][2] =  0; data[3][3] =  1;
	}

	public void zRotationMatrix(double theta) {
		double c = Math.cos(theta),
		       s = Math.sin(theta);

		data[0][0] =  c; data[0][1] = -s; data[0][2] =  0; data[0][3] =  0;
		data[1][0] =  s; data[1][1] =  c; data[1][2] =  0; data[1][3] =  0;
		data[2][0] =  0; data[2][1] =  0; data[2][2] =  1; data[2][3] =  0;
		data[3][0] =  0; data[3][1] =  0; data[3][2] =  0; data[3][3] =  1;
	}

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

	public void translate(double a, double b, double c) {
		Matrix3D temp = new Matrix3D();

		temp.translationMatrix(a, b, c);

		multiply(temp);
	}

	public void rotateX(double theta) {
		Matrix3D temp = new Matrix3D();

		temp.xRotationMatrix(theta);

		multiply(temp);
	}

	public void rotateY(double theta) {
		Matrix3D temp = new Matrix3D();

		temp.yRotationMatrix(theta);

		multiply(temp);
	}

	public void rotateZ(double theta) {
		Matrix3D temp = new Matrix3D();

		temp.zRotationMatrix(theta);

		multiply(temp);
	}

	public void scale(double a, double b, double c) {
		Matrix3D temp = new Matrix3D();

		temp.scaleMatrix(a, b, c);

		multiply(temp);
	}

	void multiply(Matrix3D M) {
		Matrix3D temp = new Matrix3D();

		temp.copy(this);

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				data[i][j] = M.get(i, 0)*temp.get(0, j)
					   + M.get(i, 1)*temp.get(1, j)
					   + M.get(i, 2)*temp.get(2, j)
					   + M.get(i, 3)*temp.get(3, j);
			}
		}
	}

	public double[] transform(double u[]) {
		double v[] = new double[3];

		v[0] = data[0][0]*u[0] + data[0][1]*u[1] + data[0][2]*u[2] + data[0][3];
		v[1] = data[1][0]*u[0] + data[1][1]*u[1] + data[1][2]*u[2] + data[1][3];
		v[2] = data[2][0]*u[0] + data[2][1]*u[1] + data[2][2]*u[2] + data[2][3];

		return v;
	}

	public void print() {
		for (int i = 0; i < 4; i++) {
			System.err.println(data[i][0] + " " + data[i][1] + " " + data[i][2] + " " + data[i][3]);
		}
	}

	public void invert() {
		Matrix3D temp = new Matrix3D();
		temp.copy(this);

		// COMPUTE ADJOINT COFACTOR MATRIX FOR THE ROTATION+SCALE 3x3

		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				int i0 = (i + 1)%3, i1 = (i + 2)%3,
				    j0 = (j + 1)%3, j1 = (j + 2)%3;
				data[j][i] = temp.get(i0, j0) * temp.get(i1, j1) - temp.get(i0, j1) * temp.get(i1, j0);
			}
		}

		// RENORMALIZE BY DETERMINANT TO GET ROTATION+SCALE 3x3 INVERSE

		double determinant = temp.get(0, 0) * data[0][0] + temp.get(1, 0) * data[0][1] + temp.get(2, 0) * data[0][2];

		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				data[i][j] /= determinant;
			}
		}

		// COMPUTE INVERSE TRANSLATION

		for (int i = 0; i < 3; i++) {
			data[i][3] = -data[i][0] * temp.get(0, 3) - data[i][1] * temp.get(1, 3) - data[i][2] * temp.get(2, 3);
		}

		data[3][3] = 1;
	}
}