Mat4.java

import org.lwjgl.BufferUtils;

import java.nio.FloatBuffer;
import static org.lwjgl.opengl.GL11.*;

public class Mat4 {

	public static final Mat4 IDENTITY = new Mat4().identity();

	private float[][] m;

	public Mat4() {
		m = new float[4][4];
	}

	public Mat4 gl(int matrix) {
		float[] params = new float[16];
		glGetFloatv(matrix, params);
		
		setM(params);
		return this;
	}
	
	public Mat4 identity() {
		m[0][0] = 1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = 1;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		return this;
	}

	public Mat4 translation(float x, float y, float z) {
		m[0][0] = 1;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = x;
		m[1][0] = 0;
		m[1][1] = 1;
		m[1][2] = 0;
		m[1][3] = y;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = 1;
		m[2][3] = z;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		return this;
	}

	public Mat4 translation(Vec3 t) {
		return translation(t.x, t.y, t.z);
	}

	public Mat4 rotation(float x, float y, float z) {
		Mat4 rx = new Mat4();
		Mat4 ry = new Mat4();
		Mat4 rz = new Mat4();

		rz.m[0][0] = (float) Math.cos(z);
		rz.m[0][1] = -(float) Math.sin(z);
		rz.m[0][2] = 0;
		rz.m[0][3] = 0;
		rz.m[1][0] = (float) Math.sin(z);
		rz.m[1][1] = (float) Math.cos(z);
		rz.m[1][2] = 0;
		rz.m[1][3] = 0;
		rz.m[2][0] = 0;
		rz.m[2][1] = 0;
		rz.m[2][2] = 1;
		rz.m[2][3] = 0;
		rz.m[3][0] = 0;
		rz.m[3][1] = 0;
		rz.m[3][2] = 0;
		rz.m[3][3] = 1;

		rx.m[0][0] = 1;
		rx.m[0][1] = 0;
		rx.m[0][2] = 0;
		rx.m[0][3] = 0;
		rx.m[1][0] = 0;
		rx.m[1][1] = (float) Math.cos(x);
		rx.m[1][2] = -(float) Math.sin(x);
		rx.m[1][3] = 0;
		rx.m[2][0] = 0;
		rx.m[2][1] = (float) Math.sin(x);
		rx.m[2][2] = (float) Math.cos(x);
		rx.m[2][3] = 0;
		rx.m[3][0] = 0;
		rx.m[3][1] = 0;
		rx.m[3][2] = 0;
		rx.m[3][3] = 1;

		ry.m[0][0] = (float) Math.cos(y);
		ry.m[0][1] = 0;
		ry.m[0][2] = -(float) Math.sin(y);
		ry.m[0][3] = 0;
		ry.m[1][0] = 0;
		ry.m[1][1] = 1;
		ry.m[1][2] = 0;
		ry.m[1][3] = 0;
		ry.m[2][0] = (float) Math.sin(y);
		ry.m[2][1] = 0;
		ry.m[2][2] = (float) Math.cos(y);
		ry.m[2][3] = 0;
		ry.m[3][0] = 0;
		ry.m[3][1] = 0;
		ry.m[3][2] = 0;
		ry.m[3][3] = 1;

		m = rz.mul(ry.mul(rx)).getM();

		return this;
	}

	public Mat4 rotation(Vec3 axis, float a) {
		return rotation(axis.x * a, axis.y * a, axis.z * a);
	}

	public Mat4 scale(float x, float y, float z) {
		m[0][0] = x;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = y;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = z;
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		return this;
	}

	public Mat4 scale(Vec3 s) {
		return scale(s.x, s.y, s.z);
	}

	public Mat4 perspective(float fov, float aspectRatio, float zNear, float zFar) {
		float tanHalfFOV = (float) Math.tan(fov / 2);
		float zRange = zNear - zFar;

		m[0][0] = 1.0f / (tanHalfFOV * aspectRatio);
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = 0;
		m[1][0] = 0;
		m[1][1] = 1.0f / tanHalfFOV;
		m[1][2] = 0;
		m[1][3] = 0;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = (-zNear - zFar) / zRange;
		m[2][3] = 2 * zFar * zNear / zRange;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 1;
		m[3][3] = 0;

		return this;
	}

	public Mat4 ortho(float left, float right, float bottom, float top, float near, float far) {
		float width = right - left;
		float height = top - bottom;
		float depth = far - near;

		m[0][0] = 2 / width;
		m[0][1] = 0;
		m[0][2] = 0;
		m[0][3] = -(right + left) / width;
		m[1][0] = 0;
		m[1][1] = 2 / height;
		m[1][2] = 0;
		m[1][3] = -(top + bottom) / height;
		m[2][0] = 0;
		m[2][1] = 0;
		m[2][2] = -2 / depth;
		m[2][3] = -(far + near) / depth;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		return this;
	}

