math.hpp

#ifndef MATH_HPP
#define MATH_HPP

#include <math.h>

#ifdef ASSERT
	#define MATH_ASSERT(...) ASSERT(__VA_ARGS__)
#else
	#define MATH_ASSERT(...)
#endif

#ifdef ARRAY_COUNT
	#define MATH_ARRAY_COUNT(...) ARRAY_COUNT(__VA_ARGS__)
#else
	#define MATH_ARRAY_COUNT(X) (sizeof((X)) / sizeof((X)[0]))
#endif

#define TAU 6.28318530717958647692f
#define PI 3.14159265358979323846f
#define TO_RAD (TAU / 360.0f)
#define TO_DEG (360.0f / TAU)

inline float clamp01(float x) {
	return x < 0.0f ? 0.0f : (x > 1.0f ? 1.0f : x);
}

inline float clamp(float x, float u, float v) {
	return x < u ? u : (x > v ? v : x);
}

inline float lerp(float x, float y, float t) {
	return x * (1.0f - t) + y * t;
}

inline float smooth_step(float x, float y, float t) {
	t = t * t * (3.0f - 2.0f * t);
	return x * (1.0f - t) + y * t;
}

inline float frac(float x) {
	return x - (int)x;
}

inline float sqr(float x) {
	return x * x;
}

union Vec2 {
	struct { float x; float y; };
	struct { float v[2]; };
};

inline Vec2 vec2(float x, float y) {
	Vec2 v;
	v.x = x;
	v.y = y;
	return v;
}

inline Vec2 vec2(float x) {
	return vec2(x, x);
}

inline Vec2 operator+(Vec2 v, float x) {
	v.x += x;
	v.y += x;
	return v;
}

inline Vec2 operator+(Vec2 x, Vec2 y) {
	x.x += y.x;
	x.y += y.y;
	return x;
}

inline Vec2 & operator+=(Vec2 & x, Vec2 y) {
	x.x += y.x;
	x.y += y.y;
	return x;
}

inline Vec2 operator-(Vec2 v, float x) {
	v.x -= x;
	v.y -= x;
	return v;
}

inline Vec2 operator-(Vec2 x, Vec2 y) {
	x.x -= y.x;
	x.y -= y.y;
	return x;
}

inline Vec2 & operator*=(Vec2 & v, float x) {
	v.x *= x;
	v.y *= x;
	return v;
}

inline Vec2 operator*(Vec2 v, float x) {
	v.x *= x;
	v.y *= x;
	return v;
}

inline Vec2 operator*(Vec2 x, Vec2 y) {
	x.x *= y.x;
	x.y *= y.y;
	return x;
}

inline float dot(Vec2 x, Vec2 y) {
	return x.x * y.x + x.y * y.y;
}

inline float length_sqr(Vec2 x) {
	return x.x * x.x + x.y * x.y;
}

inline float length(Vec2 x) {
	return sqrt(x.x * x.x + x.y * x.y);
}

inline Vec2 normalize(Vec2 x) {
	float len = sqrt(x.x * x.x + x.y * x.y);
	return len > 0.0f ? x * (1.0f / len) : vec2(0.0f);
}

inline Vec2 lerp(Vec2 x, Vec2 y, float t) {
	return x * (1.0f - t) + y * t;
}

union Vec3 {
	struct { float x; float y; float z; };
	struct { Vec2 xy; float z_; };
	struct { float v[3]; };
};

inline Vec3 vec3(float x, float y, float z) {
	Vec3 v;
	v.x = x;
	v.y = y;
	v.z = z;
	return v;
}

inline Vec3 vec3(Vec2 xy, float z) {
	return vec3(xy.x, xy.y, z);
}

inline Vec3 vec3(float x) {
	return vec3(x, x, x);
}

inline Vec3 operator-(Vec3 x) {
	x.x = -x.x;
	x.y = -x.y;
	x.z = -x.z;
	return x;
}

inline Vec3 & operator+=(Vec3 & x, Vec3 y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	return x;
}

inline Vec3 operator+(Vec3 x, Vec3 y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	return x;
}

inline Vec3 operator-(Vec3 x, Vec3 y) {
	x.x -= y.x;
	x.y -= y.y;
	x.z -= y.z;
	return x;
}

inline Vec3 & operator-=(Vec3 & x, Vec3 y) {
	x.x -= y.x;
	x.y -= y.y;
	x.z -= y.z;
	return x;
}

