Common 3D math

vmath.h

#pragma once

#ifndef VMATH_INLINE
#	define VMATH_INLINE inline
#endif

VMATH_INLINE float clamp(float x, float minv, float maxv) { return fmaxf(minv, fminf(maxv, x)); }
VMATH_INLINE float lerp(float a, float b, float t) { return a * (1.0f - t) + b * t; }

struct V3
{
	constexpr V3() { *this = { 0, 0, 0 }; }
	constexpr V3(float x, float y, float z) : x(x), y(y), z(z) {}
	constexpr VMATH_INLINE float operator[](int i) { return *((float*)this + i); }

	float x;
	float y;
	float z;
};

VMATH_INLINE V3 operator+(V3 a, V3 b) { return { a.x + b.x, a.y + b.y, a.z + b.z }; }
VMATH_INLINE V3 operator-(V3 a, V3 b) { return { a.x - b.x, a.y - b.y, a.z - b.z }; }
VMATH_INLINE V3 operator*(V3 a, V3 b) { return { a.x * b.x, a.y * b.y, a.z * b.z }; }
VMATH_INLINE V3 operator/(V3 a, V3 b) { return { a.x / b.x, a.y / b.y, a.z / b.z }; }
VMATH_INLINE V3 operator-(V3 a) { return { -a.x, -a.y, -a.z }; }

VMATH_INLINE V3 operator*(V3 a, float s) { return { a.x * s, a.y * s, a.z * s }; }
VMATH_INLINE V3 operator*(float s, V3 a) { return a * s; }
VMATH_INLINE V3 operator/(V3 a, float s) { float r = 1.0f / s; return a * r; }

VMATH_INLINE V3& operator+=(V3& a, V3 b) { return a = a + b; }
VMATH_INLINE V3& operator-=(V3& a, V3 b) { return a = a - b; }
VMATH_INLINE V3& operator*=(V3& a, V3 b) { return a = a * b; }
VMATH_INLINE V3& operator/=(V3& a, V3 b) { return a = a / b; }
VMATH_INLINE V3& operator*=(V3& a, float s) { return a = a * s; }
VMATH_INLINE V3& operator/=(V3& a, float s) { return a = a / s; }

