1 files changed, 101 insertions, 0 deletions
diff --git a/physics.c b/physics.c
index 5b32356..51f8999 100644
--- a/physics.c
+++ b/physics.c
@@ -1,8 +1,77 @@
 #include <math.h>
+#include <stdbool.h>
+#include <stddef.h>
 
 #include "dat.h"
 
 static void mink(Body *a, Body *b, float *minx, float *miny, float *maxx, float *maxy);
+static int checkcollision(Body *a, Body *b);
+static void resolvecoll(Body *a, Body *b, float p[2]);
+
+static EntityCollision ecoll[2048];
+static int             ecollstart, ecollend, ecollsi;
+
+void
+entphysics(float delta)
+{
+	for(int i = 0; i < ebufi; i++) {
+		Entity *e = entitybuffer + i;
+		if(!e->active)
+			continue;
+
+		for(int y = 0; y < map_height; y++)
+			for(int x = 0; x < map_width; x++) {
+				if(!map_data[x + y * map_width])
+					continue;
+
+				Body b = {
+					.pos = {
+						x * ENTITY_SIZE * 2.0,
+						y * ENTITY_SIZE * 2.0,
+					},
+					.size = { ENTITY_SIZE, ENTITY_SIZE },
+				};
+
+				if(checkcollision(&e->body, &b)) {
+					float p[2];
+					resolvecoll(&e->body, &b, p);
+
+					for(int i = 0; i < 2; i++)
+						e->body.pos[i] -= p[i];
+
+					if(p[0] != 0.0) {
+						e->body.vel[0] = 0;
+					} else {
+						e->body.vel[1] = 0;
+					}
+				}
+			}
+
+		for(int j = i; j < ebufi; j++) {
+			Entity *f = entitybuffer + j;
+			if(!f->active)
+				break;
+
+			if(checkcollision(&e->body, &f->body)) {
+				EntityCollision cl;
+
+				cl.self = e;
+				cl.target = f;
+
+				enqcoll(&cl);
+			}
+		}
+	}
+
+	for(int i = 0; i < ebufi; i++) {
+		Entity *e = entitybuffer + i;
+		if(!e->active)
+			continue;
+
+		for(int j = 0; j < 2; j++)
+			e->body.pos[j] += e->body.vel[j] * delta;
+	}
+}
 
 int
 checkcollision(Body *a, Body *b)
@@ -47,3 +116,35 @@ mink(Body *a, Body *b, float *minx, float *miny, float *maxx, float *maxy)
 	*miny = p[1] - s[1];
 	*maxy = p[1] + s[1];
 }
+
+void
+enqcoll(EntityCollision *c)
+{
+	if(ecollsi >= 2048)
+		return;
+
+	ecollsi++;
+
+	ecoll[ecollend++] = *c;
+	if(ecollend >= 2048)
+		ecollend = 0;
+}
+
+EntityCollision *
+deqcoll(void)
+{
+	EntityCollision *c;
+	if(ecollsi <= 0)
+		return NULL;
+
+	ecollsi--;
+
+	c = ecoll + ecollstart++;
+	
+	if(ecollstart >= 2048)
+		ecollstart = 0;
+
+	return c;
+}
+
+