inline Vec3 & operator*=(Vec3 & v, float x) {
	v.x *= x;
	v.y *= x;
	v.z *= x;
	return v;
}

inline Vec3 operator*(float x, Vec3 v) {
	v.x *= x;
	v.y *= x;
	v.z *= x;
	return v;
}

inline Vec3 operator*(Vec3 v, float x) {
	v.x *= x;
	v.y *= x;
	v.z *= x;
	return v;
}

inline Vec3 & operator/=(Vec3 & v, float x) {
	v.x /= x;
	v.y /= x;
	v.z /= x;
	return v;
}

inline float dot(Vec3 x, Vec3 y) {
	return x.x * y.x + x.y * y.y + x.z * y.z;
}

inline Vec3 cross(Vec3 x, Vec3 y) {
	return { x.y * y.z - x.z * y.y, x.z * y.x - x.x * y.z, x.x * y.y - x.y * y.x };
}

inline float length_sqr(Vec3 x) {
	return x.x * x.x + x.y * x.y + x.z * x.z;
}

inline float length(Vec3 x) {
	return sqrt(x.x * x.x + x.y * x.y + x.z * x.z);
}

inline Vec3 normalize(Vec3 x) {
	float len = sqrt(x.x * x.x + x.y * x.y + x.z * x.z);
	return len > 0.0f ? x * (1.0f / len) : vec3(0.0f);
}

inline Vec3 lerp(Vec3 x, Vec3 y, float t) {
	return x * (1.0f - t) + y * t;
}

union Vec4 {
	struct { float x; float y; float z; float w; };
	struct { Vec3 xyz; float w_; };
	struct { float v[4]; };
};

inline Vec4 vec4(float x, float y, float z, float w) {
	Vec4 v;
	v.x = x;
	v.y = y;
	v.z = z;
	v.w = w;
	return v;
}

inline Vec4 vec4(Vec3 xyz, float w) {
	return vec4(xyz.x, xyz.y, xyz.z, w);
}

inline Vec4 vec4(float x) {
	return vec4(x, x, x, x);
}

inline Vec4 operator*(float x, Vec4 v) {
	v.x *= x;
	v.y *= x;
	v.z *= x;
	v.w *= x;
	return v;
}

inline Vec4 operator*(Vec4 v, float x) {
	v.x *= x;
	v.y *= x;
	v.z *= x;
	v.w *= x;
	return v;
}

inline Vec4 operator/(Vec4 v, float x) {
	v.x /= x;
	v.y /= x;
	v.z /= x;
	v.w /= x;
	return v;
}

inline Vec4 & operator+=(Vec4 & x, Vec4 y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	x.w += y.w;
	return x;
}

inline Vec4 operator+(Vec4 v, float x) {
	v.x += x;
	v.y += x;
	v.z += x;
	v.w += x;
	return v;
}

inline Vec4 operator+(Vec4 x, Vec4 y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	x.w += y.w;
	return x;
}

inline Vec4 operator-(Vec4 v, float x) {
	v.x -= x;
	v.y -= x;
	v.z -= x;
	v.w -= x;
	return v;
}

inline Vec4 operator-(Vec4 x, Vec4 y) {
	x.x -= y.x;
	x.y -= y.y;
	x.z -= y.z;
	x.w -= y.w;
	return x;
}

inline float dot(Vec4 x, Vec4 y) {
	return x.x * y.x + x.y * y.y + x.z * y.z + x.w * y.w;
}

inline float length_sqr(Vec4 x) {
	return x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w;
}

inline float length(Vec4 x) {
	return sqrt(x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w);
}

inline Vec4 normalize(Vec4 x) {
	float len = sqrt(x.x * x.x + x.y * x.y + x.z * x.z + x.w * x.w);
	return len > 0.0f ? x * (1.0f / len) : vec4(0.0f);
}

union Quat {
	struct { float x; float y; float z; float w; };
	struct { Vec3 xyz; float w_; };
	struct { float v[4]; };
};

inline Quat quat(float w) {
	return { 0.0f, 0.0f, 0.0f, w };
}

inline Quat quat(float x, float y, float z, float w) {
	return { x, y, z, w };
}

inline Quat quat(Vec3 xyz, float w) {
	Quat q;
	q.xyz = xyz;
	q.w = w;
	return q;
}