VMATH_INLINE float dot(V3 a, V3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
VMATH_INLINE V3 cross(V3 a, V3 b) { return { a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x }; }

VMATH_INLINE float length_sq(V3 a) { return dot(a, a); }
VMATH_INLINE float length(V3 a) { return sqrtf(length_sq(a)); }
VMATH_INLINE V3 norm(V3 a) { return a / length(a); }
VMATH_INLINE V3 safe_norm(V3 a) { float l = length(a); return l != 0.0f ? a / l : V3(0,0,0); }
VMATH_INLINE V3 min(V3 a, V3 b) { return { fminf(a.x, b.x), fminf(a.y, b.y), fminf(a.z, b.z) }; }
VMATH_INLINE V3 max(V3 a, V3 b) { return { fmaxf(a.x, b.x), fmaxf(a.y, b.y), fmaxf(a.z, b.z) }; }
VMATH_INLINE float hmin(V3 a) { return fminf(fminf(a.x, a.y), a.z); }
VMATH_INLINE float hmax(V3 a) { return fmaxf(fmaxf(a.x, a.y), a.z); }
VMATH_INLINE V3 reflect(V3 v, V3 n) { return v - n * (2.0f * dot(v, n)); }
VMATH_INLINE V3 project(V3 a, V3 b) { return b * (dot(a, b) / dot(b, b)); } // a onto b
VMATH_INLINE float triple(V3 a, V3 b, V3 c) { return dot(a, cross(b, c)); }
VMATH_INLINE V3 clamp(V3 v, V3 minv, V3 maxv) { return max(minv, min(v, maxv)); }
VMATH_INLINE V3 abs(V3 v) { return V3(fabsf(v.x), fabsf(v.y), fabsf(v.z)); }
VMATH_INLINE bool is_zero(V3 v) { return v.x == 0.0f && v.y == 0.0f && v.z == 0.0f; }
VMATH_INLINE V3 lerp(V3 a, V3 b, float t) { return a * (1.0f - t) + b * t; }
VMATH_INLINE V3 bary_lerp(V3 a, V3 b, V3 c, float u, float v) { return a * (1.0f - u - v) + b * u + c * v; }
VMATH_INLINE float distance(V3 a, V3 b) { V3 d = b - a; return sqrtf(dot(d, d)); }
VMATH_INLINE float distance_sq(V3 a, V3 b) { V3 d = b - a; return dot(d, d); }

VMATH_INLINE float sq_distance_point_segment(V3 p, V3 a, V3 b)
{
	V3 ab = b - a;
	V3 ap = a - p;
	float c = dot(ab, ap);

	// Closest point is a.
	if (c > 0.0f) {
		return dot(ap, ap);
	}

	V3 bp = p - b;

	// Closest point is b.
	if (dot(ab, bp) > 0.0f) {
		return dot(bp, bp);
	}

	// Closest point is between a and b.
	V3 e = ap - ab * (c / dot(ab, ab));
	return dot(e, e);
}

VMATH_INLINE V3 spherical_to_cartesian(float theta, float phi)
{
	return V3(
		sinf(phi) * cosf(theta),
		cosf(phi),
		sinf(phi) * sinf(theta)
	);
}

struct M3
{
	constexpr M3() { *this = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; }
	constexpr M3(V3 x, V3 y, V3 z) : x(x), y(y), z(z) {}
	constexpr VMATH_INLINE V3 operator[](int i) { return *((V3*)this + i); }

	V3 x;
	V3 y;
	V3 z;
};

VMATH_INLINE V3 operator*(M3 m, V3 v) { return m.x * v.x + m.y * v.y + m.z * v.z; }
VMATH_INLINE M3 operator*(M3 a, M3 b) { return M3(a * b.x, a * b.y, a * b.z ); }
VMATH_INLINE M3 operator+(M3 a, M3 b) { return M3(a.x + b.x, a.y + b.y, a.z + b.z); }
VMATH_INLINE M3 operator-(M3 a, M3 b) { return M3(a.x - b.x, a.y - b.y, a.z - b.z); }
VMATH_INLINE M3 operator*(float s, M3 a) { return M3(s * a.x, s * a.y, s * a.z ); }
VMATH_INLINE M3 transpose(M3 m) { return M3(V3(m.x.x, m.y.x, m.z.x), V3(m.x.y, m.y.y, m.z.y), V3(m.x.z, m.y.z, m.z.z)); }
VMATH_INLINE float determinant(M3 m) { return dot(m.x, cross(m.y, m.z)); }
VMATH_INLINE M3 skew(V3 v) { return M3(V3(0, -v.z, v.y), V3(v.z, 0, -v.x), V3(-v.y, v.x, 0)); }
VMATH_INLINE M3 rotate_x(float a) { return M3(V3(1, 0, 0), V3(0, cosf(a), -sinf(a)), V3(0, sinf(a), cosf(a))); }
VMATH_INLINE M3 rotate_y(float a) { return M3(V3(cosf(a), 0, sinf(a)), V3(0, 1, 0), V3(-sinf(a), 0, cosf(a))); }
VMATH_INLINE M3 rotate_z(float a) { return M3(V3(cosf(a), -sinf(a), 0), V3(sinf(a), cosf(a), 0), V3(0, 0, 1)); }

VMATH_INLINE M3 inverse(M3 m)
{
	V3 c0 = cross(m.y, m.z);
	V3 c1 = cross(m.z, m.x);
	V3 c2 = cross(m.x, m.y);
	float inv_det = 1.0f / dot(m.x, c0);
	return M3(c0 * inv_det, c1 * inv_det, c2 * inv_det);
}

VMATH_INLINE V3 mul_T(M3 m, V3 v)
{
	return V3(
		v.x * m.x.x + v.y * m.y.x + v.z * m.z.x,
		v.x * m.x.y + v.y * m.y.y + v.z * m.z.y,
		v.x * m.x.z + v.y * m.y.z + v.z * m.z.z
	);
}

VMATH_INLINE M3 to_M3(V3 axis, float angle)
{
	axis = norm(axis);
	float c = cosf(angle), s = sinf(angle), t = 1.0f - c;
	float x = axis.x, y = axis.y, z = axis.z;

	return M3(
		V3(t * x * x + c,     t * x * y - s * z, t * x * z + s * y),
		V3(t * x * y + s * z, t * y * y + c,     t * y * z - s * x),
		V3(t * x * z - s * y, t * y * z + s * x, t * z * z + c)
	);
}

VMATH_INLINE void orthonormal_basis(V3 n, V3* out_tangent, V3* out_bitangent)
{
	V3 t = (fabsf(n.x) < 0.99f) ? V3(1, 0, 0) : V3(0, 1, 0);
	*out_tangent = norm(cross(t, n));
	*out_bitangent = cross(n, *out_tangent);
}

VMATH_INLINE V3 solve33(M3 A, V3 b)
{
	float det = dot(cross(A.x, A.y), A.z);
	float inv_det = 1.0f / det;

	V3 x;
	x.x = dot(cross(A.y, A.z), b) * inv_det;
	x.y = dot(cross(A.z, A.x), b) * inv_det;
	x.z = dot(cross(A.x, A.y), b) * inv_det;
	return x;
}

struct Q4
{
	constexpr Q4() { *this = { 0, 0, 0, 1 }; }
	constexpr Q4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}

	float x, y, z, w;
};

VMATH_INLINE Q4 operator+(Q4 a, Q4 b) { return Q4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); }
VMATH_INLINE Q4 operator-(Q4 a, Q4 b) { return Q4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); }
VMATH_INLINE Q4 operator*(Q4 a, float s) { return Q4(a.x * s, a.y * s, a.z * s, a.w * s); }
VMATH_INLINE Q4 operator/(Q4 a, float s) { float r = 1.0f / s; return a * r; }

