libphysics: funções de broadphase

1 files changed, 66 insertions, 0 deletions

diff --git a/libphysics/blockmap.c b/libphysics/blockmap.c
index 7f02b1a..26af021 100644
--- a/libphysics/blockmap.c
+++ b/libphysics/blockmap.c
@@ -13,15 +13,46 @@
 #define PHI   1.618033988749895 
 #define HMUL  (int)(BUCKETS / PHI)
 
+#define LENGTH(X) (sizeof (X) / sizeof (X)[0])
+
 static int hash(int a);
 static void makeblockmap(Body *b);
 
+static void enqueuetest(TestCollision *t);
+static void bodycollisionmap(Body *);
+static void bodycollisionmapsub(Body *, BlockmapNode *);
 static BlockmapNode *allocnode(void);
 
+static void phxmakeblkmap(void);
+static BlockmapNode *phxnodelist(float x, float y);
+
 static BlockmapNode nodes[(1 << 20)];
 static int nodesi;
 static BlockmapNode *lists[BUCKETS];
 
+static TestCollision testcollision[0x1000];
+static int testcollisioncount;
+
+void
+__phxbroadphasereset(void)
+{
+	static int count = 0;
+	if(count == 0)
+		phxmakeblkmap();
+	count = (count + 1) % 4;
+}
+
+void 
+__phxbroadphase(int n, BodyID bid[n])
+{
+	testcollisioncount = 0;
+	for(int i = 0; i < n; i++) {
+		Body *b = phxbodypool + bid[i];
+		bodycollisionmap(b);
+	}
+	__phxnarrowphase(testcollisioncount, testcollision);
+}
+
 void
 phxmakeblkmap()
 {
@@ -78,3 +109,38 @@ phxnodelist(float x, float y)
 	int h = hash(mapx + hash(mapy));
 	return lists[h];
 }
+
+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)
+{
+	while(n) {
+		Body *b = phxbodypool + n->id;
+		enqueuetest(&(TestCollision) {
+			.a = a - phxbodypool,
+			.b = b - phxbodypool
+		});
+		n = n->next;
+	}
+}
+
+void
+enqueuetest(TestCollision *t)
+{
+	testcollision[testcollisioncount++] = *t;
+	if(testcollisioncount >= LENGTH(testcollision)) {
+		__phxnarrowphase(testcollisioncount, testcollision);
+		testcollisioncount = 0;
+	}
+}