1 files changed, 46 insertions, 1 deletions

diff --git a/libphysics/tick.c b/libphysics/tick.c
index 0d57a8d..8477ff7 100644
--- a/libphysics/tick.c
+++ b/libphysics/tick.c
@@ -10,6 +10,9 @@ static void tick(void);
 static void mapcollision(Body *);
 static void bodycollision(Body *);
 
+static void bodycollisionmap(Body *);
+static void bodycollisionmapsub(Body *, BlockmapNode *);
+
 #define TILE_SIZE 16
 
 void
@@ -45,13 +48,14 @@ tick(void)
 		vec2_add_scaled(b->vel, b->vel, b->accel, PHX_TICK_TIME);
 	}
 
+	phxmakeblkmap();
 	for(int i = 0; i < phxbodypoolsize; i++) {
 		Body *b = phxbodypool + i;
 		if(!b->active)
 			continue;
 
 		mapcollision(b);
-		bodycollision(b);
+		bodycollisionmap(b);
 	}
 
 	for(int i = 0; i < phxbodypoolsize; i++) {
@@ -136,3 +140,44 @@ bodycollision(Body *a)
 		}
 	}
 }
+
+void
+bodycollisionmap(Body *a)
+{
+	for(int y = -1; y <= 1; y++)
+	for(int x = -1; x <= 1; x++) {
+		bodycollisionmapsub(a, phxnodelist(
+			a->pos[0] + x * BLOCK_SIZE, 
+			a->pos[1] + y * BLOCK_SIZE
+		));	
+	}
+}
+
+void
+bodycollisionmapsub(Body *a, BlockmapNode *n)
+{
+	CollisionEvent event;
+	while(n) {
+		Body *b = phxbodypool + n->id;
+
+		if(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 = a - phxbodypool;
+			event.body2 = b - phxbodypool;
+			phxenqevent(&event);
+		}
+
+		n = n->next;
+	}
+}