VMATH_INLINE float dot(Q4 a, Q4 b) { return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; }
VMATH_INLINE float length_sq(Q4 q) { return dot(q, q); }
VMATH_INLINE float length(Q4 q) { return sqrtf(length_sq(q)); }
VMATH_INLINE Q4 norm(Q4 q) { return q / length(q); }
VMATH_INLINE Q4 conjugate(Q4 q) { return Q4(-q.x, -q.y, -q.z, q.w); }
VMATH_INLINE Q4 inverse(Q4 q) { return conjugate(q) / length_sq(q); }

VMATH_INLINE Q4 operator*(Q4 a, Q4 b)
{
	return Q4(
		a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
		a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
		a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
		a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
	);
}

VMATH_INLINE V3 operator*(Q4 q, V3 v)
{
	V3 u(q.x, q.y, q.z);
	float s = q.w;
	return u * (2.0f * dot(u, v)) + v * (s * s - dot(u, u)) + cross(u, v) * (2.0f * s);
}

VMATH_INLINE V3 mul_T(Q4 q, V3 v) { return conjugate(q) * v; }

VMATH_INLINE Q4 to_Q4(V3 axis, float angle)
{
	axis = norm(axis);
	float half = 0.5f * angle;
	float s = sinf(half);
	return Q4(axis.x * s, axis.y * s, axis.z * s, cosf(half));
}

VMATH_INLINE Q4 to_Q4(M3 m)
{
	float trace = m.x.x + m.y.y + m.z.z;
	if (trace > 0.0f) {
		float s = sqrtf(trace + 1.0f) * 2.0f;
		return Q4(
			(m.y.z - m.z.y) / s,
			(m.z.x - m.x.z) / s,
			(m.x.y - m.y.x) / s,
			0.25f * s
		);
	} else if (m.x.x > m.y.y && m.x.x > m.z.z) {
		float s = sqrtf(1.0f + m.x.x - m.y.y - m.z.z) * 2.0f;
		return Q4(
			0.25f * s,
			(m.x.y + m.y.x) / s,
			(m.z.x + m.x.z) / s,
			(m.y.z - m.z.y) / s
		);
	} else if (m.y.y > m.z.z) {
		float s = sqrtf(1.0f + m.y.y - m.x.x - m.z.z) * 2.0f;
		return Q4(
			(m.x.y + m.y.x) / s,
			0.25f * s,
			(m.y.z + m.z.y) / s,
			(m.z.x - m.x.z) / s
		);
	} else {
		float s = sqrtf(1.0f + m.z.z - m.x.x - m.y.y) * 2.0f;
		return Q4(
			(m.z.x + m.x.z) / s,
			(m.y.z + m.z.y) / s,
			0.25f * s,
			(m.x.y - m.y.x) / s
		);
	}
}

VMATH_INLINE M3 to_M3(Q4 q)
{
	float x = q.x, y = q.y, z = q.z, w = q.w;
	float x2 = x + x, y2 = y + y, z2 = z + z;

	float xx = x * x2, yy = y * y2, zz = z * z2;
	float xy = x * y2, xz = x * z2, yz = y * z2;
	float wx = w * x2, wy = w * y2, wz = w * z2;

	return M3(
		V3(1.0f - (yy + zz), xy - wz,          xz + wy),
		V3(xy + wz,          1.0f - (xx + zz), yz - wx),
		V3(xz - wy,          yz + wx,          1.0f - (xx + yy))
	);
}

VMATH_INLINE Q4 slerp(Q4 a, Q4 b, float t)
{
	float d = dot(a, b);
	if (d < 0.0f) { b = b * -1.0f; d = -d; }
	if (d > 0.9995f) return norm(a * (1.0f - t) + b * t);
	float theta = acosf(d);
	float sin_theta = sinf(theta);
	return norm((a * sinf((1.0f - t) * theta) + b * sinf(t * theta)) / sin_theta);
}

struct Transform
{
	constexpr Transform() { *this = { { 0, 0, 0 }, { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } } }; }
	constexpr Transform(V3 p, M3 r) : p(p), r(r) {}

	V3 p;
	M3 r;
};

VMATH_INLINE V3 mul(Transform t, V3 v) { return t.r * v + t.p; }
VMATH_INLINE Transform mul(Transform a, Transform b) { return Transform(a.r * b.p + a.p, a.r * b.r); }
VMATH_INLINE V3 mul_T(Transform t, V3 v) { return mul_T(t.r, v - t.p); }
VMATH_INLINE Transform mul_T(Transform a, Transform b) { return Transform(mul_T(a.r, b.p - a.p), M3(mul_T(a.r, b.r.x), mul_T(a.r, b.r.y), mul_T(a.r, b.r.z))); }

