Common 3D math

vmath.h

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstdint>
#include <initializer_list>
#include <cassert>
#include <cfloat>

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

struct V3
{
	V3() {}
	V3(float x, float y, float z) : x(x), y(y), z(z) {}

	float operator[](int i) { return *((float*)this + i); }

	float x;
	float y;
	float z;
};

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

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

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

float dot(V3 a, V3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
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 }; }

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

struct M3
{
	M3() {}
	M3(V3 x, V3 y, V3 z) : x(x), y(y), z(z) {}

	V3 operator[](int i) { return *((V3*)this + i); }

	V3 x;
	V3 y;
	V3 z;
};

M3 identity() { return M3(V3(1, 0, 0), V3(0, 1, 0), V3(0, 0, 1)); }
V3 operator*(M3 m, V3 v) { return m.x * v.x + m.y * v.y + m.z * v.z; }
M3 operator*(M3 a, M3 b) { return M3(a * b.x, a * b.y, a * b.z); }
M3 operator+(M3 a, M3 b) { return M3(a.x + b.x, a.y + b.y, a.z + b.z); }
M3 operator-(M3 a, M3 b) { return M3(a.x - b.x, a.y - b.y, a.z - b.z); }
M3 operator*(float s, M3 a) { return M3(s * a.x, s * a.y, s * a.z); }
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)); }
float determinant(M3 m) { return dot(m.x, cross(m.y, m.z)); }
M3 skew(V3 v) { return M3(V3(0, -v.z, v.y), V3(v.z, 0, -v.x), V3(-v.y, v.x, 0)); }

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);
}

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
	);
}

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)
	);
}

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);
}

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
{
	Q4() {}
	Q4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {}

	float x, y, z, w;
};

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

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

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
	);
}

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);
}

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

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));
}

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
		);
	}
}

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))
	);
}

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
{
	Transform() {}
	Transform(V3 p, M3 r) : p(p), r(r) {}

	V3 p;
	M3 r;
};

V3 mul(Transform t, V3 v) { return t.r * v + t.p; }
Transform mul(Transform a, Transform b) { return Transform(a.r * b.p + a.p, a.r * b.r); }
V3 mul_T(Transform t, V3 v) { return mul_T(t.r, v - t.p); }
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))); }

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
{
	float m[16];

	M4() {}
	M4(std::initializer_list<float> list)
	{
		int i = 0;
		for (float f : list) m[i++] = f;
	}
};

M4 operator*(M4 a, M4 b)
{
	M4 result;
	for (int row = 0; row < 4; ++row)
		for (int col = 0; col < 4; ++col)
			result.m[row + col * 4] =
			a.m[0 + col * 4] * b.m[row + 0 * 4] +
			a.m[1 + col * 4] * b.m[row + 1 * 4] +
			a.m[2 + col * 4] * b.m[row + 2 * 4] +
			a.m[3 + col * 4] * b.m[row + 3 * 4];
	return result;
}

V3 operator*(M4 m, V3 v)
{
	float x = v.x, y = v.y, z = v.z;
	float rx = m.m[0] * x + m.m[4] * y + m.m[8] * z + m.m[12];
	float ry = m.m[1] * x + m.m[5] * y + m.m[9] * z + m.m[13];
	float rz = m.m[2] * x + m.m[6] * y + m.m[10] * z + m.m[14];
	float rw = m.m[3] * x + m.m[7] * y + m.m[11] * z + m.m[15];
	float rcp_w = 1.0f / rw;
	return V3(rx * rcp_w, ry * rcp_w, rz * rcp_w);
}

M4 perspective(float fov_radians, float aspect, float z_near, float z_far)
{
	float f = 1.0f / tanf(fov_radians * 0.5f);
	float rcp = 1.0f / (z_near - z_far);

	return M4({
		f / aspect, 0, 0, 0,
		0, f, 0, 0,
		0, 0, (z_far + z_near) * rcp, -1,
		0, 0, (2.0f * z_far * z_near) * rcp, 0
		});
}

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
	};
}

M4 look_at(V3 eye, V3 target, V3 up)
{
	V3 f = norm(target - eye);
	V3 r = norm(cross(f, up));
	V3 u = cross(r, f);

	return M4({
		r.x, u.x, -f.x, 0,
		r.y, u.y, -f.y, 0,
		r.z, u.z, -f.z, 0,
		-dot(r, eye), -dot(u, eye), dot(f, eye), 1
		});
}

M4 inverse(M4 m)
{
	float a00 = m.m[0], a01 = m.m[4], a02 = m.m[8], a03 = m.m[12];
	float a10 = m.m[1], a11 = m.m[5], a12 = m.m[9], a13 = m.m[13];
	float a20 = m.m[2], a21 = m.m[6], a22 = m.m[10], a23 = m.m[14];
	float a30 = m.m[3], a31 = m.m[7], a32 = m.m[11], a33 = m.m[15];

	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.m[0] = (a11 * b11 - a12 * b10 + a13 * b09) * inv_det;
	out.m[1] = (-a10 * b11 + a12 * b08 - a13 * b07) * inv_det;
	out.m[2] = (a10 * b10 - a11 * b08 + a13 * b06) * inv_det;
	out.m[3] = (-a10 * b09 + a11 * b07 - a12 * b06) * inv_det;
	out.m[4] = (-a01 * b11 + a02 * b10 - a03 * b09) * inv_det;
	out.m[5] = (a00 * b11 - a02 * b08 + a03 * b07) * inv_det;
	out.m[6] = (-a00 * b10 + a01 * b08 - a03 * b06) * inv_det;
	out.m[7] = (a00 * b09 - a01 * b07 + a02 * b06) * inv_det;
	out.m[8] = (a31 * b05 - a32 * b04 + a33 * b03) * inv_det;
	out.m[9] = (-a30 * b05 + a32 * b02 - a33 * b01) * inv_det;
	out.m[10] = (a30 * b04 - a31 * b02 + a33 * b00) * inv_det;
	out.m[11] = (-a30 * b03 + a31 * b01 - a32 * b00) * inv_det;
	out.m[12] = (-a21 * b05 + a22 * b04 - a23 * b03) * inv_det;
	out.m[13] = (a20 * b05 - a22 * b02 + a23 * b01) * inv_det;
	out.m[14] = (-a20 * b04 + a21 * b02 - a23 * b00) * inv_det;
	out.m[15] = (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) {}

	V3 n;
	float d;
};

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

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

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

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;
}

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;
}

bool raycast(Ray r, Box b, RayHit* out)
{
	const float k_eps = 1e-6f;
	V3 p = b.p - r.o;
	V3 f = V3(1.0f / (r.d.x + k_eps), 1.0f / (r.d.y + k_eps), 1.0f / (r.d.z + k_eps));
	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;
}

bool raycast(Ray r, Plane p, RayHit* out)
{
	const float k_eps = 1e-6f;
	float denom = dot(r.d, p.n);
	if (fabsf(denom) < 1e-6f) 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;

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

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

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)); }

void print(V3 v) { printf("V3(%.6f, %.6f, %.6f)\n", v.x, v.y, v.z); }
void print(M3 m) { printf("M3(\n"); print(m.x); print(m.y); print(m.z); printf(")\n"); }
void print(Q4 q) { printf("Q4(%.6f, %.6f, %.6f, %.6f)\n", q.x, q.y, q.z, q.w); }
void print(Transform t) { printf("Transform(\n"); print(t.p); print(t.r); printf(")\n"); }