inline Quat operator-(Quat q) {
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
	q.w = -q.w;
	return q;
}

inline Quat operator+(Quat x, Quat y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	x.w += y.w;
	return x;
}

inline Quat & operator+=(Quat & x, Quat y) {
	x.x += y.x;
	x.y += y.y;
	x.z += y.z;
	x.w += y.w;
	return x;
}

inline Quat operator*(Quat x, Quat y) {
	Quat q;
	q.xyz = x.w * y.xyz + y.w * x.xyz + cross(x.xyz, y.xyz);
	q.w = x.w * y.w - dot(x.xyz, y.xyz);
	return q;
}

inline Quat operator*(float x, Quat q) {
	q.x *= x;
	q.y *= x;
	q.z *= x;
	q.w *= x;
	return q;
}

inline Quat operator*(Quat q, float x) {
	q.x *= x;
	q.y *= x;
	q.z *= x;
	q.w *= x;
	return q;
}

inline Quat & operator*=(Quat & q, float x) {
	q.x *= x;
	q.y *= x;
	q.z *= x;
	q.w *= x;
	return q;
}

inline Quat angle_axis(Vec3 v, float t) {
	Quat q;
	float a = t * 0.5f;
	q.xyz = sin(a) * v;
	q.w = cos(a);
	return q;
}

inline float length(Quat q) {
	return sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
}

inline Quat normalize(Quat q) {
	float len = sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
	return q * (1.0f / len);
}

inline Quat inverse(Quat q) {
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
	return q;
}

inline Quat lerp(Quat x, Quat y, float t) {
	float cosa = x.w * y.w + dot(x.xyz, y.xyz);
	if(cosa < 0.0f) {
		cosa = -cosa;
		y.xyz = -y.xyz;
		y.w = -y.w;
	}

#if 0
	float k0, k1;

	float eps = 0.0001f;
	if(cosa < (1.0f - eps)) {
		float sina = sqrt(1.0f - cosa * cosa);
		float r_sina = 1.0f / sina;
		float a = atan2(sina, cosa);

		k0 = sin((1.0f - t) * a) * r_sina;
		k1 = sin(t * a) * r_sina;
	}
	else {
		k0 = (1.0f - t);
		k1 = t;
	}

	return k0 * x + k1 * y;
#else
	//TODO: Remap t so lerp moves at a constant speed over the sphere??
	return normalize((1.0f - t) * x + t * y);
#endif
}

inline float angle_from_to(Quat x, Quat y) {
	float w = x.w * y.w + dot(x.xyz, y.xyz);
	return acos(w) * 2.0f;
}

inline Quat rotate_from_to(Quat x, Quat y) {
	return y * inverse(x);
}

inline Quat rotate_from_to(Vec3 x, Vec3 y) {
	float u = sqrt(2.0f * dot(x, y) + 2.0f);

	Quat q;
	q.xyz = (1.0f / u) * cross(x, y);
	q.w = u * 0.5f;
	return q;
}

inline Vec3 rotate_vec(Vec3 v, Quat q) {
	//TODO: Simplify this!!
	return (q * quat(v, 0.0f) * inverse(q)).xyz;
}

struct Mat3 {
	float v[9];
};

inline Mat3 mat3(float x) {
	return {
		   x, 0.0f, 0.0f,
		0.0f,    x, 0.0f,
		0.0f, 0.0f,    x,
	};
}

inline Mat3 operator*(float x, Mat3 a) {
	for(int i = 0; i < MATH_ARRAY_COUNT(a.v); i++) {
		a.v[i] *= x;
	}

	return a;
}

inline Mat3 operator*(Mat3 a, float x) {
	for(int i = 0; i < MATH_ARRAY_COUNT(a.v); i++) {
		a.v[i] *= x;
	}

	return a;
}

inline Mat3 operator*(Mat3 a, Mat3 b) {
	return {
		a.v[0] * b.v[0] + a.v[1] * b.v[3] + a.v[2] * b.v[6],
		a.v[0] * b.v[1] + a.v[1] * b.v[4] + a.v[2] * b.v[7],
		a.v[0] * b.v[2] + a.v[1] * b.v[5] + a.v[2] * b.v[8],

		a.v[3] * b.v[0] + a.v[4] * b.v[3] + a.v[5] * b.v[6],
		a.v[3] * b.v[1] + a.v[4] * b.v[4] + a.v[5] * b.v[7],
		a.v[3] * b.v[2] + a.v[4] * b.v[5] + a.v[5] * b.v[8],

		a.v[6] * b.v[0] + a.v[7] * b.v[3] + a.v[8] * b.v[6],
		a.v[6] * b.v[1] + a.v[7] * b.v[4] + a.v[8] * b.v[7],
		a.v[6] * b.v[2] + a.v[7] * b.v[5] + a.v[8] * b.v[8],
	};
}

