collision yes/no

1 files changed, 194 insertions, 6 deletions

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