tinymath2d.h

/*
	tinymath2d.h - v1.00

	SUMMARY:
	2d vector algebra implementation in C++. Makes use of operator
	overloading and function overloading, so not quite compatible with
	pure C.

	Here's a recommended alternative for pure C in 2d/3d:
	https://github.com/ferreiradaselva/mathc

	Note:
	No SIMD support. Many 2d applications run quite fast in scalar
	operations without the need for any vectorization. Adding SIMD support
	to tinymath2d would increase implementation difficulty and bloat the
	header quite a bit. As such the initial release went with pure scalar.

	Note:
	This header is basically a C++ port of the math from tinyc2.h:
	https://github.com/RandyGaul/tinyheaders/blob/master/tinyc2.h

	Revision history:
		1.00 (11/02/2017) initial release
*/

#if !defined(TINYMATH2D_H)

// 2d vector
struct v2
{
	v2() {}
	v2(float x, float y) : x(x), y(y) {}
	float x;
	float y;
};

// 2d rotation composed of cos/sin pair
struct rotation
{
	float s;
	float c;
};

// 2d matrix
struct m2
{
	v2 x;
	v2 y;
};

// 2d affine transformation
struct transform
{
	rotation r;
	v2 p; // translation, or position
};

// 2d plane, aka line
struct halfspace
{
	v2 n;    // normal
	float d; // distance to origin; d = ax + by = dot(n, p)
};

struct ray
{
	v2 p;    // position
	v2 d;    // direction (normalized)
	float t; // distance along d from position p to find endpoint of ray
};

struct raycast
{
	float t; // time of impact
	v2 n;    // normal of surface at impact (unit length)
};

struct circle
{
	float r;
	v2 p;
};

struct aabb
{
	v2 min;
	v2 max;
};

#define TM2D_INLINE inline

#include <cmath>

// scalar ops
TM2D_INLINE float min(float a, float b) { return a < b ? a : b; }
TM2D_INLINE float max(float a, float b) { return b < a ? a : b; }
TM2D_INLINE float clamp(float a, float lo, float hi) { return max(lo, min(a, hi)); }
TM2D_INLINE float sign(float a) { return a < 0 ? -1.0f : 1.0f; }
TM2D_INLINE float intersect(float da, float db) { return da / (da - db); }

// vector ops
TM2D_INLINE v2 operator+(v2 a, v2 b) { return v2(a.x + b.x, a.y + b.y); }
TM2D_INLINE v2 operator-(v2 a, v2 b) { return v2(a.x - b.x, a.y - b.y); }
TM2D_INLINE v2& operator+=(v2& a, v2 b) { return a = a + b; }
TM2D_INLINE v2& operator-=(v2& a, v2 b) { return a = a - b; }
TM2D_INLINE float dot(v2 a, v2 b) { return a.x * b.x + a.y * b.y; }
TM2D_INLINE v2 operator*(v2 a, float b) { return v2(a.x * b, a.y * b); }
TM2D_INLINE v2 operator*(v2 a, v2 b) { return v2(a.x * b.x, a.y * b.y); }
TM2D_INLINE v2& operator*=(v2& a, float b) { return a = a * b; }
TM2D_INLINE v2& operator*=(v2& a, v2 b) { return a = a * b; }
TM2D_INLINE v2 operator/(v2 a, float b) { return v2(a.x / b, a.y / b); }
TM2D_INLINE v2& operator/=(v2& a, float b) { return a = a / b; }
TM2D_INLINE v2 skew(v2 a) { return v2(-a.y, a.x); }
TM2D_INLINE v2 ccw90(v2 a) { return v2(a.y, -a.x); }
TM2D_INLINE float det2(v2 a, v2 b) { return a.x * b.y - a.y * b.x; }
TM2D_INLINE v2 min(v2 a, v2 b) { return v2(min(a.x, b.x), min(a.y, b.y)); }
TM2D_INLINE v2 max(v2 a, v2 b) { return v2(max(a.x, b.x), max(a.y, b.y)); }
TM2D_INLINE v2 clamp(v2 a, v2 lo, v2 hi) { return max(lo, min(a, hi)); }
TM2D_INLINE v2 abs(v2 a ) { return v2(abs(a.x), abs(a.y)); }
TM2D_INLINE float hmin(v2 a ) { return min(a.x, a.y); }
TM2D_INLINE float hmax(v2 a ) { return max(a.x, a.y); }
TM2D_INLINE float len(v2 a) { return sqrt(dot(a, a)); }
TM2D_INLINE v2 norm(v2 a) { return a / len(a); }
TM2D_INLINE v2 operator-(v2 a) { v2(-a.x, -a.y); }
TM2D_INLINE v2 lerp(v2 a, v2 b, float t) { return a + (b - a) * t; }
TM2D_INLINE int operator<(v2 a, v2 b) { return a.x < b.x && a.y < b.y; }
TM2D_INLINE int operator>(v2 a, v2 b) { return a.x > b.x && a.y > b.y; }
TM2D_INLINE int operator<=(v2 a, v2 b) { return a.x <= b.x && a.y <= b.y; }
TM2D_INLINE int operator>=(v2 a, v2 b) { return a.x >= b.x && a.y >= b.y; }