inline Vec3 operator*(Mat3 a, Vec3 x) {
	return {
		a.v[0] * x.x + a.v[1] * x.y + a.v[2] * x.z,
		a.v[3] * x.x + a.v[4] * x.y + a.v[5] * x.z,
		a.v[6] * x.x + a.v[7] * x.y + a.v[8] * x.z,
	};
}

inline Mat3 transpose(Mat3 a) {
	return {
		a.v[0], a.v[3], a.v[6],
		a.v[1], a.v[4], a.v[7],
		a.v[2], a.v[5], a.v[8],
	};
}

inline float determinant(Mat3 a) {
	float c11 =  a.v[4] * a.v[8] - a.v[5] * a.v[7];
	float c12 = -a.v[3] * a.v[8] + a.v[5] * a.v[6];
	float c13 =  a.v[3] * a.v[7] - a.v[4] * a.v[6];

	return a.v[0] * c11 + a.v[1] * c12 + a.v[2] * c13;
}

inline Mat3 adjoint(Mat3 a) {
	return {
		 a.v[4] * a.v[8] - a.v[5] * a.v[7],
		-a.v[1] * a.v[8] + a.v[2] * a.v[7],
		 a.v[1] * a.v[5] - a.v[2] * a.v[4],

		-a.v[3] * a.v[8] + a.v[5] * a.v[6],
		 a.v[0] * a.v[8] - a.v[2] * a.v[6],
		-a.v[0] * a.v[5] + a.v[2] * a.v[3],

		 a.v[3] * a.v[7] - a.v[4] * a.v[6],
		-a.v[0] * a.v[7] + a.v[1] * a.v[6],
		 a.v[0] * a.v[4] - a.v[1] * a.v[3],
	};
}

inline Mat3 inverse(Mat3 a) {
	Mat3 ai;

	ai.v[0] =  a.v[4] * a.v[8] - a.v[5] * a.v[7];
	ai.v[3] = -a.v[3] * a.v[8] + a.v[5] * a.v[6];
	ai.v[6] =  a.v[3] * a.v[7] - a.v[4] * a.v[6];

	ai.v[1] = -a.v[1] * a.v[8] + a.v[2] * a.v[7];
	ai.v[4] =  a.v[0] * a.v[8] - a.v[2] * a.v[6];
	ai.v[7] = -a.v[0] * a.v[7] + a.v[1] * a.v[6];

	ai.v[2] =  a.v[1] * a.v[5] - a.v[2] * a.v[4];
	ai.v[5] = -a.v[0] * a.v[5] + a.v[2] * a.v[3];
	ai.v[8] =  a.v[0] * a.v[4] - a.v[1] * a.v[3];

	float det = a.v[0] * ai.v[0] + a.v[1] * ai.v[3] + a.v[2] * ai.v[6];
	MATH_ASSERT(det > 0.0f || det < 0.0f);

	float d = 1.0f / det;
	for(int i = 0; i < MATH_ARRAY_COUNT(ai.v); i++) {
		ai.v[i] *= d;
	}

	return ai;
}

struct Mat4 {
	float v[16];
};

inline Mat4 mat4(float x) {
	return {
		   x, 0.0f, 0.0f, 0.0f,
		0.0f,    x, 0.0f, 0.0f,
		0.0f, 0.0f,    x, 0.0f,
		0.0f, 0.0f, 0.0f,    x,
	};
}

inline Mat4 mat4(float x, float y, float z, float w) {
	return {
		   x, 0.0f, 0.0f, 0.0f,
		0.0f,    y, 0.0f, 0.0f,
		0.0f, 0.0f,    z, 0.0f,
		0.0f, 0.0f, 0.0f,    w,
	};
}

inline Mat4 operator*(float x, Mat4 a) {
	for(int i = 0; i < MATH_ARRAY_COUNT(a.v); i++) {
		a.v[i] *= x;
	}

	return a;
}