VMATH_INLINE Transform invert(Transform t)
{
	M3 rt = transpose(t.r);
	return Transform(
		rt * (V3(-t.p.x, -t.p.y, -t.p.z)),
		rt
	);
}

struct M4
{
	constexpr M4() { *this = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; }
	constexpr M4(std::initializer_list<float> list)
	{
		auto it = list.begin();
		m11 = *it++;
		m12 = *it++;
		m13 = *it++;
		m14 = *it++;

		m21 = *it++;
		m22 = *it++;
		m23 = *it++;
		m24 = *it++;

		m31 = *it++;
		m32 = *it++;
		m33 = *it++;
		m34 = *it++;

		m41 = *it++;
		m42 = *it++;
		m43 = *it++;
		m44 = *it++;
	}

	float m11, m12, m13, m14;
	float m21, m22, m23, m24;
	float m31, m32, m33, m34;
	float m41, m42, m43, m44;
};

VMATH_INLINE M4 operator*(M4 a, M4 b)
{
	M4 r;

	r.m11 = a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31 + a.m14 * b.m41;
	r.m12 = a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32 + a.m14 * b.m42;
	r.m13 = a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33 + a.m14 * b.m43;
	r.m14 = a.m11 * b.m14 + a.m12 * b.m24 + a.m13 * b.m34 + a.m14 * b.m44;

	r.m21 = a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31 + a.m24 * b.m41;
	r.m22 = a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32 + a.m24 * b.m42;
	r.m23 = a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33 + a.m24 * b.m43;
	r.m24 = a.m21 * b.m14 + a.m22 * b.m24 + a.m23 * b.m34 + a.m24 * b.m44;

	r.m31 = a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31 + a.m34 * b.m41;
	r.m32 = a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32 + a.m34 * b.m42;
	r.m33 = a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33 + a.m34 * b.m43;
	r.m34 = a.m31 * b.m14 + a.m32 * b.m24 + a.m33 * b.m34 + a.m34 * b.m44;

	r.m41 = a.m41 * b.m11 + a.m42 * b.m21 + a.m43 * b.m31 + a.m44 * b.m41;
	r.m42 = a.m41 * b.m12 + a.m42 * b.m22 + a.m43 * b.m32 + a.m44 * b.m42;
	r.m43 = a.m41 * b.m13 + a.m42 * b.m23 + a.m43 * b.m33 + a.m44 * b.m43;
	r.m44 = a.m41 * b.m14 + a.m42 * b.m24 + a.m43 * b.m34 + a.m44 * b.m44;

	return r;
}

VMATH_INLINE V3 mul_point(M4 m, V3 v)
{
	float x = v.x, y = v.y, z = v.z;
	return V3(
		m.m11 * x + m.m21 * y + m.m31 * z + m.m41,
		m.m12 * x + m.m22 * y + m.m32 * z + m.m42,
		m.m13 * x + m.m23 * y + m.m33 * z + m.m43
	);
}

VMATH_INLINE V3 mul_direction(M4 m, V3 v)
{
	float x = v.x, y = v.y, z = v.z;
	return V3(
		m.m11 * x + m.m21 * y + m.m31 * z,
		m.m12 * x + m.m22 * y + m.m32 * z,
		m.m13 * x + m.m23 * y + m.m33 * z
	);
}

VMATH_INLINE M4 to_M4(Transform t)
{
	M3 r = t.r;
	V3 p = t.p;
	return M4{
		r.x.x, r.x.y, r.x.z, 0,
		r.y.x, r.y.y, r.y.z, 0,
		r.z.x, r.z.y, r.z.z, 0,
		p.x,   p.y,   p.z,   1
	};
}

VMATH_INLINE M4 look_at(V3 camera_pos, V3 camera_target, V3 up)
{
	V3 z = norm(camera_pos - camera_target); // Forward (Z+).
	V3 x = norm(cross(up, z));               // Right (X+).
	V3 y = cross(z, x);                      // Up (Y+).

	return M4({
		x.x, y.x, z.x, 0,
		x.y, y.y, z.y, 0,
		x.z, y.z, z.z, 0,
		-dot(x, camera_pos), -dot(y, camera_pos), -dot(z, camera_pos), 1
	});
}

VMATH_INLINE M4 m4_translate(V3 pos)
{
	return M4{
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		pos.x, pos.y, pos.z, 1
	};
}

VMATH_INLINE M4 orthographic(float left, float right, float bottom, float top, float z_near, float z_far)
{
	return M4({
		2.0f / (right - left), 0, 0, 0,
		0, 2.0f / (top - bottom), 0, 0,
		0, 0, 1.0f / (z_near - z_far), 0,
		(left + right) / (left - right),
		(top + bottom) / (bottom - top),
		z_near / (z_near - z_far),
		1
	});
}

VMATH_INLINE M4 perspective(float fov, float aspect, float z_near, float z_far)
{
	float f = 1.0f / tanf(fov * 0.5f);
	return M4({
		f / aspect, 0, 0, 0,
		0, f, 0, 0,
		0, 0, z_far / (z_near - z_far), -1.0f,
		0, 0, (z_near * z_far) / (z_near - z_far), 0
	});
}

