package speiger.src.coreengine.math.vector.matrix;

import java.nio.ByteBuffer;
import java.nio.FloatBuffer;
import java.util.Arrays;

import speiger.src.coreengine.math.MathUtils;
import speiger.src.coreengine.math.vector.VectorUtil;
import speiger.src.coreengine.math.vector.floats.Vec2f;
import speiger.src.coreengine.math.vector.floats.Vec3f;
import speiger.src.coreengine.math.vector.floats.Vec4f;
import speiger.src.coreengine.math.vector.quaternion.Quaternion;

public class Matrix4f
{
	static final Vec3f X_ROTATION = Vec3f.newVec(1F, 0F, 0F);
	static final Vec3f Y_ROTATION = Vec3f.newVec(0F, 1F, 0F);
	static final Vec3f Z_ROTATION = Vec3f.newVec(0F, 0F, 1F);
	
	float[] data = new float[16];
	
	public Matrix4f()
	{
		setIdentity();
	}
	
	public Matrix4f(float[] data)
	{
		System.arraycopy(data, 0, this.data, 0, 16);
	}
	
	public Matrix4f(Matrix4f other)
	{
		this(other.data);
	}
	
	public Matrix4f store(FloatBuffer buffer)
	{
		buffer.put(data);
		return this;
	}
	
	public Matrix4f store(ByteBuffer buffer)
	{
		for(int i = 0;i < 16;i++)
		{
			buffer.putFloat(data[i]);
		}
		return this;
	}
	
	public Matrix4f store(Matrix4f other)
	{
		System.arraycopy(data, 0, other.data, 0, 16);
		return this;
	}
	
	public Matrix4f load(FloatBuffer buffer)
	{
		buffer.get(data);
		return this;
	}
	
	public Matrix4f load(ByteBuffer buffer)
	{
		for(int i = 0;i < 16;i++)
		{
			data[i] = buffer.getFloat();
		}
		return this;
	}
	
	public Matrix4f load(Matrix4f other)
	{
		System.arraycopy(other.data, 0, data, 0, 16);
		return this;
	}
	
	@Override
	public String toString()
	{
		StringBuilder buf = new StringBuilder().append("\n");
		buf.append("x0=").append(data[0]).append(' ').append("y0=").append(data[4]).append(' ').append("z0=").append(data[8]).append(' ').append("w0=").append(data[12]).append('\n');
		buf.append("x1=").append(data[1]).append(' ').append("y1=").append(data[5]).append(' ').append("z1=").append(data[9]).append(' ').append("w1=").append(data[13]).append('\n');
		buf.append("x2=").append(data[2]).append(' ').append("y2=").append(data[6]).append(' ').append("z2=").append(data[10]).append(' ').append("w2=").append(data[14]).append('\n');
		buf.append("x3=").append(data[3]).append(' ').append("y3=").append(data[7]).append(' ').append("z3=").append(data[11]).append(' ').append("w3=").append(data[15]).append('\n');
		return buf.toString();
	}
	
	@Override
	public boolean equals(Object obj)
	{
		if(obj instanceof Matrix4f)
		{
			return Arrays.equals(data, ((Matrix4f)obj).data);
		}
		return false;
	}
	
	public float[] getData()
	{
		return data;
	}
	
	public float get(int index)
	{
		return data[index];
	}
	
	public float get(int column, int row)
	{
		return data[(column * 4) + row];
	}
	
	public Matrix4f set(int column, int row, float value)
	{
		data[(column * 4) + row] = value;
		return this;
	}
	
	public Matrix4f set(int index, float value)
	{
		data[index] = value;
		return this;
	}
	
	public Matrix4f setIdentity()
	{
		Arrays.fill(data, 0F);
		data[0] = 1F;
		data[5] = 1F;
		data[10] = 1F;
		data[15] = 1F;
		return this;
	}
	
