1 files changed, 36 insertions, 0 deletions
diff --git a/libphysics/narrow.c b/libphysics/narrow.c
new file mode 100644
index 0000000..14575be
--- /dev/null
+++ b/libphysics/narrow.c
@@ -0,0 +1,36 @@
+#include <vecmath.h>
+#include <physics.h>
+#include <stdlib.h>
+
+#include "dat.h"
+
+void
+__phxnarrowphase(int n, TestCollision tests[n])
+{
+	CollisionEvent event;
+
+	for(int i = 0; i < n; i++) {
+		TestCollision *c = tests + i;
+		Body *a = c->a + phxbodypool;
+		Body *b = c->b + phxbodypool;
+
+		if(a < b && phxaabbcheck(a, b)) {
+			float p[2], n[2], rvel[2], j;
+			phxaabbresolv(a, b, p, n);
+
+			vec2_sub(rvel, a->vel, b->vel);
+			j = vec2_dot(rvel, n);
+
+			vec2_sub_scaled(a->vel, a->vel, n,  0.5);
+			vec2_sub_scaled(b->vel, b->vel, n, -0.5);
+
+			vec2_sub_scaled(a->pos, a->pos, p,  0.5);
+			vec2_sub_scaled(b->pos, b->pos, p, -0.5);
+
+			event.body1 = c->a;
+			event.body2 = c->b;
+			phxenqevent(&event);
+		}
+	}
+}
+