gunslinger/include/math/gs_math.h

#ifndef __GS_MATH_H__
#define __GS_MATH_H__

#ifdef __cplusplus
extern "C" {
#endif

#include <math.h>
#include "common/gs_types.h"

// Defines
#define gs_pi 		3.1415926535897932
#define gs_tau		2.0 * gs_pi

// Useful Utility
#define gs_v2(...)	gs_vec2_ctor(__VA_ARGS__)
#define gs_v3(...)	gs_vec3_ctor(__VA_ARGS__)
#define gs_v4(...)	gs_vec4_ctor(__VA_ARGS__)

#define gs_v2_s(s)  gs_vec2_ctor((s), (s))
#define gs_v3_s(s)  gs_vec3_ctor((s), (s), (s))
#define gs_v4_s(s)  gs_vec4_ctor((s), (s), (s), (s))

#define gs_v4_xy_v(x, y, v) gs_vec4_ctor((x), (y), (v).x, (v).y)

#define gs_x_axis 	gs_v3(1.f, 0.f, 0.f)
#define gs_y_axis 	gs_v3(0.f, 1.f, 0.f)
#define gs_z_axis 	gs_v3(0.f, 0.f, 1.f)

/*================================================================================
// Useful Common Functions
================================================================================*/

#define gs_rad_to_deg(rad)\
	(f32)((rad * 180.0) / gs_pi) 

#define gs_deg_to_rad(deg)\
	(f32)((deg * gs_pi) / 180.0)

// Interpolation
// Source: https://codeplea.com/simple-interpolation

gs_inline f32
gs_interp_linear(f32 a, f32 b, f32 t)
{
	return (a + t * (b - a));
}

gs_inline f32
gs_interp_smooth_step(f32 a, f32 b, f32 t)
{
	return gs_interp_linear(a, b, t * t * (3.0 - 2.0 * t));
}

gs_inline f32 
gs_interp_cosine(f32 a, f32 b, f32 t)
{
	return gs_interp_linear(a, b, -cos(gs_pi * t) * 0.5 + 0.5);
}

gs_inline f32 
gs_interp_acceleration(f32 a, f32 b, f32 t) 
{
	return gs_interp_linear(a, b, t * t);
}

gs_inline f32 
gs_interp_deceleration(f32 a, f32 b, f32 t) 
{
	return gs_interp_linear(a, b, 1.0 - (1.0 - t) * (1.0 - t));
}

gs_inline f32 
gs_round(f32 val) 
{
	return floor(val + 0.5f);
}

gs_inline f32
gs_map_range(f32 input_start, f32 input_end, f32 output_start, f32 output_end, f32 val)
{
	f32 slope = (output_end - output_start) / (input_end - input_start);
	return (output_start + (slope * (val - input_start)));
}

/*================================================================================
// Vec2
================================================================================*/

typedef struct 
{
	union 
	{
		f32 xy[2];
		struct 
		{
			f32 x, y;
		};
	};
} gs_vec2;

gs_inline gs_vec2 
gs_vec2_ctor(f32 _x, f32 _y) 
{
	gs_vec2 v;
	v.x = _x;
	v.y = _y;
	return v;
}

gs_inline gs_vec2 
gs_vec2_add(gs_vec2 v0, gs_vec2 v1) 
{
	return gs_vec2_ctor(v0.x + v1.x, v0.y + v1.y);
}

gs_inline gs_vec2 
gs_vec2_sub(gs_vec2 v0, gs_vec2 v1)
{
	return gs_vec2_ctor(v0.x - v1.x, v0.y - v1.y);
}

gs_inline gs_vec2 
gs_vec2_mul(gs_vec2 v0, gs_vec2 v1) 
{
	return gs_vec2_ctor(v0.x * v1.x, v0.y * v1.y);
}

gs_inline gs_vec2 
gs_vec2_div(gs_vec2 v0, gs_vec2 v1) 
{
	return gs_vec2_ctor(v0.x / v1.x, v0.y / v1.y);
}

gs_inline gs_vec2 
gs_vec2_scale(gs_vec2 v, f32 s)
{
	return gs_vec2_ctor(v.x * s, v.y * s);
}

gs_inline f32 
gs_vec2_dot(gs_vec2 v0, gs_vec2 v1) 
{
	return (f32)(v0.x * v1.x + v0.y * v1.y);
}

gs_inline f32 
gs_vec2_len(gs_vec2 v)
{
	return (f32)sqrt(gs_vec2_dot(v, v));
}

gs_inline gs_vec2
gs_vec2_project_onto(gs_vec2 v0, gs_vec2 v1)
{
	f32 dot = gs_vec2_dot(v0, v1);
	f32 len = gs_vec2_dot(v1, v1);

	// Orthogonal, so return v1
	if (len == 0.f) return v1;

	return gs_vec2_scale(v1, dot / len);
}

gs_inline gs_vec2 gs_vec2_norm(gs_vec2 v) 
{
	f32 len = gs_vec2_len(v);
	return gs_vec2_scale(v, len != 0.f ? 1.0f / gs_vec2_len(v) : 1.f);
}

gs_inline 
f32 gs_vec2_dist(gs_vec2 a, gs_vec2 b)
{
	f32 dx = (a.x - b.x);
	f32 dy = (a.y - b.y);
	return (sqrt(dx * dx + dy * dy));
}

gs_inline
f32 gs_vec2_cross(gs_vec2 a, gs_vec2 b) 
{
	return a.x * b.y - a.y * b.x;
}

gs_inline
f32 gs_vec2_angle(gs_vec2 a, gs_vec2 b) 
{
	return acos(gs_vec2_dot(a, b) / (gs_vec2_len(a) * gs_vec2_len(b)));
}

gs_inline
b32 gs_vec2_equal(gs_vec2 a, gs_vec2 b)
{
	return (a.x == b.x && a.y == b.y);
}

/*================================================================================
// Vec3
================================================================================*/

typedef struct
{
	union 
	{
		f32 xyz[3];
		struct 
		{
			f32 x, y, z;
		};
	};
} gs_vec3;

gs_inline gs_vec3 
gs_vec3_ctor(f32 _x, f32 _y, f32 _z)
{
	gs_vec3 v;
	v.x = _x;
	v.y = _y;
	v.z = _z;
	return v;
}

gs_inline gs_vec3 
gs_vec3_add(gs_vec3 v0, gs_vec3 v1)
{
	return gs_vec3_ctor(v0.x + v1.x, v0.y + v1.y, v0.z + v1.z);
}

gs_inline gs_vec3 
gs_vec3_sub(gs_vec3 v0, gs_vec3 v1) 
{
	return gs_vec3_ctor(v0.x - v1.x, v0.y - v1.y, v0.z - v1.z);
}

gs_inline gs_vec3 
gs_vec3_mul(gs_vec3 v0, gs_vec3 v1) 
{
	return gs_vec3_ctor(v0.x * v1.x, v0.y * v1.y, v0.z * v1.z);
}

gs_inline gs_vec3 
gs_vec3_div(gs_vec3 v0, gs_vec3 v1) 
{
	return gs_vec3_ctor(v0.x / v1.x, v0.y / v1.y, v0.z / v1.z);
}

gs_inline gs_vec3 
gs_vec3_scale(gs_vec3 v, f32 s) 
{
	return gs_vec3_ctor(v.x * s, v.y * s, v.z * s);
}

gs_inline f32 
gs_vec3_dot(gs_vec3 v0, gs_vec3 v1) 
{
	f32 dot = (f32)((v0.x * v1.x) + (v0.y * v1.y) + v0.z * v1.z);
	return dot;
}

gs_inline f32 
gs_vec3_len(gs_vec3 v)
{
	return (f32)sqrt(gs_vec3_dot(v, v));
}

gs_inline gs_vec3
gs_vec3_project_onto(gs_vec3 v0, gs_vec3 v1)
{
	f32 dot = gs_vec3_dot(v0, v1);
	f32 len = gs_vec3_dot(v1, v1);

	// Orthogonal, so return v1
	if (len == 0.f) return v1;

	return gs_vec3_scale(v1, dot / len);
}

gs_inline 
f32 gs_vec3_dist(gs_vec3 a, gs_vec3 b)
{
	f32 dx = (a.x - b.x);
	f32 dy = (a.y - b.y);
	f32 dz = (a.z - b.z);
	return (sqrt(dx * dx + dy * dy + dz * dz));
}

gs_inline gs_vec3 
gs_vec3_norm(gs_vec3 v)
{
	f32 len = gs_vec3_len(v);
	return len == 0.f ? v : gs_vec3_scale(v, 1.f / len);
}

gs_inline gs_vec3 
gs_vec3_cross(gs_vec3 v0, gs_vec3 v1) 
{
	return gs_vec3_ctor
	(
		v0.y * v1.z - v0.z * v1.y,
		v0.z * v1.x - v0.x * v1.z,
		v0.x * v1.y - v0.y * v1.x
	);
}

gs_inline void gs_vec3_scale_ip(gs_vec3* vp, f32 s)
{
	vp->x *= s;
	vp->y *= s;
	vp->z *= s;
}

/*================================================================================
// Vec4
================================================================================*/

typedef struct
{
	union 
	{
		f32 xyzw[4];
		struct 
		{
			f32 x, y, z, w;
		};
	};
} gs_vec4;

gs_inline gs_vec4 
gs_vec4_ctor(f32 _x, f32 _y, f32 _z, f32 _w)
{
	gs_vec4 v; 
	v.x = _x;
	v.y = _y; 
	v.z = _z; 
	v.w = _w;
	return v;
} 

gs_inline gs_vec4
gs_vec4_add(gs_vec4 v0, gs_vec4 v1) 
{
	return gs_vec4_ctor(v0.x + v1.y, v0.y + v1.y, v0.z + v1.z, v0.w + v1.w);
}

gs_inline gs_vec4
gs_vec4_sub(gs_vec4 v0, gs_vec4 v1) 
{
	return gs_vec4_ctor(v0.x - v1.y, v0.y - v1.y, v0.z - v1.z, v0.w - v1.w);
}

gs_inline gs_vec4
gs_vec4_div(gs_vec4 v0, gs_vec4 v1) 
{
	return gs_vec4_ctor(v0.x / v1.x, v0.y / v1.y, v0.z / v1.z, v0.w / v1.w);
}

gs_inline gs_vec4
gs_vec4_scale(gs_vec4 v, f32 s) 
{
	return gs_vec4_ctor(v.x / s, v.y / s, v.z / s, v.w / s);
}

gs_inline f32
gs_vec4_dot(gs_vec4 v0, gs_vec4 v1) 
{
	return (f32)(v0.x * v1.x + v0.y * v1.y + v0.z * v1.z + v0.w * v1.w);
}

gs_inline f32
gs_vec4_len(gs_vec4 v) 
{
	return (f32)sqrt(gs_vec4_dot(v, v));
}

gs_inline gs_vec4
gs_vec4_project_onto(gs_vec4 v0, gs_vec4 v1)
{
	f32 dot = gs_vec4_dot(v0, v1);
	f32 len = gs_vec4_dot(v1, v1);

	// Orthogonal, so return v1
	if (len == 0.f) return v1;

	return gs_vec4_scale(v1, dot / len);
}

gs_inline gs_vec4
gs_vec4_norm(gs_vec4 v) 
{
	return gs_vec4_scale(v, 1.0f / gs_vec4_len(v));
}

gs_inline f32
gs_vec4_dist(gs_vec4 v0, gs_vec4 v1)
{
	f32 dx = (v0.x - v1.x);
	f32 dy = (v0.y - v1.y);
	f32 dz = (v0.z - v1.z);
	f32 dw = (v0.w - v1.w);
	return (sqrt(dx * dx + dy * dy + dz * dz + dw * dw));
}

/*================================================================================
// Useful Vector Functions
================================================================================*/

gs_inline
gs_vec3 gs_v4_to_v3(gs_vec4 v) 
{
	return gs_v3(v.x, v.y, v.z);
}

/*================================================================================
// Mat4x4
================================================================================*/

/*
	Matrices are stored in linear, contiguous memory and assume a column-major ordering.
*/

typedef struct gs_mat4
{
	f32 elements[16];
} gs_mat4;

gs_inline gs_mat4 
gs_mat4_diag(f32 val)
{
	gs_mat4 m;
	memset(m.elements, 0, sizeof(m.elements));
	m.elements[0 + 0 * 4] = val;
	m.elements[1 + 1 * 4] = val;
	m.elements[2 + 2 * 4] = val;
	m.elements[3 + 3 * 4] = val;
	return m;
}

#define gs_mat4_identity()\
	gs_mat4_diag(1.0f)

gs_inline gs_mat4
gs_mat4_ctor() {
	gs_mat4 mat = {0};
	return mat;
}

gs_inline gs_mat4 
gs_mat4_mul(gs_mat4 m0, gs_mat4 m1)
{
	gs_mat4 m_res = gs_mat4_ctor(); 
	for (u32 y = 0; y < 4; ++y)
	{
		for (u32 x = 0; x < 4; ++x)
		{
			f32 sum = 0.0f;
			for (u32 e = 0; e < 4; ++e)
			{
				sum += m0.elements[x + e * 4] * m1.elements[e + y * 4];
			}
			m_res.elements[x + y * 4] = sum;
		}
	}

	return m_res;
}

gs_inline
void gs_mat4_set_elements(gs_mat4* m, f32* elements, u32 count)
{
	for (u32 i = 0; i < count; ++i)
	{
		m->elements[i] = elements[i];
	}
}

gs_inline
gs_mat4 gs_mat4_transpose(gs_mat4 m)
{
	gs_mat4 t = gs_mat4_identity();

	// First row
	t.elements[0 * 4 + 0] = m.elements[0 * 4 + 0];
	t.elements[1 * 4 + 0] = m.elements[0 * 4 + 1];
	t.elements[2 * 4 + 0] = m.elements[0 * 4 + 2];
	t.elements[3 * 4 + 0] = m.elements[0 * 4 + 3];

	// Second row
	t.elements[0 * 4 + 1] = m.elements[1 * 4 + 0];
	t.elements[1 * 4 + 1] = m.elements[1 * 4 + 1];
	t.elements[2 * 4 + 1] = m.elements[1 * 4 + 2];
	t.elements[3 * 4 + 1] = m.elements[1 * 4 + 3];

	// Third row
	t.elements[0 * 4 + 2] = m.elements[2 * 4 + 0];
	t.elements[1 * 4 + 2] = m.elements[2 * 4 + 1];
	t.elements[2 * 4 + 2] = m.elements[2 * 4 + 2];
	t.elements[3 * 4 + 2] = m.elements[2 * 4 + 3];

	// Fourth row
	t.elements[0 * 4 + 3] = m.elements[3 * 4 + 0];
	t.elements[1 * 4 + 3] = m.elements[3 * 4 + 1];
	t.elements[2 * 4 + 3] = m.elements[3 * 4 + 2];
	t.elements[3 * 4 + 3] = m.elements[3 * 4 + 3];

	return t;
}

gs_inline
gs_mat4 gs_mat4_inverse(gs_mat4 m)
{
	gs_mat4 res = gs_mat4_identity();

	f32 temp[16];

	temp[0] = m.elements[5] * m.elements[10] * m.elements[15] -
		m.elements[5] * m.elements[11] * m.elements[14] -
		m.elements[9] * m.elements[6] * m.elements[15] +
		m.elements[9] * m.elements[7] * m.elements[14] +
		m.elements[13] * m.elements[6] * m.elements[11] -
		m.elements[13] * m.elements[7] * m.elements[10];

	temp[4] = -m.elements[4] * m.elements[10] * m.elements[15] +
		m.elements[4] * m.elements[11] * m.elements[14] +
		m.elements[8] * m.elements[6] * m.elements[15] -
		m.elements[8] * m.elements[7] * m.elements[14] -
		m.elements[12] * m.elements[6] * m.elements[11] +
		m.elements[12] * m.elements[7] * m.elements[10];

	temp[8] = m.elements[4] * m.elements[9] * m.elements[15] -
		m.elements[4] * m.elements[11] * m.elements[13] -
		m.elements[8] * m.elements[5] * m.elements[15] +
		m.elements[8] * m.elements[7] * m.elements[13] +
		m.elements[12] * m.elements[5] * m.elements[11] -
		m.elements[12] * m.elements[7] * m.elements[9];

	temp[12] = -m.elements[4] * m.elements[9] * m.elements[14] +
		m.elements[4] * m.elements[10] * m.elements[13] +
		m.elements[8] * m.elements[5] * m.elements[14] -
		m.elements[8] * m.elements[6] * m.elements[13] -
		m.elements[12] * m.elements[5] * m.elements[10] +
		m.elements[12] * m.elements[6] * m.elements[9];

	temp[1] = -m.elements[1] * m.elements[10] * m.elements[15] +
		m.elements[1] * m.elements[11] * m.elements[14] +
		m.elements[9] * m.elements[2] * m.elements[15] -
		m.elements[9] * m.elements[3] * m.elements[14] -
		m.elements[13] * m.elements[2] * m.elements[11] +
		m.elements[13] * m.elements[3] * m.elements[10];

	temp[5] = m.elements[0] * m.elements[10] * m.elements[15] -
		m.elements[0] * m.elements[11] * m.elements[14] -
		m.elements[8] * m.elements[2] * m.elements[15] +
		m.elements[8] * m.elements[3] * m.elements[14] +
		m.elements[12] * m.elements[2] * m.elements[11] -
		m.elements[12] * m.elements[3] * m.elements[10];

	temp[9] = -m.elements[0] * m.elements[9] * m.elements[15] +
		m.elements[0] * m.elements[11] * m.elements[13] +
		m.elements[8] * m.elements[1] * m.elements[15] -
		m.elements[8] * m.elements[3] * m.elements[13] -
		m.elements[12] * m.elements[1] * m.elements[11] +
		m.elements[12] * m.elements[3] * m.elements[9];

	temp[13] = m.elements[0] * m.elements[9] * m.elements[14] -
		m.elements[0] * m.elements[10] * m.elements[13] -
		m.elements[8] * m.elements[1] * m.elements[14] +
		m.elements[8] * m.elements[2] * m.elements[13] +
		m.elements[12] * m.elements[1] * m.elements[10] -
		m.elements[12] * m.elements[2] * m.elements[9];

	temp[2] = m.elements[1] * m.elements[6] * m.elements[15] -
		m.elements[1] * m.elements[7] * m.elements[14] -
		m.elements[5] * m.elements[2] * m.elements[15] +
		m.elements[5] * m.elements[3] * m.elements[14] +
		m.elements[13] * m.elements[2] * m.elements[7] -
		m.elements[13] * m.elements[3] * m.elements[6];

	temp[6] = -m.elements[0] * m.elements[6] * m.elements[15] +
		m.elements[0] * m.elements[7] * m.elements[14] +
		m.elements[4] * m.elements[2] * m.elements[15] -
		m.elements[4] * m.elements[3] * m.elements[14] -
		m.elements[12] * m.elements[2] * m.elements[7] +
		m.elements[12] * m.elements[3] * m.elements[6];

	temp[10] = m.elements[0] * m.elements[5] * m.elements[15] -
		m.elements[0] * m.elements[7] * m.elements[13] -
		m.elements[4] * m.elements[1] * m.elements[15] +
		m.elements[4] * m.elements[3] * m.elements[13] +
		m.elements[12] * m.elements[1] * m.elements[7] -
		m.elements[12] * m.elements[3] * m.elements[5];

	temp[14] = -m.elements[0] * m.elements[5] * m.elements[14] +
		m.elements[0] * m.elements[6] * m.elements[13] +
		m.elements[4] * m.elements[1] * m.elements[14] -
		m.elements[4] * m.elements[2] * m.elements[13] -
		m.elements[12] * m.elements[1] * m.elements[6] +
		m.elements[12] * m.elements[2] * m.elements[5];

	temp[3] = -m.elements[1] * m.elements[6] * m.elements[11] +
		m.elements[1] * m.elements[7] * m.elements[10] +
		m.elements[5] * m.elements[2] * m.elements[11] -
		m.elements[5] * m.elements[3] * m.elements[10] -
		m.elements[9] * m.elements[2] * m.elements[7] +
		m.elements[9] * m.elements[3] * m.elements[6];

	temp[7] = m.elements[0] * m.elements[6] * m.elements[11] -
		m.elements[0] * m.elements[7] * m.elements[10] -
		m.elements[4] * m.elements[2] * m.elements[11] +
		m.elements[4] * m.elements[3] * m.elements[10] +
		m.elements[8] * m.elements[2] * m.elements[7] -
		m.elements[8] * m.elements[3] * m.elements[6];

	temp[11] = -m.elements[0] * m.elements[5] * m.elements[11] +
		m.elements[0] * m.elements[7] * m.elements[9] +
		m.elements[4] * m.elements[1] * m.elements[11] -
		m.elements[4] * m.elements[3] * m.elements[9] -
		m.elements[8] * m.elements[1] * m.elements[7] +
		m.elements[8] * m.elements[3] * m.elements[5];

	temp[15] = m.elements[0] * m.elements[5] * m.elements[10] -
		m.elements[0] * m.elements[6] * m.elements[9] -
		m.elements[4] * m.elements[1] * m.elements[10] +
		m.elements[4] * m.elements[2] * m.elements[9] +
		m.elements[8] * m.elements[1] * m.elements[6] -
		m.elements[8] * m.elements[2] * m.elements[5];

	f64 determinant = m.elements[0] * temp[0] + m.elements[1] * temp[4] + m.elements[2] * temp[8] + m.elements[3] * temp[12];
	determinant = 1.0 / determinant;

	for (int i = 0; i < 4 * 4; i++)
		res.elements[i] = temp[i] * determinant;

	return res;
}

/*
	f32 l : left
	f32 r : right
	f32 b : bottom
	f32 t : top
	f32 n : near
	f32 f : far
*/
gs_inline gs_mat4 
gs_mat4_ortho(f32 l, f32 r, f32 b, f32 t, f32 n, f32 f)
{
	gs_mat4 m_res = gs_mat4_identity();		

	// Main diagonal
	m_res.elements[0 + 0 * 4] = 2.0f / (r - l);
	m_res.elements[1 + 1 * 4] = 2.0f / (t - b);
	m_res.elements[2 + 2 * 4] = -2.0f / (f - n);

	// Last column
	m_res.elements[0 + 3 * 4] = -(r + l) / (r - l);
	m_res.elements[1 + 3 * 4] = -(t + b) / (t - b);
	m_res.elements[2 + 3 * 4] = -(f + n) / (f - n);

	return m_res;
}

gs_inline gs_mat4 
gs_mat4_perspective(f32 fov, f32 asp_ratio, f32 n, f32 f)
{
	// Zero matrix
	gs_mat4 m_res = gs_mat4_ctor();

	f32 q = 1.0f / tan(gs_deg_to_rad(0.5f * fov));
	f32 a = q / asp_ratio;
	f32 b = (n + f) / (n - f);
	f32 c = (2.0f * n * f) / (n - f);

	m_res.elements[0 + 0 * 4] = a;
	m_res.elements[1 + 1 * 4] = q;
	m_res.elements[2 + 2 * 4] = b;
	m_res.elements[2 + 3 * 4] = c;
	m_res.elements[3 + 2 * 4] = -1.0f;

	return m_res;
}

gs_inline gs_mat4 
gs_mat4_translate(const gs_vec3 v)
{
	gs_mat4 m_res = gs_mat4_identity();

	m_res.elements[0 + 4 * 3] = v.x;
	m_res.elements[1 + 4 * 3] = v.y;
	m_res.elements[2 + 4 * 3] = v.z;

	return m_res;
}

gs_inline gs_mat4 
gs_mat4_scale(const gs_vec3 v)
{
	gs_mat4 m_res = gs_mat4_identity();
	m_res.elements[0 + 0 * 4] = v.x;
	m_res.elements[1 + 1 * 4] = v.y;
	m_res.elements[2 + 2 * 4] = v.z;
	return m_res;
}

gs_inline gs_mat4 gs_mat4_rotate(f32 angle, gs_vec3 axis)
{
	gs_mat4 m_res = gs_mat4_identity();

	f32 a = gs_deg_to_rad(angle);
	f32 c = cos(a);
	f32 s = sin(a);

	f32 x = axis.x;
	f32 y = axis.y;
	f32 z = axis.z;

	//First column
	m_res.elements[0 + 0 * 4] = x * x * (1 - c) + c;	
	m_res.elements[1 + 0 * 4] = x * y * (1 - c) + z * s;	
	m_res.elements[2 + 0 * 4] = x * z * (1 - c) - y * s;	
	
	//Second column
	m_res.elements[0 + 1 * 4] = x * y * (1 - c) - z * s;	
	m_res.elements[1 + 1 * 4] = y * y * (1 - c) + c;	
	m_res.elements[2 + 1 * 4] = y * z * (1 - c) + x * s;	
	
	//Third column
	m_res.elements[0 + 2 * 4] = x * z * (1 - c) + y * s;	
	m_res.elements[1 + 2 * 4] = y * z * (1 - c) - x * s;	
	m_res.elements[2 + 2 * 4] = z * z * (1 - c) + c;	

	return m_res;
}

gs_inline gs_mat4 
gs_mat4_look_at(gs_vec3 position, gs_vec3 target, gs_vec3 up)
{
	gs_vec3 f = gs_vec3_norm(gs_vec3_sub(target, position));
	gs_vec3 s = gs_vec3_norm(gs_vec3_cross(f, up));
	gs_vec3 u = gs_vec3_cross(s, f);

	gs_mat4 m_res = gs_mat4_identity();
	m_res.elements[0 * 4 + 0] = s.x;
	m_res.elements[1 * 4 + 0] = s.y;
	m_res.elements[2 * 4 + 0] = s.z;

	m_res.elements[0 * 4 + 1] = u.x;
	m_res.elements[1 * 4 + 1] = u.y;
	m_res.elements[2 * 4 + 1] = u.z;

	m_res.elements[0 * 4 + 2] = -f.x;
	m_res.elements[1 * 4 + 2] = -f.y;
	m_res.elements[2 * 4 + 2] = -f.z;

	m_res.elements[3 * 4 + 0] = -gs_vec3_dot(s, position);;
	m_res.elements[3 * 4 + 1] = -gs_vec3_dot(u, position);
	m_res.elements[3 * 4 + 2] = gs_vec3_dot(f, position); 

	return m_res;
}

gs_inline
gs_vec3 gs_mat4_mul_vec3(gs_mat4 m, gs_vec3 v)
{
	m = gs_mat4_transpose(m);
	return gs_vec3_ctor
	(
		m.elements[0 * 4 + 0] * v.x + m.elements[0 * 4 + 1] * v.y + m.elements[0 * 4 + 2] * v.z,  
		m.elements[1 * 4 + 0] * v.x + m.elements[1 * 4 + 1] * v.y + m.elements[1 * 4 + 2] * v.z,  
		m.elements[2 * 4 + 0] * v.x + m.elements[2 * 4 + 1] * v.y + m.elements[2 * 4 + 2] * v.z
	);
}
	
gs_inline
gs_vec4 gs_mat4_mul_vec4(gs_mat4 m, gs_vec4 v)
{
	m = gs_mat4_transpose(m);
	return gs_vec4_ctor
	(
		m.elements[0 * 4 + 0] * v.x + m.elements[0 * 4 + 1] * v.y + m.elements[0 * 4 + 2] * v.z + m.elements[0 * 4 + 3] * v.w,  
		m.elements[1 * 4 + 0] * v.x + m.elements[1 * 4 + 1] * v.y + m.elements[1 * 4 + 2] * v.z + m.elements[1 * 4 + 3] * v.w,  
		m.elements[2 * 4 + 0] * v.x + m.elements[2 * 4 + 1] * v.y + m.elements[2 * 4 + 2] * v.z + m.elements[2 * 4 + 3] * v.w,  
		m.elements[3 * 4 + 0] * v.x + m.elements[3 * 4 + 1] * v.y + m.elements[3 * 4 + 2] * v.z + m.elements[3 * 4 + 3] * v.w
	);
}

/*================================================================================
// Quaternion
================================================================================*/

typedef struct
{
	union 
	{
		f32 xyzw[4];
		struct 
		{
			f32 x, y, z, w;
		};
	};
} gs_quat;

gs_inline
gs_quat gs_quat_default()
{
	gs_quat q;
	q.x = 0.f;	
	q.y = 0.f;	
	q.z = 0.f;	
	q.w = 1.f;	
	return q;
}

gs_inline
gs_quat gs_quat_ctor(f32 _x, f32 _y, f32 _z, f32 _w)
{
	gs_quat q;
	q.x = _x;
	q.y = _y;
	q.z = _z;
	q.w = _w;
	return q;
}

gs_inline gs_quat 
gs_quat_add(gs_quat q0, gs_quat q1) 
{
	return gs_quat_ctor(q0.x + q1.x, q0.y + q1.y, q0.z + q1.z, q0.w + q1.w);
}

gs_inline gs_quat 
gs_quat_sub(gs_quat q0, gs_quat q1)
{
	return gs_quat_ctor(q0.x - q1.x, q0.y - q1.y, q0.z - q1.z, q0.w - q1.w);
}

gs_inline gs_quat
gs_quat_mul(gs_quat q0, gs_quat q1)
{
	return gs_quat_ctor(
		q0.w * q1.x + q1.w * q0.x + q0.y * q1.z - q1.y * q0.z,
		q0.w * q1.y + q1.w * q0.y + q0.z * q1.x - q1.z * q0.x,
		q0.w * q1.z + q1.w * q0.z + q0.x * q1.y - q1.x * q0.y,
		q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z
	);
}

gs_inline 
gs_quat gs_quat_mul_list(u32 count, ...)
{
	va_list ap;
	gs_quat q = gs_quat_default();
	va_start(ap, count);
	for (u32 i = 0; i < count; ++i)
	{
		q = gs_quat_mul(q, va_arg(ap, gs_quat));
	}
	va_end(ap);
	return q;
}

gs_inline gs_quat 
gs_quat_mul_quat(gs_quat q0, gs_quat q1)
{
	return gs_quat_ctor(
		q0.w * q1.x + q1.w * q0.x + q0.y * q1.z - q1.y * q0.z,
		q0.w * q1.y + q1.w * q0.y + q0.z * q1.x - q1.z * q0.x,
		q0.w * q1.z + q1.w * q0.z + q0.x * q1.y - q1.x * q0.y,
		q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z
	);
}

gs_inline 
gs_quat gs_quat_scale(gs_quat q, f32 s)
{
	return gs_quat_ctor(q.x * s, q.y * s, q.z * s, q.w * s);
}

gs_inline f32 
gs_quat_dot(gs_quat q0, gs_quat q1)
{
	return (f32)(q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w);
}

gs_inline 
gs_quat gs_quat_conjugate(gs_quat q)
{
	return (gs_quat_ctor(-q.x, -q.y, -q.z, q.w));
}

gs_inline f32
gs_quat_len(gs_quat q)
{
	return (f32)sqrt(gs_quat_dot(q, q));
}

gs_inline gs_quat
gs_quat_norm(gs_quat q) 
{
	return gs_quat_scale(q, 1.0f / gs_quat_len(q));
}

gs_inline gs_quat
gs_quat_cross(gs_quat q0, gs_quat q1)
{
	return gs_quat_ctor (											
		q0.x * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y,	
		q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z,	
		q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x,	
		q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z 	
	);
}

// Inverse := Conjugate / Dot;
gs_inline
gs_quat gs_quat_inverse(gs_quat q)
{
	return (gs_quat_scale(gs_quat_conjugate(q), 1.0f / gs_quat_dot(q, q)));
}

gs_inline gs_vec3 gs_quat_rotate(gs_quat q, gs_vec3 v)
{
	// nVidia SDK implementation
	gs_vec3 qvec = gs_vec3_ctor(q.x, q.y, q.z);
	gs_vec3 uv = gs_vec3_cross(qvec, v);
	gs_vec3 uuv = gs_vec3_cross(qvec, uv);
	uv = gs_vec3_scale(uv, 2.f * q.w);
	uuv = gs_vec3_scale(uuv, 2.f);
	return (gs_vec3_add(v, gs_vec3_add(uv, uuv)));
}

gs_inline gs_quat gs_quat_angle_axis(f32 rad, gs_vec3 axis)
{
	// Normalize axis
	gs_vec3 a = gs_vec3_norm(axis);

	// Get scalar
	f32 half_angle = 0.5f * rad;
	f32 s = sin(half_angle);

	return gs_quat_ctor(a.x * s, a.y * s, a.z * s, cos(half_angle));
}

gs_inline
gs_quat gs_quat_slerp(gs_quat a, gs_quat b, f32 t)
{
	f32 c = gs_quat_dot(a, b);
	gs_quat end = b;

	if (c < 0.0f)
	{
		// Reverse all signs
		c *= -1.0f;
		end.x *= -1.0f;
		end.y *= -1.0f;
		end.z *= -1.0f;
		end.w *= -1.0f;
	}

	// Calculate coefficients
	f32 sclp, sclq;
	if ((1.0f - c) > 0.0001f)
	{
		f32 omega = acosf(c);
		f32 s = sinf(omega);
		sclp = sinf((1.0f - t) * omega) / s;
		sclq = sinf(t * omega) / s; 
	}
	else
	{
		sclp = 1.0f - t;
		sclq = t;
	}

	gs_quat q;
	q.x = sclp * a.x + sclq * end.x;
	q.y = sclp * a.y + sclq * end.y;
	q.z = sclp * a.z + sclq * end.z;
	q.w = sclp * a.w + sclq * end.w;

	return q;
}

#define quat_axis_angle(axis, angle)\
	gs_quat_angle_axis(angle, axis)

/*
* @brief Convert given quaternion param into equivalent 4x4 rotation matrix
* @note: From http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm 
*/
gs_inline gs_mat4 gs_quat_to_mat4(gs_quat _q)
{
	gs_mat4 mat = gs_mat4_identity();
	gs_quat q = gs_quat_norm(_q);

	f32 xx = q.x * q.x;	
	f32 yy = q.y * q.y;	
	f32 zz = q.z * q.z;	
	f32 xy = q.x * q.y;
	f32 xz = q.x * q.z;
	f32 yz = q.y * q.z;
	f32 wx = q.w * q.x;
	f32 wy = q.w * q.y;
	f32 wz = q.w * q.z;

	mat.elements[0 * 4 + 0] = 1.0f - 2.0f * (yy + zz);
	mat.elements[1 * 4 + 0] = 2.0f * (xy - wz);
	mat.elements[2 * 4 + 0] = 2.0f * (xz + wy);

	mat.elements[0 * 4 + 1] = 2.0f * (xy + wz);
	mat.elements[1 * 4 + 1] = 1.0f - 2.0f * (xx + zz);
	mat.elements[2 * 4 + 1] = 2.0f * (yz - wx);

	mat.elements[0 * 4 + 2] = 2.0f * (xz - wy);
	mat.elements[1 * 4 + 2] = 2.0f * (yz + wx);
	mat.elements[2 * 4 + 2] = 1.0f - 2.0f * (xx + yy);

	return mat;
}

gs_inline 
gs_quat gs_quat_from_euler(f32 yaw_deg, f32 pitch_deg, f32 roll_deg)
{
	f32 yaw = gs_deg_to_rad(yaw_deg);
	f32 pitch = gs_deg_to_rad(pitch_deg);
	f32 roll = gs_deg_to_rad(roll_deg);

	gs_quat q;
	f32 cy = cos(yaw * 0.5f);
	f32 sy = sin(yaw * 0.5f);
	f32 cr = cos(roll * 0.5f);
	f32 sr = sin(roll * 0.5f);
	f32 cp = cos(pitch * 0.5f);
	f32 sp = sin(pitch * 0.5f);

	q.x = cy * sr * cp - sy * cr * sp;
	q.y = cy * cr * sp + sy * sr * cp;
	q.z = sy * cr * cp - cy * sr * sp;
	q.w = cy * cr * cp + sy * sr * sp;

	return q;
}

/*================================================================================
// Transform (Non-Uniform Scalar VQS)
================================================================================*/

/*
	- This follows a traditional 'VQS' structure for complex object transformations, 
		however it differs from the standard in that it allows for non-uniform 
		scaling in the form of a vec3.
*/
// Source: https://www.eurosis.org/cms/files/conf/gameon-asia/gameon-asia2007/R-SESSION/G1.pdf

typedef struct 
{
	gs_vec3 	position;
	gs_quat 	rotation;
	gs_vec3 	scale;		
} gs_vqs;

gs_inline gs_vqs gs_vqs_ctor(gs_vec3 tns, gs_quat rot, gs_vec3 scl)
{
	gs_vqs t;	
	t.position = tns;
	t.rotation = rot;
	t.scale = scl;
	return t;
}

gs_inline 
gs_vqs gs_vqs_default()
{
	gs_vqs t = gs_vqs_ctor
	(
		gs_vec3_ctor(0.0f, 0.0f, 0.0f),
		gs_quat_ctor(0.0f, 0.0f, 0.0f, 1.0f),
		gs_vec3_ctor(1.0f, 1.0f, 1.0f)
	);
	return t;
}

// AbsScale	= ParentScale * LocalScale
// AbsRot	= LocalRot * ParentRot
// AbsTrans	= ParentPos + [ParentRot * (ParentScale * LocalPos)]
gs_inline gs_vqs gs_vqs_absolute_transform(const gs_vqs* local, const gs_vqs* parent)
{
	// Normalized rotations
	gs_quat p_rot_norm = gs_quat_norm(parent->rotation);
	gs_quat l_rot_norm = gs_quat_norm(local->rotation);

	// Scale
	gs_vec3 scl = gs_vec3_mul(local->scale, parent->scale);
	// Rotation
	gs_quat rot = gs_quat_norm(gs_quat_mul(p_rot_norm, l_rot_norm));
	// position
	gs_vec3 tns = gs_vec3_add(parent->position, gs_quat_rotate(p_rot_norm, gs_vec3_mul(parent->scale, local->position)));

	return gs_vqs_ctor(tns, rot, scl);
}

// RelScale = AbsScale / ParentScale 
// RelRot	= Inverse(ParentRot) * AbsRot
// RelTrans	= [Inverse(ParentRot) * (AbsPos - ParentPosition)] / ParentScale;
gs_inline gs_vqs gs_vqs_relative_transform(const gs_vqs* absolute, const gs_vqs* parent)
{
	// Get inverse rotation normalized
	gs_quat p_rot_inv = gs_quat_norm(gs_quat_inverse(parent->rotation));
	// Normalized abs rotation
	gs_quat a_rot_norm = gs_quat_norm(absolute->rotation);

	// Scale
	gs_vec3 scl = gs_vec3_div(absolute->scale, parent->scale);
	// Rotation
	gs_quat rot = gs_quat_norm(gs_quat_mul(p_rot_inv, a_rot_norm));
	// position
	gs_vec3 tns = gs_vec3_div(gs_quat_rotate(p_rot_inv, gs_vec3_sub(absolute->position, parent->position)), parent->scale);

	return gs_vqs_ctor(tns, rot, scl);
}

gs_inline gs_mat4 gs_vqs_to_mat4(const gs_vqs* transform)
{
	gs_mat4 mat = gs_mat4_identity();
	gs_mat4 trans = gs_mat4_translate(transform->position);
	gs_mat4 rot = gs_quat_to_mat4(transform->rotation);
	gs_mat4 scl = gs_mat4_scale(transform->scale);
	mat = gs_mat4_mul(mat, trans);
	mat = gs_mat4_mul(mat, rot);
	mat = gs_mat4_mul(mat, scl);
	return mat;
}

/*================================================================================
// Ray
================================================================================*/

typedef struct 
{
	gs_vec3 point;
	gs_vec3 direction;	
} gs_ray;

gs_inline gs_ray gs_ray_ctor(gs_vec3 pt, gs_vec3 dir)
{
	gs_ray r;
	r.point = pt;
	r.direction = dir;
	return r;
}

/*================================================================================
// Plane
================================================================================*/

typedef struct gs_plane_t
{
	union
	{
		gs_vec3 n;
		struct 
		{
			f32 a;
			f32 b;
			f32 c;
		};
	};

	f32 d;
} gs_plane_t;

/*================================================================================
// Utils
================================================================================*/

/*
	min is top left of rect,
	max is bottom right
*/
typedef struct gs_rect_t
{
	gs_vec2 min;
	gs_vec2 max;
} gs_rect_t;

gs_inline
b32 gs_rect_vs_rect(gs_rect_t a, gs_rect_t b)
{
	if ( a.max.x > b.min.x && 
		 a.max.y > b.min.y && 
		 a.min.x < b.max.x && 
		 a.min.y < b.max.y )
	{
		return true;
	}

	return false;
}

#ifdef __cplusplus
}
#endif 	// c++

#endif // __GS_MATH_H__