VMATH_INLINE M4 inverse(M4 m)
{
	float a00 = m.m11, a01 = m.m12, a02 = m.m13, a03 = m.m14;
	float a10 = m.m21, a11 = m.m22, a12 = m.m23, a13 = m.m24;
	float a20 = m.m31, a21 = m.m32, a22 = m.m33, a23 = m.m34;
	float a30 = m.m41, a31 = m.m42, a32 = m.m43, a33 = m.m44;

	float b00 = a00 * a11 - a01 * a10;
	float b01 = a00 * a12 - a02 * a10;
	float b02 = a00 * a13 - a03 * a10;
	float b03 = a01 * a12 - a02 * a11;
	float b04 = a01 * a13 - a03 * a11;
	float b05 = a02 * a13 - a03 * a12;
	float b06 = a20 * a31 - a21 * a30;
	float b07 = a20 * a32 - a22 * a30;
	float b08 = a20 * a33 - a23 * a30;
	float b09 = a21 * a32 - a22 * a31;
	float b10 = a21 * a33 - a23 * a31;
	float b11 = a22 * a33 - a23 * a32;

	float det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
	float inv_det = 1.0f / det;

	M4 out;
	out.m11 = ( a11 * b11 - a12 * b10 + a13 * b09) * inv_det;
	out.m12 = (-a01 * b11 + a02 * b10 - a03 * b09) * inv_det;
	out.m13 = ( a31 * b05 - a32 * b04 + a33 * b03) * inv_det;
	out.m14 = (-a21 * b05 + a22 * b04 - a23 * b03) * inv_det;

	out.m21 = (-a10 * b11 + a12 * b08 - a13 * b07) * inv_det;
	out.m22 = ( a00 * b11 - a02 * b08 + a03 * b07) * inv_det;
	out.m23 = (-a30 * b05 + a32 * b02 - a33 * b01) * inv_det;
	out.m24 = ( a20 * b05 - a22 * b02 + a23 * b01) * inv_det;

	out.m31 = ( a10 * b10 - a11 * b08 + a13 * b06) * inv_det;
	out.m32 = (-a00 * b10 + a01 * b08 - a03 * b06) * inv_det;
	out.m33 = ( a30 * b04 - a31 * b02 + a33 * b00) * inv_det;
	out.m34 = (-a20 * b04 + a21 * b02 - a23 * b00) * inv_det;

	out.m41 = (-a10 * b09 + a11 * b07 - a12 * b06) * inv_det;
	out.m42 = ( a00 * b09 - a01 * b07 + a02 * b06) * inv_det;
	out.m43 = (-a30 * b03 + a31 * b01 - a32 * b00) * inv_det;
	out.m44 = ( a20 * b03 - a21 * b01 + a22 * b00) * inv_det;

	return out;
}

struct Sphere
{
	Sphere() {}
	Sphere(V3 p, float r) : p(p), r(r) {}

	V3 p;
	float r;
};

struct Capsule
{
	Capsule() {}
	Capsule(V3 a, V3 b, float r) : a(a), b(b), r(r) {}

	V3 a;
	V3 b;
	float r;
};

struct Box
{
	Box() {}
	Box(V3 he, V3 p) : he(he), p(p) {}

	V3 he; // half-extents
	V3 p;  // center
};

struct Plane
{
	Plane() {}
	Plane(V3 n, float d) : n(n), d(d) {}
	Plane(V3 a, V3 b, V3 c) : n(norm(cross(b - a, c - a))), d(-dot(n, a)) { }

	V3 n;
	float d;
};

VMATH_INLINE V3 project(V3 pt, Plane plane) { return pt - plane.n * (dot(pt, plane.n) + plane.d); }
VMATH_INLINE float distance(V3 pt, Plane plane) { return dot(pt, plane.n) + plane.d; }
VMATH_INLINE float distance(Plane plane, V3 pt) { return dot(pt, plane.n) + plane.d; }

VMATH_INLINE Plane newell_plane(const V3* verts, int count)
{
	V3 n = V3(0, 0, 0);
	V3 p = V3(0, 0, 0);

	for (int i = 0; i < count; ++i) {
		const V3& a = verts[i];
		const V3& b = verts[(i + 1) % count];
		n.x += (a.y - b.y) * (a.z + b.z);
		n.y += (a.z - b.z) * (a.x + b.x);
		n.z += (a.x - b.x) * (a.y + b.y);
		p += a;
	}

	p = p * (1.0f / count);
	V3 normal = norm(n);
	float d = -dot(normal, p);
	Plane result = { normal, d };

	return result;
}