	public Mat4 rotation(Vec3 forward, Vec3 up) {
		Vec3 f = forward.normalized();
		Vec3 r = up.normalized();
		r = r.cross(f);

		Vec3 u = f.cross(r);
		return rotation(f, u, r);
	}

	public Mat4 rotation(Vec3 forward, Vec3 up, Vec3 right) {
		Vec3 f = forward;
		Vec3 r = right;
		Vec3 u = up;

		m[0][0] = r.x;
		m[0][1] = r.y;
		m[0][2] = r.z;
		m[0][3] = 0;
		m[1][0] = u.x;
		m[1][1] = u.y;
		m[1][2] = u.z;
		m[1][3] = 0;
		m[2][0] = f.x;
		m[2][1] = f.y;
		m[2][2] = f.z;
		m[2][3] = 0;
		m[3][0] = 0;
		m[3][1] = 0;
		m[3][2] = 0;
		m[3][3] = 1;

		return this;
	}

	public Mat4 invert() {
		float[] tmp = new float[12];
		float[] src = new float[16];
		float[] dst = new float[16];

		// Transpose matrix
		for (int i = 0; i < 4; i++) {
			src[i + 0] = get(i * 4 + 0);
			src[i + 4] = get(i * 4 + 1);
			src[i + 8] = get(i * 4 + 2);
			src[i + 12] = get(i * 4 + 3);
		}

		// Calculate pairs for first 8 elements (cofactors) 
		tmp[0] = src[10] * src[15];
		tmp[1] = src[11] * src[14];
		tmp[2] = src[9] * src[15];
		tmp[3] = src[11] * src[13];
		tmp[4] = src[9] * src[14];
		tmp[5] = src[10] * src[13];
		tmp[6] = src[8] * src[15];
		tmp[7] = src[11] * src[12];
		tmp[8] = src[8] * src[14];
		tmp[9] = src[10] * src[12];
		tmp[10] = src[8] * src[13];
		tmp[11] = src[9] * src[12];

		// Calculate first 8 elements (cofactors)
		dst[0] = tmp[0] * src[5] + tmp[3] * src[6] + tmp[4] * src[7];
		dst[0] -= tmp[1] * src[5] + tmp[2] * src[6] + tmp[5] * src[7];
		dst[1] = tmp[1] * src[4] + tmp[6] * src[6] + tmp[9] * src[7];
		dst[1] -= tmp[0] * src[4] + tmp[7] * src[6] + tmp[8] * src[7];
		dst[2] = tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7];
		dst[2] -= tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7];
		dst[3] = tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6];
		dst[3] -= tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6];
		dst[4] = tmp[1] * src[1] + tmp[2] * src[2] + tmp[5] * src[3];
		dst[4] -= tmp[0] * src[1] + tmp[3] * src[2] + tmp[4] * src[3];
		dst[5] = tmp[0] * src[0] + tmp[7] * src[2] + tmp[8] * src[3];
		dst[5] -= tmp[1] * src[0] + tmp[6] * src[2] + tmp[9] * src[3];
		dst[6] = tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3];
		dst[6] -= tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3];
		dst[7] = tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2];
		dst[7] -= tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2];

		// Calculate pairs for second 8 elements (cofactors)
		tmp[0] = src[2] * src[7];
		tmp[1] = src[3] * src[6];
		tmp[2] = src[1] * src[7];
		tmp[3] = src[3] * src[5];
		tmp[4] = src[1] * src[6];
		tmp[5] = src[2] * src[5];
		tmp[6] = src[0] * src[7];
		tmp[7] = src[3] * src[4];
		tmp[8] = src[0] * src[6];
		tmp[9] = src[2] * src[4];
		tmp[10] = src[0] * src[5];
		tmp[11] = src[1] * src[4];

		// Calculate second 8 elements (cofactors)
		dst[8] = tmp[0] * src[13] + tmp[3] * src[14] + tmp[4] * src[15];
		dst[8] -= tmp[1] * src[13] + tmp[2] * src[14] + tmp[5] * src[15];
		dst[9] = tmp[1] * src[12] + tmp[6] * src[14] + tmp[9] * src[15];
		dst[9] -= tmp[0] * src[12] + tmp[7] * src[14] + tmp[8] * src[15];
		dst[10] = tmp[2] * src[12] + tmp[7] * src[13] + tmp[10] * src[15];
		dst[10] -= tmp[3] * src[12] + tmp[6] * src[13] + tmp[11] * src[15];
		dst[11] = tmp[5] * src[12] + tmp[8] * src[13] + tmp[11] * src[14];
		dst[11] -= tmp[4] * src[12] + tmp[9] * src[13] + tmp[10] * src[14];
		dst[12] = tmp[2] * src[10] + tmp[5] * src[11] + tmp[1] * src[9];
		dst[12] -= tmp[4] * src[11] + tmp[0] * src[9] + tmp[3] * src[10];
		dst[13] = tmp[8] * src[11] + tmp[0] * src[8] + tmp[7] * src[10];
		dst[13] -= tmp[6] * src[10] + tmp[9] * src[11] + tmp[1] * src[8];
		dst[14] = tmp[6] * src[9] + tmp[11] * src[11] + tmp[3] * src[8];
		dst[14] -= tmp[10] * src[11] + tmp[2] * src[8] + tmp[7] * src[9];
		dst[15] = tmp[10] * src[10] + tmp[4] * src[8] + tmp[9] * src[9];
		dst[15] -= tmp[8] * src[9] + tmp[11] * src[10] + tmp[5] * src[8];

		// Calculate determinant
		float det = src[0] * dst[0] + src[1] * dst[1] + src[2] * dst[2] + src[3] * dst[3];
		det = 1.0f / det;
		for (int i = 0; i < 16; i++) {
			set(i, dst[i] * det);
		}
		
		return this;
	}

	public Vec3 transform(Vec3 r) {
		return new Vec3(m[0][0] * r.x + m[0][1] * r.y + m[0][2] * r.z + m[0][3],
						   m[1][0] * r.x + m[1][1] * r.y + m[1][2] * r.z + m[1][3],
						   m[2][0] * r.x + m[2][1] * r.y + m[2][2] * r.z + m[2][3]);
	}

	public Mat4 mul(Mat4 r) {
		Mat4 res = new Mat4();
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				res.set(i, j, m[i][0] * r.get(0, j)
							  + m[i][1] * r.get(1, j)
							  + m[i][2] * r.get(2, j)
							  + m[i][3] * r.get(3, j));
			}
		}
		return res;
	}
	
	public Vec4 mul(Vec4 lhs) {
		return new Vec4(
			(lhs.x * m[0][0]) + (lhs.y * m[1][0]) + (lhs.z * m[2][0]) + (lhs.w * m[3][0]),
			(lhs.x * m[0][1]) + (lhs.y * m[1][1]) + (lhs.z * m[2][1]) + (lhs.w * m[3][1]),
			(lhs.x * m[0][2]) + (lhs.y * m[1][2]) + (lhs.z * m[2][2]) + (lhs.w * m[3][2]),
			(lhs.x * m[0][3]) + (lhs.y * m[1][3]) + (lhs.z * m[2][3]) + (lhs.w * m[3][3])
		);
	}

	public Mat4 clone() {
		float[][] _m = new float[4][4];
		for (int i = 0; i < 4; i++) {
			_m[i] = m[i].clone();
		}
		return new Mat4().setM(_m);
	}

	public float[][] getM() {
		float[][] res = new float[4][4];

		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				res[i][j] = m[i][j];
			}
		}

		return res;
	}

	public float get(int x, int y) {
		return m[x][y];
	}

	public Mat4 setM(float[][] m) {
		this.m = m;
		return this;
	}
	
	public Mat4 setM(float[] m) {
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				this.m[i][j] = m[j * 4 + i];
			}
		}
		return this;
	}

	public void set(int x, int y, float value) {
		m[x][y] = value;
	}
	
	public void set(int i, float value) {
		m[i % 4][(int) (i / 4)] = value;
	}

	public float get(int i) {
		return m[i % 4][(int) (i / 4)];
	}

	private final static FloatBuffer direct = BufferUtils.createFloatBuffer(16);

	public FloatBuffer get() {
		direct.clear();
		for (int i = 0; i < 16; i++) {
			direct.put(get(i));
		}
		direct.flip();
		return direct;
	}

	@Override
	public String toString() {
		return "[ " + get(0) + ", " + get(1) + ", " + get(2) + ", " + get(3) + " ]\n"
			   + "[ " + get(4) + ", " + get(5) + ", " + get(6) + ", " + get(7) + " ]\n"
			   + "[ " + get(8) + ", " + get(9) + ", " + get(10) + ", " + get(11) + " ]\n"
			   + "[ " + get(12) + ", " + get(13) + ", " + get(14) + ", " + get(15) + " ]";
	}
}