TM2D_INLINE int parallel(v2 a, v2 b, float tol)
{
	float k = len(a) / len(b);
	b =  b * k;
	if (abs(a.x - b.x) < tol && abs(a.y - b.y) < tol ) return 1;
	return 0;
}

// rotation ops
TM2D_INLINE rotation make_rotation(float radians) { rotation r; r.s = sin(radians); r.c = cos(radians); return r; }
TM2D_INLINE rotation make_rotation() { rotation r; r.c = 1.0f; r.s = 0; return r; }
TM2D_INLINE v2 x_axis(rotation r) { return v2(r.c, r.s); }
TM2D_INLINE v2 y_axis(rotation r) { return v2(-r.s, r.c); }
TM2D_INLINE v2 mul(rotation a, v2 b) { return v2(a.c * b.x - a.s * b.y,  a.s * b.x + a.c * b.y); }
TM2D_INLINE v2 mulT(rotation a, v2 b) { return v2(a.c * b.x + a.s * b.y, -a.s * b.x + a.c * b.y); }
TM2D_INLINE rotation mul(rotation a, rotation b)  { rotation c; c.c = a.c * b.c - a.s * b.s; c.s = a.s * b.c + a.c * b.s; return c; }
TM2D_INLINE rotation mulT(rotation a, rotation b) { rotation c; c.c = a.c * b.c + a.s * b.s; c.s = a.c * b.s - a.s * b.c; return c; }

TM2D_INLINE v2 mul(m2 a, v2 b)  { v2 c; c.x = a.x.x * b.x + a.y.x * b.y; c.y = a.x.y * b.x + a.y.y * b.y; return c; }
TM2D_INLINE v2 mulT(m2 a, v2 b) { v2 c; c.x = a.x.x * b.x + a.x.y * b.y; c.y = a.y.x * b.x + a.y.y * b.y; return c; }
TM2D_INLINE m2 mul(m2 a, m2 b)  { m2 c; c.x = mul(a,  b.x); c.y = mul(a,  b.y); return c; }
TM2D_INLINE m2 mulT(m2 a, m2 b) { m2 c; c.x = mulT(a, b.x); c.y = mulT(a, b.y); return c; }

// transform ops
TM2D_INLINE transform make_transform() { transform x; x.p = v2(0, 0); x.r = make_rotation(); return x; }
TM2D_INLINE transform make_transform(v2 p, float radians) { transform x; x.r = make_rotation(radians); x.p = p; return x; }
TM2D_INLINE v2 mul(transform a, v2 b) { return mul(a.r, b) + a.p; }
TM2D_INLINE v2 mulT(transform a, v2 b) { return mulT(a.r, b - a.p); }
TM2D_INLINE transform mul(transform a, transform b) { transform c; c.r = mul(a.r, b.r); c.p = mul( a.r, b.p ) + a.p; return c; }
TM2D_INLINE transform mulT(transform a, transform b) { transform c; c.r = mulT(a.r, b.r); c.p = mulT(a.r, b.p - a.p); return c; }

// halfspace ops
TM2D_INLINE v2 origin(halfspace h) { return h.n * h.d; }
TM2D_INLINE float distance(halfspace h, v2 p) { return dot(h.n, p) - h.d; }
TM2D_INLINE v2 project(halfspace h, v2 p) { return p - h.n * distance(h, p); }
TM2D_INLINE halfspace mul(transform a, halfspace b) { halfspace c; c.n = mul(a.r, b.n); c.d = dot(mul(a, origin(b) ), c.n); return c; }
TM2D_INLINE halfspace mulT(transform a, halfspace b) { halfspace c; c.n = mulT(a.r, b.n); c.d = dot(mulT(a, origin(b) ), c.n); return c; }
TM2D_INLINE v2 intersect(v2 a, v2 b, float da, float db) { return a + (b - a) * (da / (da - db)); }