VMATH_INLINE Plane mul(Transform t, Plane p) { V3 n = t.r * p.n; float d = dot(n, t.p) + p.d; return Plane(n, d); }
VMATH_INLINE Plane mul_T(Transform t, Plane p) { V3 n = mul_T(t.r, p.n); float d = p.d - dot(n, t.p); return Plane(n, d); }
VMATH_INLINE Sphere mul(Transform t, Sphere s) { return Sphere(mul(t, s.p), s.r); }
VMATH_INLINE Sphere mul_T(Transform t, Sphere s) { return Sphere(mul_T(t, s.p), s.r); }
VMATH_INLINE Capsule mul(Transform t, Capsule c) { return Capsule(mul(t, c.a), mul(t, c.b), c.r); }
VMATH_INLINE Capsule mul_T(Transform t, Capsule c) { return Capsule(mul_T(t, c.a), mul_T(t, c.b), c.r); }
VMATH_INLINE Box mul(Transform t, Box b) { return Box(b.he, mul(t, b.p)); }
VMATH_INLINE Box mul_T(Transform t, Box b) { return Box(b.he, mul_T(t, b.p)); }

struct Ray
{
	Ray() {}
	Ray(V3 o, V3 d) : o(o), d(d) {}

	V3 o; // origin
	V3 d; // direction (should be normalized)
};

struct RayHit
{
	RayHit() {}
	RayHit(float t, V3 n) : t(t), n(n) {}

	float t; // hit distance along the ray
	V3 n;    // surface normal at hit point
};

VMATH_INLINE V3 point_on_ray(Ray ray, float t) { return ray.o + ray.d * t; }
VMATH_INLINE Ray mul(Transform t, Ray r) { return Ray(mul(t, r.o), t.r * r.d); }
VMATH_INLINE Ray mul_T(Transform t, Ray r) { return Ray(mul_T(t, r.o), mul_T(t.r, r.d)); }

VMATH_INLINE bool raycast(Ray r, Sphere s, RayHit* out)
{
	V3 oc = r.o - s.p;
	float b = dot(oc, r.d);
	float c = dot(oc, oc) - s.r * s.r;
	float h = b * b - c;
	if (h < 0.0f) return false;
	h = sqrtf(h);
	float t = -b - h;
	if (t < 0.0f) t = -b + h;
	if (t < 0.0f) return false;
	if (out) *out = RayHit(t, norm(r.o + r.d * t - s.p));
	return true;
}

VMATH_INLINE bool raycast(Ray r, Capsule capsule, RayHit* out)
{
	V3 ba = capsule.b - capsule.a;
	V3 oa = r.o - capsule.a;
	float baba = dot(ba, ba);
	float bard = dot(ba, r.d);
	float baoa = dot(ba, oa);
	float rdoa = dot(r.d, oa);
	float oaoa = dot(oa, oa);

	float A = baba - bard * bard;
	float B = baba * rdoa - baoa * bard;
	float C = baba * oaoa - baoa * baoa - capsule.r * capsule.r * baba;

	float discriminant = B * B - A * C;
	if (discriminant < 0.0f) return false;
	float sqrt_discriminant = sqrtf(discriminant);

	float t = (-B - sqrt_discriminant) / A;
	if (t < 0.0f) t = (-B + sqrt_discriminant) / A;
	if (t < 0.0f) return false;

	float y = baoa + t * bard;
	if (y >= 0.0f && y <= baba) {
		if (out) {
			V3 hit = r.o + r.d * t;
			V3 pa = capsule.a + ba * (y / baba);
			*out = RayHit(t, norm(hit - pa));
		}
		return true;
	}

	// Hit may be in spherical caps.
	RayHit hit_a, hit_b;
	bool hit0 = raycast(r, Sphere(capsule.a, capsule.r), &hit_a);
	bool hit1 = raycast(r, Sphere(capsule.b, capsule.r), &hit_b);

	if (hit0 && (!hit1 || hit_a.t < hit_b.t)) { if (out) *out = hit_a; return true; }
	if (hit1) { if (out) *out = hit_b; return true; }

	return false;
}

// TODO - Remove this.
#define SYML_EPSILON 1e-6f

VMATH_INLINE bool raycast(Ray r, Box b, RayHit* out)
{
	V3 p = b.p - r.o;
	V3 f = V3(1.0f / (r.d.x + SYML_EPSILON), 1.0f / (r.d.y + SYML_EPSILON), 1.0f / (r.d.z + SYML_EPSILON));
	V3 i = p + b.he;
	V3 o = p - b.he;
	V3 t0 = o * f;
	V3 t1 = i * f;
	V3 tmin = min(t0, t1);
	V3 tmax = max(t0, t1);
	float tnear = hmax(tmin);
	float tfar = hmin(tmax);
	if (tnear > tfar || tfar < 0.0f) return false;
	float t = (tnear >= 0.0f) ? tnear : tfar;
	if (out) {
		V3 hit = r.o + r.d * t;
		V3 local = hit - b.p;
		V3 n = V3(0, 0, 0);
		if (fabsf(local.x) > fabsf(local.y) && fabsf(local.x) > fabsf(local.z)) n.x = (local.x < 0.0f) ? -1.0f : 1.0f;
		else if (fabsf(local.y) > fabsf(local.z)) n.y = (local.y < 0.0f) ? -1.0f : 1.0f;
		else n.z = (local.z < 0.0f) ? -1.0f : 1.0f;
		*out = RayHit(t, n);
	}
	return true;
}

