collision yes/no

7 files changed, 710 insertions, 14 deletions

diff --git a/c2.cpp b/c2.cpp
index db9239b..73927ad 100644
--- a/c2.cpp
+++ b/c2.cpp
@@ -742,10 +742,14 @@ struct C2 : public App {
 			box_col = make_box(&asset_arena, v3f(1.0f, 1.0f, 1.0f));
 			floor_col = make_box(&asset_arena, v3f(10.0f, 0.1f, 10.0f));
 			box = world->create_entity();
-			auto [trans, rb] = world->add<Transform, Rigidbody>(box);
+			auto [trans, rb, mod] = world->add<Transform, Rigidbody, C_Model>(box);
 			trans.mat = m4f::identity();
 			rb.init(box_col, v3f(0.0f, 5.0f, 0.0f), quat::identity(), 1.0f);
 			rb.add_force(v3f(0.4f, -1.0f, 0.0f), v3f(0.0f, 0.1f, 0.0f));
+			mod.i = scene.instantiate(
+				dev,
+				(Model*)assets.load("cube.mdl")
+			);
 
 			floor = world->create_entity();
 			auto [transf, rbf] = world->add<Transform, Rigidbody>(floor);
@@ -781,7 +785,12 @@ struct C2 : public App {
 
 		if (es.pause_physics)
 			phys_dt = 0.0f;
-		physics_update(*world, &frame_arena, phys_dt);
+		physics_update(
+			*world,
+			&frame_arena,
+			phys_dt,
+			editor_physics_debughook()
+		);
 
 		dev->begin_frame();
 		lr.begin(w, h);
diff --git a/editor.cpp b/editor.cpp
index 9cc4539..32e4498 100644
--- a/editor.cpp
+++ b/editor.cpp
@@ -2,6 +2,7 @@
 #include "debugdraw.hpp"
 #include "editor.hpp"
 #include "model.hpp"
+#include "physics.hpp"
 #include "scene.hpp"
 #include "ui.hpp"
 #include "world.hpp"
@@ -11,13 +12,32 @@ extern "C" {
 #include "plat.h"
 }
 
+#include <vector>
+
+struct Editor_PD : Physics_Debughook {
+	void shrinkwrap(
+		const Int_Collider* col,
+		int axis,
+		const v2f* points,
+		int count
+	) override;
+	void projection(
+		const Int_Collider* col,
+		const v3f& n,
+		int axis,
+		const v2f* points,
+		int count
+	) override;
+};
+
 static struct {
 	Editor_Settings settings;
+	Editor_PD physics_debughook;
 	UI* ui;
 	UI::Toolbar* toolbar;
 	UI::Button* mat_btn, * ent_btn, * phy_btn;
 	UI::Modal* mat_win, * ent_win, * phy_win;
-	UI::Label* mat_name;
+	UI::Label* mat_name, * spl;
 	UI::Slider* metalness_input;
 	UI::Slider* roughness_input;
 	UI::Slider* ao_input;
@@ -28,8 +48,12 @@ static struct {
 	int selected_mesh;
 	int hovered_axis;
 	int dragging_gizmo;
+	int debug_sat_axis;
 	v3f drag_axis, drag_off;
 	v3f gf, gu, gr;
+	std::vector<v2f> shrinkwrap_points;
+	std::vector<v2f> projection_points;
+	v3f projection_start, projection_end;
 } editor;
 
 static int mat_win_handler(UI::Element* e, const UI::Message& m) {
@@ -215,6 +239,29 @@ static int phy_btn_handler(UI::Element* e, const UI::Message& m) {
 			}
 			return 0;
 		};
+		auto csat = ui->create_element<UI::Checkbox>(cont, "Debug SAT");
+		csat->set_val(editor.settings.debug_sat);
+		csat->handler = [](UI::Element* e, const UI::Message& m) {
+			if (m.type == UI::Message::Type::checkbox_changed) {
+				UI::Checkbox* ch = (UI::Checkbox*)e;
+				editor.settings.debug_sat = ch->val;
+			}
+			return 0;
+		};
+		int rows[] = { 150, 50, 0 };
+		auto tab = ui->create_element<UI::Table>(cont, rows);
+		ui->create_element<UI::Label>(tab, "Debug Axis");
+		auto iaxis = ui->create_element<UI::Int_Input>(tab);
+		iaxis->set_val(editor.debug_sat_axis);
+		iaxis->handler = [](UI::Element* e, const UI::Message& m) {
+			if (m.type == UI::Message::Type::input_finalised) {
+				UI::Int_Input* i = (UI::Int_Input*)e;
+				editor.debug_sat_axis = i->val;
+			}
+			return 0;
+		};
+		ui->create_element<UI::Label>(tab, "Shrinkwrap");
+		editor.spl = ui->create_element<UI::Label>(tab, "0");
 	}
 	return 0;
 }
@@ -240,6 +287,7 @@ void init_editor(UI* ui, World* world) {
 	editor.mat_win = 0;
 	editor.ent_win = 0;
 	editor.phy_win = 0;
+	editor.debug_sat_axis = 0;
 	editor.world = world;
 	editor.dragging_gizmo = 0;
 	zero(&editor.settings, sizeof editor.settings);
@@ -249,7 +297,7 @@ void deinit_editor() {
 }
 
 void editor_on_select(Entity_Id e, int m) {
-	if (!editor.ui->hovered) {
+	if (!editor.ui->hovered && !editor.dragging_gizmo) {
 		editor.selected = e;
 		if (e && editor.world->has<C_Model>(e)) {
 			auto& cm = editor.world->get<C_Model>(e);
@@ -300,6 +348,25 @@ void editor_draw(Line_Renderer& lr) {
 				lr.colour(v3f(0.0f, 0.0f, 1.0f));
 			lr.add_arrow(p, p + r);
 		}
+		if (w.has<Rigidbody>(e)) {
+			if (editor.settings.debug_sat) {
+				lr.colour(v3f(1.0f, 1.0f, 1.0f));
+				int i, c = editor.shrinkwrap_points.size();
+				for (i = 0; i < c; i++) {
+					v2f point = editor.shrinkwrap_points[i];
+					v2f prev = editor.shrinkwrap_points[(i + 1) % c];
+					lr.add_line(v3f(prev, 0.0f), v3f(point, 0.0f));
+				}
+				lr.colour(v3f(1.0f, 0.0f, 0.0f));
+				c = editor.projection_points.size();
+				for (i = 0; i < c; i++) {
+					v2f point = editor.projection_points[i];
+					lr.add_point(v3f(point, 0.0f));
+				}
+				lr.colour(v3f(1.0f, 0.5f, 1.0f));
+				lr.add_arrow(editor.projection_start, editor.projection_end);
+			}
+		}
 	}
 }
 
@@ -332,6 +399,10 @@ void editor_update(const App& app, const Camera& cam) {
     m.m[3][0] += (gp.x - m.m[3][0]) * editor.drag_axis.x;
     m.m[3][1] += (gp.y - m.m[3][1]) * editor.drag_axis.y;
     m.m[3][2] += (gp.z - m.m[3][2]) * editor.drag_axis.z;
+    if (w.has<Rigidbody>(e)) {
+			Rigidbody& rb = w.get<Rigidbody>(e);
+			rb.set_pos(v3f(m.m[3][0], m.m[3][1], m.m[3][2]));
+    }
 		if (app.mjr(mbtn_left))
 			editor.dragging_gizmo = 0;
 		return;
@@ -383,3 +454,59 @@ void editor_update(const App& app, const Camera& cam) {
 Editor_Settings& editor_settings() {
 	return editor.settings;
 }
+
+Physics_Debughook* editor_physics_debughook() {
+	return &editor.physics_debughook;
+}
+
+void Editor_PD::shrinkwrap(
+	const Int_Collider* col,
+	int axis,
+	const v2f* points,
+	int count
+) {
+	World& world = *editor.world;
+	Entity_Id s = editor.selected;
+	Rigidbody* rb;
+	if (!editor.settings.debug_sat) return;
+	if (!s) return;
+	if (!world.has<Rigidbody>(s)) return;
+	rb = &world.get<Rigidbody>(s);
+	if (axis == editor.debug_sat_axis && col->rb == rb) {
+		int i;
+		auto& v = editor.shrinkwrap_points;
+		v.clear();
+		editor.spl->set_int(count);
+		for (i = 0; i < count; i++)
+			v.push_back(points[i]);
+	}
+}
+
+void Editor_PD::projection(
+	const Int_Collider* col,
+	const v3f& n,
+	int axis,
+	const v2f* points,
+	int count
+) {
+	World& world = *editor.world;
+	Entity_Id s = editor.selected;
+	Rigidbody* rb;
+	if (!editor.settings.debug_sat) return;
+	if (!s) return;
+	if (!world.has<Rigidbody>(s)) return;
+	rb = &world.get<Rigidbody>(s);
+	if (axis == editor.debug_sat_axis && col->rb == rb) {
+		int i;
+		float w = 1.0f / (float)count;
+		auto& v = editor.projection_points;
+		v.clear();
+		editor.projection_start = v3f(0.0f);
+		for (i = 0; i < count; i++) {
+			const v2f& point = points[i];
+			v.push_back(point);
+			editor.projection_start += v3f(point, 0.0f) * w;
+		}
+		editor.projection_end = editor.projection_start + n;
+	}
+}
diff --git a/editor.hpp b/editor.hpp
index afa785e..ba64a49 100644
--- a/editor.hpp
+++ b/editor.hpp
@@ -3,13 +3,15 @@
 
 #include "world.hpp"
 
-struct UI;
-struct Model_Instance;
 struct Line_Renderer;
+struct Model_Instance;
+struct Physics_Debughook;
+struct UI;
 
 struct Editor_Settings {
 	bool pause_physics;
 	bool debug_physics;
+	bool debug_sat;
 };
 
 void init_editor(UI* ui, World* w);
@@ -19,4 +21,6 @@ void editor_update(const App& app, const Camera& cam);
 void editor_draw(Line_Renderer& lr);
 Editor_Settings& editor_settings();
 
+Physics_Debughook* editor_physics_debughook();
+
 #endif
diff --git a/physics.cpp b/physics.cpp
index 993b587..0c67747 100644
--- a/physics.cpp
+++ b/physics.cpp
@@ -5,9 +5,11 @@
 
 extern "C" {
 #include "memory.h"
+#include "plat.h"
 #include "str.h"
 }
 
+#include <algorithm>
 #include <new>
 
 void Collider::debug_render(
@@ -45,6 +47,8 @@ Int_Collider* Collider::transform(const m4f& t, Arena* a) const {
 	Int_Collider* r = (Int_Collider*)arena_alloc(a, sizeof *r);
 	r->faces = (v3f*)arena_alloc(a, face_count * sizeof *r->faces);
 	r->verts = (v3f*)arena_alloc(a, vert_count * sizeof *r->verts);
+	r->vert_count = vert_count;
+	r->face_count = face_count;
 	for (i = 0; i < face_count; i++)
 		r->faces[i] = (t * v4f(faces[i].normal, 0.0f)).xyz();
 	for (i = 0; i < vert_count; i++)
@@ -207,8 +211,160 @@ void physics_tick(World& w, float ts) {
 	}
 }
 
-bool Int_Collider::collide(const Int_Collider& other) const {
-	return false;
+v2f Int_Collider::Projection::Iter::operator*() {
+	v3f& pos = p->col->verts[vert];
+	return v2f(v3f::dot(p->r, pos), v3f::dot(p->u, pos));
+}
+
+Int_Collider::Projection Int_Collider::project(const v3f& axis) const {
+	v3f r = v3f::perp(axis);
+	v3f u = v3f::cross(r, axis);
+	return Projection { this, r, u };
+}
+
+struct Shrinkwrap {
+	v2f* points;
+	int count;
+	Shrinkwrap(Arena* a, v2f* src, int src_count):
+		count(0) {
+		int i, top = 0;
+		int stack_size = src_count;
+		v2f p0(INFINITY, INFINITY);
+		v2f* stack = (v2f*)arena_alloc(a, stack_size * sizeof *points);
+		points = (v2f*)arena_alloc(a, src_count * sizeof *points);
+		auto push = [&](v2f p) {
+			assert(top < stack_size);
+			stack[top++] = p;
+		};
+		auto pop = [&]() {
+			assert(top);
+			return stack[--top];
+		};
+		auto peek = [&](int i) {
+			assert(top - 1 - i >= 0);
+			return stack[top - 1 - i];
+		};
+		auto ccw = [](v2f a, v2f b, v2f c) {
+    	float angle =
+    		(b.y - a.y) * (c.x - b.x) -
+    		(b.x - a.x) * (c.y - b.y);
+    	return angle > -0.00001f;
+		};
+		for (i = 0; i < src_count; i++) {
+			v2f p = src[i];
+			if (p.y < p0.y || (p.y == p0.y && p.x < p0.x))
+				p0 = p;
+		}
+		std::sort(
+			src,
+			src + src_count,
+			[p0](const v2f& f, const v2f& s) {
+    		float a =
+    			(f.y - p0.y) * (s.x - f.x) -
+    			(f.x - p0.x) * (s.y - f.y);
+    		bool colinear = fabsf(a) < 0.001f;
+    		if (colinear)
+    			return v2f::mag(p0 - s) >= v2f::mag(p0 - f);
+    		return a < 0.0f;
+			}
+		);
+		for (i = 0; i < src_count; i++) {
+			v2f p = src[i];
+			while (top > 1 && ccw(peek(1), peek(0), p))
+				pop();
+			push(p);
+		}
+		while (top)
+			points[count++] = pop();
+	}
+
+	std::pair<v2f*, int> normals(Arena* a) const {
+		int c = (count >> 1) + 1, i;
+		v2f* normals = (v2f*)arena_alloc(a, c * sizeof *normals);
+		for (i = 0; i < count; i++) {
+			v2f a = points[i];
+			v2f b = points[(i + 1) % count];
+			v2f e = a - b;
+			v2f n = v2f::normalised(v2f(e.y, -e.x));
+			normals[i] = n;
+		}
+		return { normals, i };
+	}
+
+	bool SAT(Arena* a, const Shrinkwrap& other) {
+		int i;
+		auto[fn, fnc] = normals(a);
+		auto[sn, snc] = other.normals(a);
+		auto project = [](const Shrinkwrap& shape, v2f axis) {
+			int i;
+			float mini =  INFINITY;
+			float maxi = -INFINITY;
+			for (i = 0; i < shape.count; i++) {
+				const v2f& p = shape.points[i];
+				float d = v2f::dot(p, axis);
+				if (d < mini) mini = d;
+				if (d > maxi) maxi = d;
+			}
+			return std::pair<float, float>{ mini, maxi };
+		};
+		for (i = 0; i < fnc; i++) {
+			v2f axis = fn[i];
+			auto [fp1, fp2] = project(*this, axis);
+			auto [sp1, sp2] = project(other, axis);
+			if (fp2 > sp1 && fp1 > sp2)
+				return true;
+		}
+		for (i = 0; i < snc; i++) {
+			v2f axis = sn[i];
+			auto [fp1, fp2] = project(*this, axis);
+			auto [sp1, sp2] = project(other, axis);
+			if (fp2 > sp1 && fp1 > sp2)
+				return true;
+		}
+		return false;
+	}
+};
+
+bool Int_Collider::collide(
+	Physics_Debughook* hook,
+	Arena* a,
+	const Int_Collider& other
+) const {
+	auto check_axes = [&](
+		const Int_Collider& f,
+		const Int_Collider& s
+	) {
+		int i, c = s.face_count;
+		v2f* fp, * sp;
+		arena_push(a);
+		fp = (v2f*)arena_alloc(a, sizeof *fp * f.vert_count);
+		sp = (v2f*)arena_alloc(a, sizeof *sp * s.vert_count);
+		for (i = 0; i < c; i++) {
+			const v3f& axis = s.faces[i];
+			int fpc = 0, spc = 0;
+			for (v2f p : f.project(axis))
+				fp[fpc++] = p;
+			for (v2f p : s.project(axis))
+				sp[spc++] = p;
+			Shrinkwrap fwf(a, fp, fpc);
+			Shrinkwrap swf(a, sp, spc);
+#ifdef DEBUG
+			if (hook) {
+				hook->projection(&s, axis, i, sp, spc);
+				hook->projection(&f, axis, i, fp, fpc);
+				hook->shrinkwrap(&s, i, swf.points, swf.count);
+				hook->shrinkwrap(&f, i, fwf.points, fwf.count);
+			}
+#endif
+			if (fwf.SAT(a, swf)) return true;
+		}
+		arena_pop(a);
+		return false;
+	};
+	if (check_axes(*this, other)) return false;
+	if (check_axes(other, *this)) return false;
+	(void)hook;
+	return true;
 }
 
 struct Collision_Engine {
@@ -227,12 +383,12 @@ struct Collision_Engine {
 			bodies = b;
 		}
 	}
-	void detect() {
+	void detect(Physics_Debughook* hook) {
 		Int_Collider* a;
 		for (a = bodies; a; a = a->next) {
 			Int_Collider* b;
 			for (b = a->next; b; b = b->next) {
-				if (a->collide(*b)) {
+				if (a->collide(hook, arena, *b)) {
 					a->rb->colliding = 1;
 					b->rb->colliding = 1;
 				}
@@ -241,10 +397,15 @@ struct Collision_Engine {
 	}
 };
 
-void physics_update(World& w, Arena* a, float ts) {
+void physics_update(
+	World& w,
+	Arena* a,
+	float ts,
+	Physics_Debughook* hook
+) {
 	Collision_Engine ce;
 	ce.init(w, a);
-	ce.detect();
+	ce.detect(hook);
 	physics_tick(w, ts);
 	for (auto v : w.view<Transform, Rigidbody>()) {
 		auto& t = v.get<Transform>();
@@ -272,3 +433,30 @@ void physics_debug(World& w, Line_Renderer& r) {
 		r.add_arrow(rb.pos, rb.pos + rb.torque);
 	}
 }
+
+void Physics_Debughook::shrinkwrap(
+	const Int_Collider* col,
+	int axis,
+	const v2f* points,
+	int count
+) {
+	(void)col;
+	(void)axis;
+	(void)points;
+	(void)count;
+}
+
+void Physics_Debughook::projection(
+	const Int_Collider* col,
+	const v3f& n,
+	int axis,
+	const v2f* points,
+	int count
+) {
+	(void)col;
+	(void)n;
+	(void)axis;
+	(void)points;
+	(void)count;
+}
+
diff --git a/physics.hpp b/physics.hpp
index 6e6b259..a800b16 100644
--- a/physics.hpp
+++ b/physics.hpp
@@ -17,6 +17,23 @@ struct Manifold {
 	v3f normal;
 };
 
+struct Int_Collider;
+struct Physics_Debughook {
+	virtual void shrinkwrap(
+		const Int_Collider* col,
+		int axis,
+		const v2f* points,
+		int count
+	);
+	virtual void projection(
+		const Int_Collider* col,
+		const v3f& n,
+		int axis,
+		const v2f* points,
+		int count
+	);
+};
+
 struct Rigidbody;
 struct Int_Collider {
 	Rigidbody* rb;
@@ -26,7 +43,46 @@ struct Int_Collider {
 	v3f* verts;
 	v3f* faces;
 
-	bool collide(const Int_Collider& other) const;
+	bool collide(
+		Physics_Debughook* hook,
+		Arena* a,
+		const Int_Collider& other
+	) const;
+
+	struct Projection {
+		const Int_Collider* col;
+		v3f r, u;
+
+		struct Iter {
+			const Projection* p;
+			int vert;
+
+			bool equals(const Iter& other) const {
+				return vert == other.vert;
+			}
+			bool operator==(const Iter& other) const {
+				return equals(other);
+			}
+			bool operator!=(const Iter& other) const {
+				return !equals(other);
+			}
+			Iter operator++() {
+				vert++;
+				return *this;
+			}
+			v2f operator*();
+		};
+
+		Iter begin() {
+			return { this, 0 };
+		}
+
+		Iter end() {
+			return { this, col->vert_count };
+		}
+	};
+
+	Projection project(const v3f& axis) const;
 };
 
 struct Collider : Asset {
@@ -72,7 +128,12 @@ struct Rigidbody {
 
 Collider* make_box(Arena* a, const v3f& size);
 
-void physics_update(World& w, Arena* a, float ts);
+void physics_update(
+	World& w,
+	Arena* a,
+	float ts,
+	Physics_Debughook* hook
+);
 void physics_debug(World& w, Line_Renderer& lr);
 
 #endif
diff --git a/testhull.cpp b/testhull.cpp
new file mode 100644
index 0000000..d02f17a
--- /dev/null
+++ b/testhull.cpp
@@ -0,0 +1,152 @@
+#include <SDL2/SDL.h>
+
+#include <assert.h>
+#include <stdlib.h>
+
+#include "maths.cpp"
+
+struct Shrinkwrap {
+	v2f* points;
+	int count;
+	v2f p0;
+	Shrinkwrap(v2f* src, int src_count):
+		count(0), p0(INFINITY, INFINITY) {
+		int i, top = 0;
+		int stack_size = src_count;
+		v2f* stack = (v2f*)malloc(stack_size * sizeof *stack);
+		points = (v2f*)malloc(src_count * sizeof *points);
+		auto push = [&](v2f p) {
+			assert(top < stack_size);
+			stack[top++] = p;
+		};
+		auto pop = [&]() {
+			assert(top > 0);
+			return stack[--top];
+		};
+		auto peek = [&](int i) {
+			assert(top - 1 - i >= 0);
+			return stack[top - 1 - i];
+		};
+		auto ccw = [](v2f a, v2f b, v2f c) {
+    	float angle =
+    		(b.y - a.y) * (c.x - b.x) -
+    		(b.x - a.x) * (c.y - b.y);
+    	return angle > -0.00001f;
+		};
+		for (i = 0; i < src_count; i++) {
+			v2f p = src[i];
+			if (p.y < p0.y || (fabsf(p.y - p0.y) < 0.0001f && p.x < p0.x))
+				p0 = p;
+		}
+		std::sort(
+			src,
+			src + src_count,
+			[this](const v2f& f, const v2f& s) {
+    		float a =
+    			(f.y - p0.y) * (s.x - f.x) -
+    			(f.x - p0.x) * (s.y - f.y);
+    		bool colinear = fabsf(a - 0.0f) < 0.001f;
+    		if (colinear)
+    			return v2f::mag(p0 - s) >= v2f::mag(p0 - f);
+    		return a < 0.0f;
+			}
+		);
+		for (i = 0; i < src_count; i++) {
+			v2f p = src[i];
+			while (top > 1 && ccw(peek(1), peek(0), p))
+				pop();
+			push(p);
+		}
+		while (top)
+			points[count++] = pop();
+		free(stack);
+	}
+};
+
+int main(int argc, const char** argv) {
+	int r = 1;
+	float rot = 3.5f;
+	SDL_Window* window;
+	SDL_Renderer* ren;
+	SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO | SDL_INIT_JOYSTICK);
+	SDL_CreateWindowAndRenderer(
+		800,
+		800,
+		0,
+		&window,
+		&ren
+	);
+	v2f points[] = {
+		{ 129.0f, 148.0f },
+		{ 178.0f, 178.0f },
+		{ 146.0f, 146.0f },
+		{ 163.0f, 172.0f },
+		{ 129.0f, 159.0f },
+		{ 102.0f, 151.0f },
+		{ 167.0f, 122.0f }
+	};
+	float rots[] = {
+		0.0f,
+		1.0f,
+		1.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+	};
+	constexpr int point_count = sizeof points / sizeof *points;
+	v2f points2[point_count];
+	while (r) {
+		int i;
+		SDL_Event e;
+		while (SDL_PollEvent(&e)) {
+			switch (e.type) {
+				case SDL_QUIT:
+					r = 0;
+					break;
+			}
+		}
+		for (i = 0; i < point_count; i++) {
+			v2f p = points[i];
+			points2[i] = v2f(
+				p.x * 2 + (sinf(rot * rots[i]) * p.x - cosf(rot * rots[i]) * p.y),
+				p.y * 2 + (sinf(rot * rots[i]) * p.x + cosf(rot * rots[i]) * p.y)
+			);
+		}
+		Shrinkwrap s(points2, point_count);
+		SDL_SetRenderDrawColor(ren, 0x00, 0x00, 0x00, 0x00);
+		SDL_RenderClear(ren);
+		for (int i = 0; i < s.count; i++) {
+			v2f a = s.points[i];
+			v2f b = s.points[(i + 1) % s.count];
+			v2f e = a - b;
+			v2f h = e * 0.5f;
+			v2f n = v2f(e.y, -e.x);
+			n = v2f::normalised(n);
+			SDL_SetRenderDrawColor(ren, 0x00, 0x00, 0xff, 0xff);
+			SDL_RenderDrawLine(ren, (int)a.x, (int)a.y, (int)b.x, (int)b.y);
+			SDL_SetRenderDrawColor(ren, 0xff, 0x00, 0xff, 0xff);
+			SDL_RenderDrawLine(
+				ren,
+				(int)(a.x - h.x),
+				(int)(a.y - h.y),
+				(int)((a.x - h.x) + n.x * 32.0f),
+				(int)((a.y - h.y) + n.y * 32.0f)
+			);
+		}
+		SDL_SetRenderDrawColor(ren, 0xff, 0x00, 0x00, 0xff);
+		for (i = 0; i < point_count; i++) {
+			v2f p = points2[i];
+			SDL_RenderDrawPoint(ren, (int)p.x, (int)p.y);
+		}
+		SDL_SetRenderDrawColor(ren, 0x00, 0xff, 0x00, 0xff);
+		SDL_RenderDrawPoint(ren, (int)s.p0.x, (int)s.p0.y);
+		SDL_RenderPresent(ren);
+		free(s.points);
+		rot += 0.0003f;
+	}
+	SDL_DestroyRenderer(ren);
+	SDL_DestroyWindow(window);
+	SDL_Quit();
+	return 0;
+}
diff --git a/testsat.cpp b/testsat.cpp
new file mode 100644
index 0000000..45f3891
--- /dev/null
+++ b/testsat.cpp
@@ -0,0 +1,155 @@
+#include <SDL2/SDL.h>
+
+#include <assert.h>
+#include <stdlib.h>
+
+#include "maths.cpp"
+
+struct Shape {
+	v2f* points;
+	int count;
+
+	Shape(std::initializer_list<v2f> in) {
+		int i;
+		points = (v2f*)malloc(in.size() * sizeof *points);
+		count = in.size();
+		for (i = 0; i < count; i++)
+			points[i] = *(in.begin() + i);
+	}
+
+	Shape(const Shape& other) {
+		int s = other.count * sizeof *points;
+		count = other.count;
+		points = (v2f*)malloc(s);
+		memcpy(points, other.points, s);
+	}
+
+	~Shape() {
+		free(points);
+	}
+
+	void render(SDL_Renderer* ren) {
+		int i, c = count;
+		for (i = 0; i < c; i++) {
+			v2f a = points[i];
+			v2f b = points[(i + 1) % c];
+			SDL_RenderDrawLine(
+				ren,
+				(int)a.x,
+				(int)a.y,
+				(int)b.x,
+				(int)b.y
+			);
+		}
+	}
+
+	std::pair<v2f*, int> normals() const {
+		int c = (count >> 1) + 1, i;
+		v2f* normals = (v2f*)malloc(c * sizeof *normals);
+		for (i = 0; i < count; i++) {
+			v2f a = points[i];
+			v2f b = points[(i + 1) % count];
+			v2f e = a - b;
+			v2f n = v2f::normalised(v2f(e.y, -e.x));
+			normals[i] = n;
+		}
+		return { normals, i };
+	}
+};
+
+bool intersect(Shape& a, Shape& b) {
+	bool r = true;
+	int i;
+	auto[fn, fnc] = a.normals();
+	auto[sn, snc] = b.normals();
+	auto project = [](const Shape& shape, v2f axis) {
+		int i;
+		float mini =  INFINITY;
+		float maxi = -INFINITY;
+		for (i = 0; i < shape.count; i++) {
+			v2f p = shape.points[i];
+			float d = v2f::dot(p, axis);
+			if (d < mini) mini = d;
+			if (d > maxi) maxi = d;
+		}
+		return std::pair<float, float>{ mini, maxi };
+	};
+	for (i = 0; i < fnc; i++) {
+		v2f axis = fn[i];
+		auto [fp1, fp2] = project(a, axis);
+		auto [sp1, sp2] = project(b, axis);
+		if (fp2 > sp1 && fp1 > sp2) {
+			r = false;
+			goto end;
+		}
+	}
+	for (i = 0; i < snc; i++) {
+		v2f axis = sn[i];
+		auto [fp1, fp2] = project(a, axis);
+		auto [sp1, sp2] = project(b, axis);
+		if (fp2 > sp1 && fp1 > sp2) {
+			r = false;
+			goto end;
+		}
+	}
+end:
+	free(fn);
+	free(sn);
+	return r;
+}
+
+int main(int argc, const char** argv) {
+	int r = 1;
+	SDL_Window* window;
+	SDL_Renderer* ren;
+	SDL_Init(SDL_INIT_VIDEO | SDL_INIT_AUDIO | SDL_INIT_JOYSTICK);
+	SDL_CreateWindowAndRenderer(
+		800,
+		800,
+		0,
+		&window,
+		&ren
+	);
+	Shape a({
+		v2f(300.0f, 300.0f),
+		v2f(400.0f, 400.0f),
+		v2f(250.0f, 500.0f),
+	});
+	Shape tri({
+		v2f(0.0f, 0.0f),
+		v2f(50.0f, 200.0f),
+		v2f(200.0f, 100.0f),
+	});
+	while (r) {
+		int i;
+		SDL_Event e;
+		while (SDL_PollEvent(&e)) {
+			switch (e.type) {
+				case SDL_QUIT:
+					r = 0;
+					break;
+			}
+		}
+		int mx, my;
+		SDL_GetMouseState(&mx, &my);
+		Shape b(tri);
+		for (i = 0; i < b.count; i++) {
+			v2f& p = b.points[i];
+			p.x += mx;
+			p.y += my;
+		}
+		SDL_SetRenderDrawColor(ren, 0x00, 0x00, 0x00, 0x00);
+		SDL_RenderClear(ren);
+		SDL_SetRenderDrawColor(ren, 0xff, 0x00, 0xff, 0xff);
+		a.render(ren);
+		if (intersect(a, b)) {
+			SDL_SetRenderDrawColor(ren, 0xff, 0x00, 0x00, 0xff);
+		}
+		b.render(ren);
+		SDL_RenderPresent(ren);
+	}
+	SDL_DestroyRenderer(ren);
+	SDL_DestroyWindow(window);
+	SDL_Quit();
+	return 0;
+}