1 files changed, 80 insertions, 0 deletions
diff --git a/libphysics/blockmap.c b/libphysics/blockmap.c
new file mode 100644
index 0000000..7f02b1a
--- /dev/null
+++ b/libphysics/blockmap.c
@@ -0,0 +1,80 @@
+#include <vecmath.h>
+#include <physics.h>
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "dat.h"
+
+#define BUCKETS      0x10000 /* must be a power of 2 */     
+
+#define BUCKETMASK  (BUCKETS - 1)
+
+#define PHI   1.618033988749895 
+#define HMUL  (int)(BUCKETS / PHI)
+
+static int hash(int a);
+static void makeblockmap(Body *b);
+
+static BlockmapNode *allocnode(void);
+
+static BlockmapNode nodes[(1 << 20)];
+static int nodesi;
+static BlockmapNode *lists[BUCKETS];
+
+void
+phxmakeblkmap()
+{
+	nodesi = 0;
+
+	for(int i = 0; i < BUCKETS; i++)
+		lists[i] = NULL;
+
+	for(int i = 0; i < phxbodypoolsize; i++) {
+		Body *b = phxbodypool + i;
+		if(!b->active)
+			continue;
+		makeblockmap(b);
+	}
+}
+
+void
+makeblockmap(Body *b)
+{
+	BlockmapNode *n;
+	if((n = allocnode()) == NULL)
+		return;
+
+	n->id = b - phxbodypool;
+	n->next = NULL;
+
+	int mapx = (int)floorf(b->pos[0] / BLOCK_SIZE);
+	int mapy = (int)floorf(b->pos[1] / BLOCK_SIZE);
+	int h = hash(mapx + hash(mapy));
+
+	n->next = lists[h];
+	lists[h] = n;
+}
+
+int
+hash(int a)
+{
+	return (a * HMUL) & BUCKETMASK;
+}
+
+BlockmapNode *
+allocnode(void)
+{
+	if(nodesi == (sizeof(nodes) / sizeof(nodes[0])))
+		return NULL;
+	return nodes + nodesi++;
+}
+
+BlockmapNode *
+phxnodelist(float x, float y)
+{
+	int mapx = (int)floorf(x / BLOCK_SIZE);
+	int mapy = (int)floorf(y / BLOCK_SIZE);
+	int h = hash(mapx + hash(mapy));
+	return lists[h];
+}