inline Mat4 operator*(Mat4 a, float x) {
	for(int i = 0; i < MATH_ARRAY_COUNT(a.v); i++) {
		a.v[i] *= x;
	}

	return a;
}

inline Mat4 operator*(Mat4 a, Mat4 b) {
	return {
		a.v[ 0] * b.v[ 0] + a.v[ 1] * b.v[ 4] + a.v[ 2] * b.v[ 8] + a.v[ 3] * b.v[12],
		a.v[ 0] * b.v[ 1] + a.v[ 1] * b.v[ 5] + a.v[ 2] * b.v[ 9] + a.v[ 3] * b.v[13],
		a.v[ 0] * b.v[ 2] + a.v[ 1] * b.v[ 6] + a.v[ 2] * b.v[10] + a.v[ 3] * b.v[14],
		a.v[ 0] * b.v[ 3] + a.v[ 1] * b.v[ 7] + a.v[ 2] * b.v[11] + a.v[ 3] * b.v[15],

		a.v[ 4] * b.v[ 0] + a.v[ 5] * b.v[ 4] + a.v[ 6] * b.v[ 8] + a.v[ 7] * b.v[12],
		a.v[ 4] * b.v[ 1] + a.v[ 5] * b.v[ 5] + a.v[ 6] * b.v[ 9] + a.v[ 7] * b.v[13],
		a.v[ 4] * b.v[ 2] + a.v[ 5] * b.v[ 6] + a.v[ 6] * b.v[10] + a.v[ 7] * b.v[14],
		a.v[ 4] * b.v[ 3] + a.v[ 5] * b.v[ 7] + a.v[ 6] * b.v[11] + a.v[ 7] * b.v[15],

		a.v[ 8] * b.v[ 0] + a.v[ 9] * b.v[ 4] + a.v[10] * b.v[ 8] + a.v[11] * b.v[12],
		a.v[ 8] * b.v[ 1] + a.v[ 9] * b.v[ 5] + a.v[10] * b.v[ 9] + a.v[11] * b.v[13],
		a.v[ 8] * b.v[ 2] + a.v[ 9] * b.v[ 6] + a.v[10] * b.v[10] + a.v[11] * b.v[14],
		a.v[ 8] * b.v[ 3] + a.v[ 9] * b.v[ 7] + a.v[10] * b.v[11] + a.v[11] * b.v[15],

		a.v[12] * b.v[ 0] + a.v[13] * b.v[ 4] + a.v[14] * b.v[ 8] + a.v[15] * b.v[12],
		a.v[12] * b.v[ 1] + a.v[13] * b.v[ 5] + a.v[14] * b.v[ 9] + a.v[15] * b.v[13],
		a.v[12] * b.v[ 2] + a.v[13] * b.v[ 6] + a.v[14] * b.v[10] + a.v[15] * b.v[14],
		a.v[12] * b.v[ 3] + a.v[13] * b.v[ 7] + a.v[14] * b.v[11] + a.v[15] * b.v[15],
	};
}

inline Vec4 operator*(Mat4 a, Vec4 x) {
	return {
		a.v[ 0] * x.x + a.v[ 1] * x.y + a.v[ 2] * x.z + a.v[ 3] * x.w,
		a.v[ 4] * x.x + a.v[ 5] * x.y + a.v[ 6] * x.z + a.v[ 7] * x.w,
		a.v[ 8] * x.x + a.v[ 9] * x.y + a.v[10] * x.z + a.v[11] * x.w,
		a.v[12] * x.x + a.v[13] * x.y + a.v[14] * x.z + a.v[15] * x.w,
	};
}

inline Mat3 submat3(Mat4 a) {
	return {
		a.v[ 0], a.v[ 1], a.v[ 2],
		a.v[ 4], a.v[ 5], a.v[ 6],
		a.v[ 8], a.v[ 9], a.v[10],
	};
}

inline Mat4 transpose(Mat4 a) {
	return {
		a.v[ 0], a.v[ 4], a.v[ 8], a.v[12],
		a.v[ 1], a.v[ 5], a.v[ 9], a.v[13],
		a.v[ 2], a.v[ 6], a.v[10], a.v[14],
		a.v[ 3], a.v[ 7], a.v[11], a.v[15],
	};
}