VMATH_INLINE bool raycast(Ray r, Plane p, RayHit* out)
{
	float denom = dot(r.d, p.n);
	if (fabsf(denom) < SYML_EPSILON) return false;
	float t = -(dot(r.o, p.n) + p.d) / denom;
	if (t < 0.0f) return false;
	if (out) *out = RayHit(t, p.n * ((denom < 0.0f) ? 1.0f : -1.0f));
	return true;
}

struct AABB
{
	V3 min;
	V3 max;

	VMATH_INLINE AABB() {}
	VMATH_INLINE AABB(V3 min, V3 max) : min(min), max(max) {}
};

VMATH_INLINE AABB to_aabb(Sphere s) { return AABB(s.p - V3(s.r, s.r, s.r), s.p + V3(s.r, s.r, s.r)); }
VMATH_INLINE AABB to_aabb(Box b) { return AABB(b.p - b.he, b.p + b.he); }
VMATH_INLINE AABB combine(AABB a, AABB b) { return AABB(min(a.min, b.min), max(a.max, b.max)); }
VMATH_INLINE bool overlaps(AABB a, AABB b) { return hmax(max(a.min - b.max, b.min - a.max)) <= 0.0f; }
VMATH_INLINE bool contains(AABB a, V3 pt) { return hmax(max(pt - a.max, a.min - pt)) <= 0.0f; }
VMATH_INLINE AABB expand(AABB a, float r) { return AABB(a.min - V3(r, r, r), a.max + V3(r, r, r)); }
VMATH_INLINE AABB expand(AABB a, V3 margin) { return AABB(a.min - margin, a.max + margin); }
VMATH_INLINE V3 center(AABB a) { return (a.min + a.max) * 0.5f; }
VMATH_INLINE V3 half_extents(AABB a) { return (a.max - a.min) * 0.5f; }

VMATH_INLINE bool ray_vs_aabb(Ray r, AABB a, float* tmin_out = 0)
{
	V3 t1 = (a.min - r.o) / r.d;
	V3 t2 = (a.max - r.o) / r.d;
	V3 tmin = min(t1, t2);
	V3 tmax = max(t1, t2);
	float t_near = hmax(tmin);
	float t_far = hmin(tmax);
	if (t_near > t_far || t_far < 0.0f) return false;
	if (tmin_out) *tmin_out = t_near;
	return true;
}

struct Rnd
{
	uint64_t state[2];
};

uint64_t murmur3_avalanche64(uint64_t h)
{
	h ^= h >> 33;
	h *= 0xff51afd7ed558ccd;
	h ^= h >> 33;
	h *= 0xc4ceb9fe1a85ec53;
	h ^= h >> 33;
	return h;
}

Rnd rnd_seed(uint64_t seed)
{
	Rnd rnd;
	uint64_t value = murmur3_avalanche64((seed << 1ULL) | 1ULL);
	rnd.state[0] = value;
	rnd.state[1] = murmur3_avalanche64(value);
	return rnd;
}

uint64_t rnd_uint64(Rnd* rnd)
{
	uint64_t x = rnd->state[0];
	uint64_t y = rnd->state[1];
	rnd->state[0] = y;
	x ^= x << 23;
	x ^= x >> 17;
	x ^= y ^ (y >> 26);
	rnd->state[1] = x;
	return x + y;
}

float rnd_float(Rnd* rnd)
{
	uint32_t value = (uint32_t)(rnd_uint64(rnd) >> 32);

	// Convert a randomized uint32_t value to a float value x in the range 0.0f <= x < 1.0f.
	// Contributed by Jonatan Hedborg.
	uint32_t exponent = 127;
	uint32_t mantissa = value >> 9;
	uint32_t result = (exponent << 23) | mantissa;
	return *(float*)&result - 1.0f;
}

double rnd_double(Rnd* rnd)
{
	uint64_t value = rnd_uint64(rnd);
	uint64_t exponent = 1023;
	uint64_t mantissa = value >> 12;
	uint64_t result = (exponent << 52) | mantissa;
	return *(double*)&result - 1.0;
}

int rnd_range_int(Rnd* rnd, int lo, int hi)
{
	int range = (hi - lo) + 1;
	int value = (int)(rnd_uint64(rnd) % range);
	return lo + value;
}

uint64_t rnd_range_uint64(Rnd* rnd, uint64_t lo, uint64_t hi)
{
	uint64_t range = (hi - lo) + 1;
	uint64_t value = rnd_uint64(rnd) % range;
	return lo + value;
}

float rnd_range_float(Rnd* rnd, float lo, float hi)
{
	float range = hi - lo;
	float value = rnd_float(rnd) * range;
	return lo + value;
}