	public Matrix4f invert()
	{
		float determinant = determinant();
		if(determinant != 0F)
		{
			float determinant_inv = 1F / determinant;
			float t00 = determinant3x3(data[5], data[6], data[7], data[9], data[10], data[11], data[13], data[14], data[15]);
			float t01 = -determinant3x3(data[4], data[6], data[7], data[8], data[10], data[11], data[12], data[14], data[15]);
			float t02 = determinant3x3(data[4], data[5], data[7], data[8], data[9], data[11], data[12], data[13], data[15]);
			float t03 = -determinant3x3(data[4], data[5], data[6], data[8], data[9], data[10], data[12], data[13], data[14]);
			float t10 = -determinant3x3(data[1], data[2], data[3], data[9], data[10], data[11], data[13], data[14], data[15]);
			float t11 = determinant3x3(data[0], data[2], data[3], data[8], data[10], data[11], data[12], data[14], data[15]);
			float t12 = -determinant3x3(data[0], data[1], data[3], data[8], data[9], data[11], data[12], data[13], data[15]);
			float t13 = determinant3x3(data[0], data[1], data[2], data[8], data[9], data[10], data[12], data[13], data[14]);
			float t20 = determinant3x3(data[1], data[2], data[3], data[5], data[6], data[7], data[13], data[14], data[15]);
			float t21 = -determinant3x3(data[0], data[2], data[3], data[4], data[6], data[7], data[12], data[14], data[15]);
			float t22 = determinant3x3(data[0], data[1], data[3], data[4], data[5], data[7], data[12], data[13], data[15]);
			float t23 = -determinant3x3(data[0], data[1], data[2], data[4], data[5], data[6], data[12], data[13], data[14]);
			float t30 = -determinant3x3(data[1], data[2], data[3], data[5], data[6], data[7], data[9], data[10], data[11]);
			float t31 = determinant3x3(data[0], data[2], data[3], data[4], data[6], data[7], data[8], data[10], data[11]);
			float t32 = -determinant3x3(data[0], data[1], data[3], data[4], data[5], data[7], data[8], data[9], data[11]);
			float t33 = determinant3x3(data[0], data[1], data[2], data[4], data[5], data[6], data[8], data[9], data[10]);
			
			data[0] = t00 * determinant_inv;
			data[5] = t11 * determinant_inv;
			data[10] = t22 * determinant_inv;
			data[15] = t33 * determinant_inv;
			data[1] = t10 * determinant_inv;
			data[4] = t01 * determinant_inv;
			data[8] = t02 * determinant_inv;
			data[2] = t20 * determinant_inv;
			data[6] = t21 * determinant_inv;
			data[9] = t12 * determinant_inv;
			data[3] = t30 * determinant_inv;
			data[12] = t03 * determinant_inv;
			data[7] = t31 * determinant_inv;
			data[13] = t13 * determinant_inv;
			data[14] = t23 * determinant_inv;
			data[11] = t32 * determinant_inv;
		}
		return this;
	}
	
	public Matrix4f flip()
	{
		data[0] = -data[0];
		data[1] = -data[1];
		data[2] = -data[2];
		data[3] = -data[3];
		data[4] = -data[4];
		data[5] = -data[5];
		data[6] = -data[6];
		data[7] = -data[7];
		data[8] = -data[8];
		data[9] = -data[9];
		data[10] = -data[10];
		data[11] = -data[11];
		data[12] = -data[12];
		data[13] = -data[13];
		data[14] = -data[14];
		data[15] = -data[15];
		return this;
	}
	
	public Matrix4f negate()
	{
		Arrays.fill(data, 0F);
		return this;
	}
	
	public Matrix4f add(Matrix4f other)
	{
		data[0] += other.data[0];
		data[1] += other.data[1];
		data[2] += other.data[2];
		data[3] += other.data[3];
		data[4] += other.data[4];
		data[5] += other.data[5];
		data[6] += other.data[6];
		data[7] += other.data[7];
		data[8] += other.data[8];
		data[9] += other.data[9];
		data[10] += other.data[10];
		data[11] += other.data[11];
		data[12] += other.data[12];
		data[13] += other.data[13];
		data[14] += other.data[14];
		data[15] += other.data[15];
		return this;
	}
	
	public Matrix4f sub(Matrix4f other)
	{
		data[0] -= other.data[0];
		data[1] -= other.data[1];
		data[2] -= other.data[2];
		data[3] -= other.data[3];
		data[4] -= other.data[4];
		data[5] -= other.data[5];
		data[6] -= other.data[6];
		data[7] -= other.data[7];
		data[8] -= other.data[8];
		data[9] -= other.data[9];
		data[10] -= other.data[10];
		data[11] -= other.data[11];
		data[12] -= other.data[12];
		data[13] -= other.data[13];
		data[14] -= other.data[14];
		data[15] -= other.data[15];
		return this;
	}
	