inline float determinant(Mat4 a) {
	float c11 =  a.v[ 5] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) - a.v[ 6] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) + a.v[ 7] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]);
	float c12 = -a.v[ 4] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) + a.v[ 6] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) - a.v[ 7] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]);
	float c13 =  a.v[ 4] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) - a.v[ 5] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) + a.v[ 7] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);
	float c14 = -a.v[ 4] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]) + a.v[ 5] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]) - a.v[ 6] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);

	return a.v[0] * c11 + a.v[1] * c12 + a.v[2] * c13 + a.v[3] * c14;
}

inline Mat4 adjoint(Mat4 a) {
	return {
		 a.v[ 5] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) - a.v[ 6] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) + a.v[ 7] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]),
		-a.v[ 1] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) + a.v[ 2] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) - a.v[ 3] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]),
		 a.v[ 1] * (a.v[ 6] * a.v[15] - a.v[ 7] * a.v[14]) - a.v[ 2] * (a.v[ 5] * a.v[15] - a.v[ 7] * a.v[13]) + a.v[ 3] * (a.v[ 5] * a.v[14] - a.v[ 6] * a.v[13]),
		-a.v[ 1] * (a.v[ 6] * a.v[11] - a.v[ 7] * a.v[10]) + a.v[ 2] * (a.v[ 5] * a.v[11] - a.v[ 7] * a.v[ 9]) - a.v[ 3] * (a.v[ 5] * a.v[10] - a.v[ 6] * a.v[ 9]),

		-a.v[ 4] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) + a.v[ 6] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) - a.v[ 7] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]),
		 a.v[ 0] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) - a.v[ 2] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) + a.v[ 3] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]),
		-a.v[ 0] * (a.v[ 6] * a.v[15] - a.v[ 7] * a.v[14]) + a.v[ 2] * (a.v[ 4] * a.v[15] - a.v[ 7] * a.v[12]) - a.v[ 3] * (a.v[ 4] * a.v[14] - a.v[ 6] * a.v[12]),
		 a.v[ 0] * (a.v[ 6] * a.v[11] - a.v[ 7] * a.v[10]) - a.v[ 2] * (a.v[ 4] * a.v[11] - a.v[ 7] * a.v[ 8]) + a.v[ 3] * (a.v[ 4] * a.v[10] - a.v[ 6] * a.v[ 8]),

		 a.v[ 4] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) - a.v[ 5] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) + a.v[ 7] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]),
		-a.v[ 0] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) + a.v[ 1] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) - a.v[ 3] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]),
		 a.v[ 0] * (a.v[ 5] * a.v[15] - a.v[ 7] * a.v[13]) - a.v[ 1] * (a.v[ 4] * a.v[15] - a.v[ 7] * a.v[12]) + a.v[ 3] * (a.v[ 4] * a.v[13] - a.v[ 5] * a.v[12]),
		-a.v[ 0] * (a.v[ 5] * a.v[11] - a.v[ 7] * a.v[ 9]) + a.v[ 1] * (a.v[ 4] * a.v[11] - a.v[ 7] * a.v[ 8]) - a.v[ 3] * (a.v[ 4] * a.v[ 9] - a.v[ 5] * a.v[ 8]),

		-a.v[ 4] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]) + a.v[ 5] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]) - a.v[ 6] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]),
		 a.v[ 0] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]) - a.v[ 1] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]) + a.v[ 2] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]),
		-a.v[ 0] * (a.v[ 5] * a.v[14] - a.v[ 6] * a.v[13]) + a.v[ 1] * (a.v[ 4] * a.v[14] - a.v[ 6] * a.v[12]) - a.v[ 2] * (a.v[ 4] * a.v[13] - a.v[ 5] * a.v[12]),
		 a.v[ 0] * (a.v[ 5] * a.v[10] - a.v[ 6] * a.v[ 9]) - a.v[ 1] * (a.v[ 4] * a.v[10] - a.v[ 6] * a.v[ 8]) + a.v[ 2] * (a.v[ 4] * a.v[ 9] - a.v[ 5] * a.v[ 8]),
	};
}