double rnd_range_double(Rnd* rnd, double lo, double hi)
{
	double range = hi - lo;
	double value = rnd_float(rnd) * range;
	return lo + value;
}

int      rnd_range(Rnd* rnd, int lo, int hi) { return rnd_range_int(rnd, lo, hi); }
uint64_t rnd_range(Rnd* rnd, uint64_t lo, uint64_t hi) { return rnd_range_uint64(rnd, lo, hi); }
float    rnd_range(Rnd* rnd, float lo, float hi) { return rnd_range_float(rnd, lo, hi); }
double   rnd_range(Rnd* rnd, double lo, double hi) { return rnd_range_double(rnd, lo, hi); }

uint64_t rnd_uint64(Rnd& rnd) { return rnd_uint64(&rnd); }
float    rnd_float(Rnd& rnd) { return rnd_float(&rnd); }
double   rnd_double(Rnd& rnd) { return rnd_double(&rnd); }
int      rnd_range(Rnd& rnd, int lo, int hi) { return rnd_range_int(&rnd, lo, hi); }
uint64_t rnd_range(Rnd& rnd, uint64_t lo, uint64_t hi) { return rnd_range_uint64(&rnd, lo, hi); }
float    rnd_range(Rnd& rnd, float lo, float hi) { return rnd_range_float(&rnd, lo, hi); }
double   rnd_range(Rnd& rnd, double lo, double hi) { return rnd_range_double(&rnd, lo, hi); }

V3 rnd_v3(Rnd* rnd) { return V3(rnd_range(rnd, -1.0f, 1.0f), rnd_range(rnd, -1.0f, 1.0f), rnd_range(rnd, -1.0f, 1.0f)); }
V3 rnd_v3(Rnd& rnd) { return V3(rnd_range(rnd, -1.0f, 1.0f), rnd_range(rnd, -1.0f, 1.0f), rnd_range(rnd, -1.0f, 1.0f)); }
V3 rnd_v3_range(Rnd* rnd, float lo, float hi) { return V3(rnd_range(rnd, lo, hi), rnd_range(rnd, lo, hi), rnd_range(rnd, lo, hi)); }
V3 rnd_v3_range(Rnd& rnd, float lo, float hi) { return V3(rnd_range(rnd, lo, hi), rnd_range(rnd, lo, hi), rnd_range(rnd, lo, hi)); }

float perturb(Rnd* rnd, float f, float scale = 10.0f)
{
	assert(scale > 0);
	scale = rnd_range_float(rnd, -scale, scale);
	float delta = scale * FLT_EPSILON * fabsf(f);
	if (f == 0.0f) delta = scale * FLT_EPSILON;
	return f + delta;
}
float perturb(Rnd& rnd, float f, float scale = 10.0f) { return perturb(&rnd, f, scale); }

float fuzz(Rnd* rnd, float f)
{
	int which = rnd_range(rnd, 0, 20);
	switch (which)
	{
	case 0: return NAN;
	case 1: return FLT_MIN;
	case 2: return FLT_MAX;
	case 3: return -FLT_MIN;
	case 4: return -FLT_MAX;
	case 5: return FLT_EPSILON;
	case 6: return -FLT_EPSILON;
	case 7: return INFINITY;
	case 8: return -INFINITY;
	case 9: return 0.0;
	default: return perturb(rnd, f);
	}
}
float fuzz(Rnd& rnd, float f) { return fuzz(&rnd, f); }
V3 fuzz(Rnd* rnd, V3 v) { return V3(fuzz(rnd, v.x), fuzz(rnd, v.y), fuzz(rnd, v.z)); }
V3 fuzz(Rnd& rnd, V3 v) { return V3(fuzz(rnd, v.x), fuzz(rnd, v.y), fuzz(rnd, v.z)); }

VMATH_INLINE void print(V3 v) { printf("V3(%.6f, %.6f, %.6f)\n", v.x, v.y, v.z); }
VMATH_INLINE void print(M3 m) { printf("M3(\n"); print(m.x); print(m.y); print(m.z); printf(")\n"); }
VMATH_INLINE void print(Q4 q) { printf("Q4(%.6f, %.6f, %.6f, %.6f)\n", q.x, q.y, q.z, q.w); }
VMATH_INLINE void print(Transform t) { printf("Transform(\n"); print(t.p); print(t.r); printf(")\n"); }
VMATH_INLINE void print(M4 m)
{
	printf("M4 =\n");
	printf("  [ %8.4f %8.4f %8.4f %8.4f ]\n", m.m11, m.m12, m.m13, m.m14);
	printf("  [ %8.4f %8.4f %8.4f %8.4f ]\n", m.m21, m.m22, m.m23, m.m24);
	printf("  [ %8.4f %8.4f %8.4f %8.4f ]\n", m.m31, m.m32, m.m33, m.m34);
	printf("  [ %8.4f %8.4f %8.4f %8.4f ]\n", m.m41, m.m42, m.m43, m.m44);
}