	public Matrix4f multiply(Matrix4f other)
	{
		float data0 = data[0] * other.data[0] + data[4] * other.data[1] + data[8] * other.data[2] + data[12] * other.data[3];
		float data1 = data[1] * other.data[0] + data[5] * other.data[1] + data[9] * other.data[2] + data[13] * other.data[3];
		float data2 = data[2] * other.data[0] + data[6] * other.data[1] + data[10] * other.data[2] + data[14] * other.data[3];
		float data3 = data[3] * other.data[0] + data[7] * other.data[1] + data[11] * other.data[2] + data[15] * other.data[3];
		float data4 = data[0] * other.data[4] + data[4] * other.data[5] + data[8] * other.data[6] + data[12] * other.data[7];
		float data5 = data[1] * other.data[4] + data[5] * other.data[5] + data[9] * other.data[6] + data[13] * other.data[7];
		float data6 = data[2] * other.data[4] + data[6] * other.data[5] + data[10] * other.data[6] + data[14] * other.data[7];
		float data7 = data[3] * other.data[4] + data[7] * other.data[5] + data[11] * other.data[6] + data[15] * other.data[7];
		float data8 = data[0] * other.data[8] + data[4] * other.data[9] + data[8] * other.data[10] + data[12] * other.data[11];
		float data9 = data[1] * other.data[8] + data[5] * other.data[9] + data[9] * other.data[10] + data[13] * other.data[11];
		float data10 = data[2] * other.data[8] + data[6] * other.data[9] + data[10] * other.data[10] + data[14] * other.data[11];
		float data11 = data[3] * other.data[8] + data[7] * other.data[9] + data[11] * other.data[10] + data[15] * other.data[11];
		float data12 = data[0] * other.data[12] + data[4] * other.data[13] + data[8] * other.data[14] + data[12] * other.data[15];
		float data13 = data[1] * other.data[12] + data[5] * other.data[13] + data[9] * other.data[14] + data[13] * other.data[15];
		float data14 = data[2] * other.data[12] + data[6] * other.data[13] + data[10] * other.data[14] + data[14] * other.data[15];
		float data15 = data[3] * other.data[12] + data[7] * other.data[13] + data[11] * other.data[14] + data[15] * other.data[15];
		data[0] = data0;
		data[1] = data1;
		data[2] = data2;
		data[3] = data3;
		data[4] = data4;
		data[5] = data5;
		data[6] = data6;
		data[7] = data7;
		data[8] = data8;
		data[9] = data9;
		data[10] = data10;
		data[11] = data11;
		data[12] = data12;
		data[13] = data13;
		data[14] = data14;
		data[15] = data15;
		return this;
	}
	
	public Matrix4f transpose()
	{
		float data0 = data[0];
		float data1 = data[4];
		float data2 = data[8];
		float data3 = data[12];
		float data4 = data[1];
		float data5 = data[5];
		float data6 = data[9];
		float data7 = data[13];
		float data8 = data[2];
		float data9 = data[6];
		float data10 = data[10];
		float data11 = data[14];
		float data12 = data[3];
		float data13 = data[7];
		float data14 = data[11];
		float data15 = data[15];
		
		data[0] = data0;
		data[1] = data1;
		data[2] = data2;
		data[3] = data3;
		data[4] = data4;
		data[5] = data5;
		data[6] = data6;
		data[7] = data7;
		data[8] = data8;
		data[9] = data9;
		data[10] = data10;
		data[11] = data11;
		data[12] = data12;
		data[13] = data13;
		data[14] = data14;
		data[15] = data15;
		return this;
	}
	
	public Matrix4f transpose3x3()
	{
		float nm00 = data[0];
		float nm01 = data[4];
		float nm02 = data[8];
		float nm10 = data[1];
		float nm11 = data[5];
		float nm12 = data[9];
		float nm20 = data[2];
		float nm21 = data[6];
		float nm22 = data[10];
		data[0] = nm00;
		data[1] = nm01;
		data[2] = nm02;
		data[4] = nm10;
		data[5] = nm11;
		data[6] = nm12;
		data[8] = nm20;
		data[9] = nm21;
		data[10] = nm22;
		return this;
	}
	
	public Matrix4f decompose(Vec3f position, Quaternion rotation, Vec3f scale)
	{
		position.set(get(3, 0), get(3, 1), get(3, 2));
		rotation.set(this);
		scale.set(get(0, 0), get(1, 1), get(2, 2));
		return this;
	}
	