inline Mat4 inverse(Mat4 a) {
	Mat4 ai;

	ai.v[ 0] =  a.v[ 5] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) - a.v[ 6] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) + a.v[ 7] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]);
	ai.v[ 4] = -a.v[ 4] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) + a.v[ 6] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) - a.v[ 7] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]);
	ai.v[ 8] =  a.v[ 4] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) - a.v[ 5] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) + a.v[ 7] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);
	ai.v[12] = -a.v[ 4] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]) + a.v[ 5] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]) - a.v[ 6] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);

	ai.v[ 1] = -a.v[ 1] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) + a.v[ 2] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) - a.v[ 3] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]);
	ai.v[ 5] =  a.v[ 0] * (a.v[10] * a.v[15] - a.v[11] * a.v[14]) - a.v[ 2] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) + a.v[ 3] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]);
	ai.v[ 9] = -a.v[ 0] * (a.v[ 9] * a.v[15] - a.v[11] * a.v[13]) + a.v[ 1] * (a.v[ 8] * a.v[15] - a.v[11] * a.v[12]) - a.v[ 3] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);
	ai.v[13] =  a.v[ 0] * (a.v[ 9] * a.v[14] - a.v[10] * a.v[13]) - a.v[ 1] * (a.v[ 8] * a.v[14] - a.v[10] * a.v[12]) + a.v[ 2] * (a.v[ 8] * a.v[13] - a.v[ 9] * a.v[12]);

	ai.v[ 2] =  a.v[ 1] * (a.v[ 6] * a.v[15] - a.v[ 7] * a.v[14]) - a.v[ 2] * (a.v[ 5] * a.v[15] - a.v[ 7] * a.v[13]) + a.v[ 3] * (a.v[ 5] * a.v[14] - a.v[ 6] * a.v[13]);
	ai.v[ 6] = -a.v[ 0] * (a.v[ 6] * a.v[15] - a.v[ 7] * a.v[14]) + a.v[ 2] * (a.v[ 4] * a.v[15] - a.v[ 7] * a.v[12]) - a.v[ 3] * (a.v[ 4] * a.v[14] - a.v[ 6] * a.v[12]);
	ai.v[10] =  a.v[ 0] * (a.v[ 5] * a.v[15] - a.v[ 7] * a.v[13]) - a.v[ 1] * (a.v[ 4] * a.v[15] - a.v[ 7] * a.v[12]) + a.v[ 3] * (a.v[ 4] * a.v[13] - a.v[ 5] * a.v[12]);
	ai.v[14] = -a.v[ 0] * (a.v[ 5] * a.v[14] - a.v[ 6] * a.v[13]) + a.v[ 1] * (a.v[ 4] * a.v[14] - a.v[ 6] * a.v[12]) - a.v[ 2] * (a.v[ 4] * a.v[13] - a.v[ 5] * a.v[12]);

	ai.v[ 3] = -a.v[ 1] * (a.v[ 6] * a.v[11] - a.v[ 7] * a.v[10]) + a.v[ 2] * (a.v[ 5] * a.v[11] - a.v[ 7] * a.v[ 9]) - a.v[ 3] * (a.v[ 5] * a.v[10] - a.v[ 6] * a.v[ 9]);
	ai.v[ 7] =  a.v[ 0] * (a.v[ 6] * a.v[11] - a.v[ 7] * a.v[10]) - a.v[ 2] * (a.v[ 4] * a.v[11] - a.v[ 7] * a.v[ 8]) + a.v[ 3] * (a.v[ 4] * a.v[10] - a.v[ 6] * a.v[ 8]);
	ai.v[11] = -a.v[ 0] * (a.v[ 5] * a.v[11] - a.v[ 7] * a.v[ 9]) + a.v[ 1] * (a.v[ 4] * a.v[11] - a.v[ 7] * a.v[ 8]) - a.v[ 3] * (a.v[ 4] * a.v[ 9] - a.v[ 5] * a.v[ 8]);
	ai.v[15] =  a.v[ 0] * (a.v[ 5] * a.v[10] - a.v[ 6] * a.v[ 9]) - a.v[ 1] * (a.v[ 4] * a.v[10] - a.v[ 6] * a.v[ 8]) + a.v[ 2] * (a.v[ 4] * a.v[ 9] - a.v[ 5] * a.v[ 8]);

	float det = a.v[0] * ai.v[0] + a.v[1] * ai.v[4] + a.v[2] * ai.v[8] + a.v[3] * ai.v[12];
	MATH_ASSERT(det > 0.0f || det < 0.0f);

	float d = 1.0f / det;
	for(int i = 0; i < MATH_ARRAY_COUNT(ai.v); i++) {
		ai.v[i] *= d;
	}

	return ai;
}