// aabb helpers
TM2D_INLINE aabb make_aabb(v2 min, v2 max) { aabb bb; bb.min = min; bb.max = max; return bb; }
TM2D_INLINE aabb make_aabb_center_half_extents(v2 center, v2 half_extents) { aabb bb; bb.min = center - half_extents; bb.max = center + half_extents; return bb; }
TM2D_INLINE float width(aabb bb) { return bb.max.x - bb.min.x; }
TM2D_INLINE float height(aabb bb) { return bb.max.y - bb.min.y; }
TM2D_INLINE float half_width(aabb bb) { return width(bb) * 0.5f; }
TM2D_INLINE float half_height(aabb bb) { return height(bb) * 0.5f; }
TM2D_INLINE v2 half_extents(aabb bb) { return (bb.max - bb.min) * 0.5f; };
TM2D_INLINE v2 min(aabb bb) { return bb.min; }
TM2D_INLINE v2 max(aabb bb) { return bb.max; }
TM2D_INLINE v2 midpoint(aabb bb) { return (bb.min + bb.max) * 0.5f; }
TM2D_INLINE v2 top_left(aabb bb) { return v2(bb.min.x, bb.max.y); }
TM2D_INLINE v2 top_right(aabb bb) { return v2(bb.max.x, bb.max.y); }
TM2D_INLINE v2 bottom_left(aabb bb) { return v2(bb.min.x, bb.min.y); }
TM2D_INLINE v2 bottom_right(aabb bb) { return v2(bb.max.x, bb.min.y); }
TM2D_INLINE int contains(aabb bb, v2 p) { p >= bb.min && p <= bb.max; }
TM2D_INLINE int contains(aabb a, aabb b) { a.min >= b.min && a.max <= b.max; }
TM2D_INLINE float surface_area(aabb bb) { return 2.0f * width(bb) * height(bb); }
TM2D_INLINE float area(aabb bb) { return width(bb) * height(bb); }
TM2D_INLINE v2 clamp(aabb bb, v2 p) { return clamp(p, bb.min, bb.max); }
TM2D_INLINE aabb clamp(aabb a, aabb b) { return make_aabb(clamp(a.min, b.min, b.max), clamp(a.max, b.min, b.max)); }

TM2D_INLINE int overlaps(aabb a, aabb b)
{
	int d0 = b.max.x < a.min.x;
	int d1 = a.max.x < b.min.x;
	int d2 = b.max.y < a.min.y;
	int d3 = a.max.y < b.min.y;
	return !(d0 | d1 | d2 | d3);
}

TM2D_INLINE aabb make_aabb(v2* verts, int count)
{
	v2 min = verts[0];
	v2 max = min;
	for (int i = 0; i < count; ++i)
	{
		min = ::min(min, verts[i]);
		max = ::max(max, verts[i]);
	}
	return make_aabb(min, max);
}

TM2D_INLINE void aabb_verts(v2* out, aabb* bb)
{
	out[0] = bb->min;
	out[1] = v2(bb->max.x, bb->min.y);
	out[2] = bb->max;
	out[3] = v2(bb->min.x, bb->max.y);
}

// circle helpers
TM2D_INLINE float area(circle c) { return 3.14159265f * c.r * c.r; }
TM2D_INLINE float surface_area(circle c) { return 2.0f * 3.14159265f * c.r; }
TM2D_INLINE circle mul(transform tx, circle a) { circle b; b.p = mul(tx, a.p); b.r = a.r; return b; }

// ray ops
v2 impact(ray r, float t) { return r.p + r.d * t; }

TM2D_INLINE int ray_to_halfpsace(ray A, halfspace B, raycast* out)
{
	float da = distance(B, A.p);
	float db = distance(B, impact(A, A.t));
	if (da * db > 0) return 0;
	out->n = B.n * sign(da);
	out->t = intersect(da, db);
}

TM2D_INLINE int ray_to_circle(ray A, circle B, raycast* out)
{
	v2 p = B.p;
	v2 m = A.p - p;
	float c = dot(m, m) - B.r * B.r;
	float b = dot(m, A.d);
	float disc = b * b - c;
	if (disc < 0) return 0;

	float t = -b - sqrt(disc);
	if (t >= 0 && t <= A.t)
	{
		out->t = t;
		v2 impact = ::impact(A, t);
		out->n = norm(impact - p);
		return 1;
	}
	return 0;
}

TM2D_INLINE int ray_to_aabb(ray A, aabb B, raycast* out)
{
	v2 inv = v2(1.0f / A.d.x, 1.0f / A.d.y);
	v2 d0 = (B.min - A.p) * inv;
	v2 d1 = (B.max - A.p) * inv;
	v2 v0 = min(d0, d1);
	v2 v1 = max(d0, d1);
	float lo = hmax(v0);
	float hi = hmin(v1);

	if (hi >= 0 && hi >= lo && lo <= A.t)
	{
		v2 c = midpoint(B);
		c = impact(A, lo) - c;
		v2 abs_c = abs(c);
		if (abs_c.x > abs_c.y) out->n = v2(sign(c.x), 0);
		else out->n = v2(0, sign(c.y));
		out->t = lo;
		return 1;
	}
	return 0;
}

#define TINYMATH2D_H
#endif

/*
	This is free and unencumbered software released into the public domain.
	Our intent is that anyone is free to copy and use this software,
	for any purpose, in any form, and by any means.
	The authors dedicate any and all copyright interest in the software
	to the public domain, at their own expense for the betterment of mankind.
	The software is provided "as is", without any kind of warranty, including
	any implied warranty. If it breaks, you get to keep both pieces.
*/