	public Matrix4f setTranslation(Vec3f vec)
	{
		data[12] = vec.getX();
		data[13] = vec.getY();
		data[14] = vec.getZ();
		return this;
	}
	
	public Matrix4f translate(Vec2f vec)
	{
		return translate(vec.getX(), vec.getY());
	}
	
	public Matrix4f translate(float x, float y)
	{
		data[12] += data[0] * x + data[4] * y;
		data[13] += data[1] * x + data[5] * y;
		data[14] += data[2] * x + data[6] * y;
		data[15] += data[3] * x + data[7] * y;
		return this;
	}
	
	public Matrix4f translate(Vec3f vec)
	{
		data[12] += data[0] * vec.getX() + data[4] * vec.getY() + data[8] * vec.getZ();
		data[13] += data[1] * vec.getX() + data[5] * vec.getY() + data[9] * vec.getZ();
		data[14] += data[2] * vec.getX() + data[6] * vec.getY() + data[10] * vec.getZ();
		data[15] += data[3] * vec.getX() + data[7] * vec.getY() + data[11] * vec.getZ();
		return this;
	}
	
	public Matrix4f translate(float x, float y, float z)
	{
		data[12] += data[0] * x + data[4] * y + data[8] * z;
		data[13] += data[1] * x + data[5] * y + data[9] * z;
		data[14] += data[2] * x + data[6] * y + data[10] * z;
		data[15] += data[3] * x + data[7] * y + data[11] * z;
		return this;
	}
	
	public Matrix4f rotateX(float angle)
	{
		return rotate((float)Math.toRadians(angle), X_ROTATION);
	}
	
	public Matrix4f rotateY(float angle)
	{
		return rotate((float)Math.toRadians(angle), Y_ROTATION);
	}
	
	public Matrix4f rotateZ(float angle)
	{
		return rotate((float)Math.toRadians(angle), Z_ROTATION);
	}
	
	public Matrix4f rotateRadX(float angle)
	{
		return rotate(angle, X_ROTATION);
	}
	
	public Matrix4f rotateRadY(float angle)
	{
		return rotate(angle, Y_ROTATION);
	}
	
	public Matrix4f rotateRadZ(float angle)
	{
		return rotate(angle, Z_ROTATION);
	}
	
	public Matrix4f rotate(float angle, Vec3f axis)
	{
		float c = MathUtils.cos(angle);
		float s = MathUtils.sin(angle);
		float oneminusc = 1.0f - c;
		float xy = axis.getX() * axis.getY();
		float yz = axis.getY() * axis.getZ();
		float xz = axis.getX() * axis.getZ();
		float xs = axis.getX() * s;
		float ys = axis.getY() * s;
		float zs = axis.getZ() * s;
		
		float f00 = axis.getX() * axis.getX() * oneminusc + c;
		float f01 = xy * oneminusc + zs;
		float f02 = xz * oneminusc - ys;
		
		float f10 = xy * oneminusc - zs;
		float f11 = axis.getY() * axis.getY() * oneminusc + c;
		float f12 = yz * oneminusc + xs;
		
		float f20 = xz * oneminusc + ys;
		float f21 = yz * oneminusc - xs;
		float f22 = axis.getZ() * axis.getZ() * oneminusc + c;
		
		float t00 = data[0] * f00 + data[4] * f01 + data[8] * f02;
		float t01 = data[1] * f00 + data[5] * f01 + data[9] * f02;
		float t02 = data[2] * f00 + data[6] * f01 + data[10] * f02;
		float t03 = data[3] * f00 + data[7] * f01 + data[11] * f02;
		float t10 = data[0] * f10 + data[4] * f11 + data[8] * f12;
		float t11 = data[1] * f10 + data[5] * f11 + data[9] * f12;
		float t12 = data[2] * f10 + data[6] * f11 + data[10] * f12;
		float t13 = data[3] * f10 + data[7] * f11 + data[11] * f12;
		data[8] = data[0] * f20 + data[4] * f21 + data[8] * f22;
		data[9] = data[1] * f20 + data[5] * f21 + data[9] * f22;
		data[10] = data[2] * f20 + data[6] * f21 + data[10] * f22;
		data[11] = data[3] * f20 + data[7] * f21 + data[11] * f22;
		data[0] = t00;
		data[1] = t01;
		data[2] = t02;
		data[3] = t03;
		data[4] = t10;
		data[5] = t11;
		data[6] = t12;
		data[7] = t13;
		return this;
	}
	