inline Mat4 scalar(float x) {
	return {
		   x, 0.0f, 0.0f, 0.0f,
		0.0f,    x, 0.0f, 0.0f,
		0.0f, 0.0f,    x, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 scalar(float x, float y, float z) {
	return {
		   x, 0.0f, 0.0f, 0.0f,
		0.0f,    y, 0.0f, 0.0f,
		0.0f, 0.0f,    z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 scalar(Vec3 x) {
	return {
		 x.x, 0.0f, 0.0f, 0.0f,
		0.0f,  x.y, 0.0f, 0.0f,
		0.0f, 0.0f,  x.z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 translate(float x, float y, float z) {
	return {
		1.0f, 0.0f, 0.0f,    x,
		0.0f, 1.0f, 0.0f,    y,
		0.0f, 0.0f, 1.0f,    z,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 translate(Vec3 x) {
	return {
		1.0f, 0.0f, 0.0f,  x.x,
		0.0f, 1.0f, 0.0f,  x.y,
		0.0f, 0.0f, 1.0f,  x.z,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 rotate_x(float t) {
	float cost = cos(t);
	float sint = sin(t);

	return {
		1.0f, 0.0f, 0.0f, 0.0f,
		0.0f, cost,-sint, 0.0f,
		0.0f, sint, cost, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 rotate_y(float t) {
	float cost = cos(t);
	float sint = sin(t);

	return {
		 cost, 0.0f, sint, 0.0f,
		 0.0f, 1.0f, 0.0f, 0.0f,
		-sint, 0.0f, cost, 0.0f,
		 0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 rotate_z(float t) {
	float cost = cos(t);
	float sint = sin(t);

	return {
		cost,-sint, 0.0f, 0.0f,
		sint, cost, 0.0f, 0.0f,
		0.0f, 0.0f, 1.0f, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

inline Mat4 rotate_axis(Vec3 n, float t) {
	float u = cos(t);
	float v = 1.0f - u;

	Vec3 w = sin(t) * n;

	float x2 = n.x * n.x;
	float y2 = n.y * n.y;
	float z2 = n.z * n.z;

	float xy = n.x * n.y;
	float xz = n.x * n.z;
	float yz = n.y * n.z;

	return {
		v * x2 +   u, v * xy - w.z, v * xz + w.y, 0.0f,
		v * xy + w.z, v * y2 +   u, v * yz - w.x, 0.0f,
		v * xz - w.y, v * yz + w.x, v * z2 +   u, 0.0f,
		        0.0f,         0.0f,         0.0f, 1.0f,
	};
}

inline Mat4 rotate_quat(Quat q) {
	float x2 = 2.0f * q.x * q.x;
	float y2 = 2.0f * q.y * q.y;
	float z2 = 2.0f * q.z * q.z;
	float w2 = 2.0f * q.w * q.w;

	float xy = 2.0f * q.x * q.y;
	float xz = 2.0f * q.x * q.z;
	float yz = 2.0f * q.y * q.z;

	float xw = 2.0f * q.x * q.w;
	float yw = 2.0f * q.y * q.w;
	float zw = 2.0f * q.z * q.w;

	return {
		1.0f - y2 - z2,        xy - zw,        xz + yw, 0.0f,
		       xy + zw, 1.0f - x2 - z2,        yz - xw, 0.0f,
		       xz - yw,        yz + xw, 1.0f - x2 - y2, 0.0f,
		          0.0f,           0.0f,           0.0f, 1.0f,
	};
}

inline Mat4 perspective(float aspect, float fov, float near, float far) {
	float zoom_y = 1.0f / tan(fov * 0.5f);
	float zoom_x = zoom_y / aspect;

	return {
		zoom_x, 0.0f, 0.0f, 0.0f,
		0.0f, zoom_y, 0.0f, 0.0f,
		0.0f, 0.0f, (far + near) / (far - near), (-2.0f * near * far) / (far - near),
		0.0f, 0.0f, 1.0f, 0.0f,
	};
}

inline Mat4 orthographic(float aspect, float size, float near, float far) {
	float zoom_x = 2.0f / size;
	float zoom_y = aspect * zoom_x;

	return {
		zoom_x, 0.0f, 0.0f, 0.0f,
		0.0f, zoom_y, 0.0f, 0.0f,
		0.0f, 0.0f, 2.0f / (far - near), -(far + near) / (far - near),
		0.0f, 0.0f, 0.0f, 1.0f,
	};
}

#endif