	public Matrix4f rotate(Quaternion rotation)
	{
		float w2 = rotation.getW() * rotation.getW();
		float x2 = rotation.getX() * rotation.getX();
		float y2 = rotation.getY() * rotation.getY();
		float z2 = rotation.getZ() * rotation.getZ();
		float zw = rotation.getZ() * rotation.getW();
		float dzw = zw + zw;
		float xy = rotation.getX() * rotation.getY();
		float dxy = xy + xy;
		float xz = rotation.getX() * rotation.getZ();
		float dxz = xz + xz;
		float yw = rotation.getY() * rotation.getW();
		float dyw = yw + yw;
		float yz = rotation.getY() * rotation.getZ();
		float dyz = yz + yz;
		float xw = rotation.getX() * rotation.getW();
		float dxw = xw + xw;
		float rm00 = w2 + x2 - z2 - y2;
		float rm01 = dxy + dzw;
		float rm02 = dxz - dyw;
		float rm10 = -dzw + dxy;
		float rm11 = y2 - z2 + w2 - x2;
		float rm12 = dyz + dxw;
		float rm20 = dyw + dxz;
		float rm21 = dyz - dxw;
		float rm22 = z2 - y2 - x2 + w2;
		float nm00 = data[0] * rm00 + data[4] * rm01 + data[8] * rm02;
		float nm01 = data[1] * rm00 + data[5] * rm01 + data[9] * rm02;
		float nm02 = data[2] * rm00 + data[6] * rm01 + data[10] * rm02;
		float nm03 = data[3] * rm00 + data[7] * rm01 + data[11] * rm02;
		float nm10 = data[0] * rm10 + data[4] * rm11 + data[8] * rm12;
		float nm11 = data[1] * rm10 + data[5] * rm11 + data[9] * rm12;
		float nm12 = data[2] * rm10 + data[6] * rm11 + data[10] * rm12;
		float nm13 = data[3] * rm10 + data[7] * rm11 + data[11] * rm12;
		data[8] = data[0] * rm20 + data[4] * rm21 + data[8] * rm22;
		data[9] = data[1] * rm20 + data[5] * rm21 + data[9] * rm22;
		data[10] = data[2] * rm20 + data[6] * rm21 + data[10] * rm22;
		data[11] = data[3] * rm20 + data[7] * rm21 + data[11] * rm22;
		data[0] = nm00;
		data[1] = nm01;
		data[2] = nm02;
		data[3] = nm03;
		data[4] = nm10;
		data[5] = nm11;
		data[6] = nm12;
		data[7] = nm13;
		return this;
	}
	
	public Matrix4f setRotation(Matrix4f source)
	{
		data[0] = source.data[0];
		data[1] = source.data[1];
		data[2] = source.data[2];
		data[4] = source.data[4];
		data[5] = source.data[5];
		data[6] = source.data[6];
		data[8] = source.data[8];
		data[9] = source.data[9];
		data[10] = source.data[10];
		return this;
	}
	
	public Matrix4f setBillboard(Matrix4f source)
	{
		data[0] = source.data[0];
		data[1] = source.data[4];
		data[2] = source.data[8];
		data[4] = source.data[1];
		data[5] = source.data[5];
		data[6] = source.data[9];
		data[8] = source.data[2];
		data[9] = source.data[6];
		data[10] = source.data[10];
		return this;
	}
	
	public Matrix4f scale(Vec3f vec)
	{
		return scale(vec.getX(), vec.getY(), vec.getZ());
	}
	
	public Matrix4f scale(float value)
	{
		return scale(value, value, value);
	}
	
	public Matrix4f scale(float x, float y, float z)
	{
		data[0] *= x;
		data[1] *= x;
		data[2] *= x;
		data[3] *= x;
		data[4] *= y;
		data[5] *= y;
		data[6] *= y;
		data[7] *= y;
		data[8] *= z;
		data[9] *= z;
		data[10] *= z;
		data[11] *= z;
		return this;
	}
	
	public Matrix4f unscale(Vec3f vec)
	{
		return unscale(vec.getX(), vec.getY(), vec.getZ());
	}
	
	public Matrix4f unscale(float scale)
	{
		return unscale(scale, scale, scale);
	}
	
	public Matrix4f unscale(float x, float y, float z)
	{
		data[0] /= x;
		data[1] /= x;
		data[2] /= x;
		data[3] /= x;
		data[4] /= y;
		data[5] /= y;
		data[6] /= y;
		data[7] /= y;
		data[8] /= z;
		data[9] /= z;
		data[10] /= z;
		data[11] /= z;
		return this;
	}
	
	public Matrix4f setPerspective(float fovy, float aspect, float zNear, float zFar)
	{
		float h = (float)Math.tan(fovy * 0.5f);
		set(0, 0, 1F / (h * aspect)).set(1, 1, 1F / h);
		boolean farInf = zFar > 0 && Float.isInfinite(zFar);
		boolean nearInf = zNear > 0 && Float.isInfinite(zNear);
		if(farInf)
		{
			float e = 1E-6f;
			set(2, 2, e - 1F).set(3, 2, (e - 2F) * zNear);
		}
		else if(nearInf)
		{
			float e = 1E-6f;
			set(2, 2, 1F - e).set(3, 2, (2F - e) * zFar);
		}
		else
		{
			set(2, 2, (zFar + zNear) / (zNear - zFar)).set(3, 2, (zFar + zFar) * zNear / (zNear - zFar));
		}
		return set(2, 3, -1.0f);
	}
	
	public Matrix4f getTranslation(Vec3f vec)
	{
		vec.set(data[12], data[13], data[14]);
		return this;
	}
	
	public Matrix4f getScale(Vec3f vec)
	{
		vec.setX((float)Math.sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]));
		vec.setY((float)Math.sqrt(data[4] * data[4] + data[5] * data[5] + data[6] * data[6]));
		vec.setZ((float)Math.sqrt(data[8] * data[8] + data[9] * data[9] + data[10] * data[10]));
		return this;
	}
	
	public Vec4f transform(Vec4f input)
	{
		return transform(input, input);
	}
	
	public Vec4f transform(Vec4f input, Vec4f output)
	{
		float x = data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * input.getW();
		float y = data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * input.getW();
		float z = data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * input.getW();
		float w = data[3] * input.getX() + data[7] * input.getY() + data[11] * input.getZ() + data[15] * input.getW();
		return output.set(x, y, z, w);
	}
	
	public void transform(Vec4f input, FloatBuffer buffer)
	{
		buffer.put(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * input.getW());
		buffer.put(data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * input.getW());
		buffer.put(data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * input.getW());
		buffer.put(data[3] * input.getX() + data[7] * input.getY() + data[11] * input.getZ() + data[15] * input.getW());
	}
	
	public Vec3f transform(Vec3f input, boolean position)
	{
		return transform(input, input, position);
	}
	
	public Vec3f transform(Vec3f input, Vec3f output, boolean position)
	{
		float pos = position ? 1F : 0F;
		return output.set(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * pos, data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * pos, data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * pos);
	}
	
	public void transform(Vec3f input, FloatBuffer buffer, boolean position)
	{
		float pos = position ? 1F : 0F;
		buffer.put(data[0] * input.getX() + data[4] * input.getY() + data[8] * input.getZ() + data[12] * pos);
		buffer.put(data[1] * input.getX() + data[5] * input.getY() + data[9] * input.getZ() + data[13] * pos);
		buffer.put(data[2] * input.getX() + data[6] * input.getY() + data[10] * input.getZ() + data[14] * pos);
	}
	
	public float determinant()
	{
		float f = data[0] * ((data[5] * data[10] * data[15] + data[6] * data[11] * data[13] + data[7] * data[9] * data[14]) - data[7] * data[10] * data[13] - data[5] * data[11] * data[14] - data[6] * data[9] * data[15]);
		f -= data[1] * ((data[4] * data[10] * data[15] + data[6] * data[11] * data[12] + data[7] * data[8] * data[14]) - data[7] * data[10] * data[12] - data[4] * data[11] * data[14] - data[6] * data[8] * data[15]);
		f += data[2] * ((data[4] * data[9] * data[15] + data[5] * data[11] * data[12] + data[7] * data[8] * data[13]) - data[7] * data[9] * data[12] - data[4] * data[11] * data[13] - data[5] * data[8] * data[15]);
		f -= data[3] * ((data[4] * data[9] * data[14] + data[5] * data[10] * data[12] + data[6] * data[8] * data[13]) - data[6] * data[9] * data[12] - data[4] * data[10] * data[13] - data[5] * data[8] * data[14]);
		return f;
	}
	
	public float determinant3x3(float t00, float t01, float t02, float t10, float t11, float t12, float t20, float t21, float t22)
	{
		return t00 * (t11 * t22 - t12 * t21) + t01 * (t12 * t20 - t10 * t22) + t02 * (t10 * t21 - t11 * t20);
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f rotation, float scale)
	{
		setIdentity();
		translate(position);
		rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
		rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
		rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
		scale(scale, scale, scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f rotation, Vec3f scale)
	{
		setIdentity();
		translate(position);
		rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
		rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
		rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
		scale(scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f offset, Vec3f rotation, float scale)
	{
		setIdentity();
		translate(position);
		translate(offset);
		rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
		rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
		rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
		scale(scale, scale, scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f offset, Vec3f rotation, Vec3f scale)
	{
		setIdentity();
		translate(position);
		translate(offset);
		rotate((float)Math.toRadians(rotation.getX()), X_ROTATION);
		rotate((float)Math.toRadians(rotation.getY()), Y_ROTATION);
		rotate((float)Math.toRadians(rotation.getZ()), Z_ROTATION);
		scale(scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Quaternion rotation, float scale)
	{
		setIdentity();
		translate(position);
		rotate(rotation);
		scale(scale, scale, scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Quaternion rotation, Vec3f scale)
	{
		setIdentity();
		translate(position);
		rotate(rotation);
		scale(scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f offset, Quaternion rotation, float scale)
	{
		setIdentity();
		translate(position);
		translate(offset);
		rotate(rotation);
		scale(scale, scale, scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Vec3f offset, Quaternion rotation, Vec3f scale)
	{
		setIdentity();
		translate(position);
		translate(offset);
		rotate(rotation);
		scale(scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Matrix4f billRotation, float scale)
	{
		setIdentity();
		translate(position);
		setBillboard(billRotation);
		scale(scale, scale, scale);
		return this;
	}
	
	public Matrix4f setTransform(Vec3f position, Matrix4f billRotation, Vec3f scale)
	{
		setIdentity();
		translate(position);
		setBillboard(billRotation);
		transpose3x3();
		scale(scale);
		return this;
	}
	
	public Vec3f project(float x, float y, float z, int[] viewport, Vec3f winCoordsDest)
	{
		float invW = 1F / VectorUtil.fma(data[3], x, VectorUtil.fma(data[7], y, VectorUtil.fma(data[11], z, data[15])));
		float nx = VectorUtil.fma(data[0], x, VectorUtil.fma(data[4], y, VectorUtil.fma(data[8], z, data[12]))) * invW;
		float ny = VectorUtil.fma(data[1], x, VectorUtil.fma(data[5], y, VectorUtil.fma(data[9], z, data[13]))) * invW;
		float nz = VectorUtil.fma(data[2], x, VectorUtil.fma(data[6], y, VectorUtil.fma(data[10], z, data[14]))) * invW;
		return winCoordsDest.set(VectorUtil.fma(VectorUtil.fma(nx, 0.5F, 0.5F), viewport[2], viewport[0]), VectorUtil.fma(VectorUtil.fma(ny, 0.5F, 0.5F), viewport[3], viewport[1]), VectorUtil.fma(0.5F, nz, 0.5F));
	}
	
    public Vec3f unproject(float winX, float winY, float winZ, int[] viewport, Vec3f dest) {
        float a = data[0] * data[5] - data[1] * data[4];
        float b = data[0] * data[6] - data[2] * data[4];
        float c = data[0] * data[7] - data[3] * data[4];
        float d = data[1] * data[6] - data[2] * data[5];
        float e = data[1] * data[7] - data[3] * data[5];
        float f = data[2] * data[7] - data[3] * data[6];
        float g = data[8] * data[13] - data[9] * data[12];
        float h = data[8] * data[14] - data[10] * data[12];
        float i = data[8] * data[15] - data[11] * data[12];
        float j = data[9] * data[14] - data[10] * data[13];
        float k = data[9] * data[15] - data[11] * data[13];
        float l = data[10] * data[15] - data[11] * data[14];
        float det = a * l - b * k + c * j + d * i - e * h + f * g;
        det = 1.0f / det;
        float im00 = ( data[5] * l - data[6] * k + data[7] * j) * det;
        float im01 = (-data[1] * l + data[2] * k - data[3] * j) * det;
        float im02 = ( data[13] * f - data[14] * e + data[15] * d) * det;
        float im03 = (-data[9] * f + data[10] * e - data[11] * d) * det;
        float im10 = (-data[4] * l + data[6] * i - data[7] * h) * det;
        float im11 = ( data[0] * l - data[2] * i + data[3] * h) * det;
        float im12 = (-data[12] * f + data[14] * c - data[15] * b) * det;
        float im13 = ( data[8] * f - data[10] * c + data[11] * b) * det;
        float im20 = ( data[4] * k - data[5] * i + data[7] * g) * det;
        float im21 = (-data[0] * k + data[1] * i - data[3] * g) * det;
        float im22 = ( data[12] * e - data[13] * c + data[15] * a) * det;
        float im23 = (-data[8] * e + data[9] * c - data[11] * a) * det;
        float im30 = (-data[4] * j + data[5] * h - data[6] * g) * det;
        float im31 = ( data[0] * j - data[1] * h + data[2] * g) * det;
        float im32 = (-data[12] * d + data[13] * b - data[14] * a) * det;
        float im33 = ( data[8] * d - data[9] * b + data[10] * a) * det;
        float ndcX = (winX-viewport[0])/viewport[2]*2.0f-1.0f;
        float ndcY = (winY-viewport[1])/viewport[3]*2.0f-1.0f;
        float ndcZ = winZ+winZ-1.0f;
        float invW = 1.0f / (im03 * ndcX + im13 * ndcY + im23 * ndcZ + im33);
        return dest.set((im00 * ndcX + im10 * ndcY + im20 * ndcZ + im30) * invW, (im01 * ndcX + im11 * ndcY + im21 * ndcZ + im31) * invW, (im02 * ndcX + im12 * ndcY + im22 * ndcZ + im32) * invW);
    }
    
    public Matrix4f ortho(float x, float y, float width, float height, float zNear, float zFar)
    {
    	return ortho(x, x+width, y+height, y, zNear, zFar, false);
    }
	
	public Matrix4f ortho(float left, float right, float bottom, float top, float zNear, float zFar, boolean zZeroToOne)
	{
		float rm00 = 2F / (right - left);
		float rm11 = 2F / (top - bottom);
		float rm22 = (zZeroToOne ? 1F : 2F) / (zFar - zNear);
		float rm30 = (left + right) / (left - right);
		float rm31 = (top + bottom) / (bottom - top);
		float rm32 = (zZeroToOne ? zNear : (zFar + zNear)) / (zNear - zFar);
		
		data[12] = data[0] * rm30 + data[4] * rm31 + data[8] * rm32 + data[12];
		data[13] = data[1] * rm30 + data[5] * rm31 + data[9] * rm32 + data[13];
		data[14] = data[2] * rm30 + data[6] * rm31 + data[10] * rm32 + data[14];
		data[15] = data[3] * rm30 + data[7] * rm31 + data[11] * rm32 + data[15];
		data[0] = data[0] * rm00;
		data[1] = data[1] * rm00;
		data[2] = data[2] * rm00;
		data[3] = data[3] * rm00;
		data[4] = data[4] * rm11;
		data[5] = data[5] * rm11;
		data[6] = data[6] * rm11;
		data[7] = data[7] * rm11;
		data[8] = data[8] * rm22;
		data[9] = data[9] * rm22;
		data[10] = data[10] * rm22;
		data[11] = data[11] * rm22;
		return this;
	}
	
	public Vec4f storeFrustrumPlane(int plane, Vec4f toStore)
	{
		switch(plane)
		{
			case 0:
				return toStore.set(data[3] + data[0], data[7] + data[4], data[11] + data[8], data[15] + data[12]).normalize3D();
			case 1:
				return toStore.set(data[3] - data[0], data[7] - data[4], data[11] - data[8], data[15] - data[12]).normalize3D();
			case 2:
				return toStore.set(data[3] + data[1], data[7] + data[5], data[11] + data[9], data[15] + data[13]).normalize3D();
			case 3:
				return toStore.set(data[3] - data[1], data[7] - data[5], data[11] - data[9], data[15] - data[13]).normalize3D();
			case 4:
				return toStore.set(data[3] + data[2], data[7] + data[6], data[11] + data[10], data[15] + data[14]).normalize3D();
			case 5:
				return toStore.set(data[3] - data[2], data[7] - data[6], data[11] - data[10], data[15] - data[14]).normalize3D();
		}
		return toStore;
	}
}