libphysics: otimização por blockmaps

bug: o object não pode ser maior do que o BLOCK_SIZE, definido no dat.h

6 files changed, 141 insertions, 4 deletions

diff --git a/libphysics/Makefile b/libphysics/Makefile
index cc76e28..154e9f2 100644
--- a/libphysics/Makefile
+++ b/libphysics/Makefile
@@ -2,7 +2,8 @@ OBJ=aabb.o\
 	body.o\
 	event.o\
 	_map.o\
-	tick.o
+	tick.o\
+	blockmap.o
 
 CFLAGS=-I../include -g
 
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];
+}
diff --git a/libphysics/dat.h b/libphysics/dat.h
index 75869a4..9fe3312 100644
--- a/libphysics/dat.h
+++ b/libphysics/dat.h
@@ -1,3 +1,5 @@
+#define BLOCK_SIZE   128
+
 typedef struct Body {
 	BodyType type;
 	int active;
@@ -7,6 +9,12 @@ typedef struct Body {
 	vec2 accel;
 } Body;
 
+typedef struct BlockmapNode BlockmapNode;
+struct BlockmapNode {
+	BodyID id;
+	BlockmapNode *next;
+};
+
 void phxmapcollision(Body *);
 void phxbodycollision(Body *, Body *);
 
@@ -17,6 +25,9 @@ void phxaabbresolv(Body *a, Body *b, vec2 p, vec2 n);
 int phxenqevent(CollisionEvent *ev);
 int phxdeqevent(CollisionEvent *ev);
 
+void phxmakeblkmap(void);
+BlockmapNode *phxnodelist(float x, float y);
+
 extern int  phxmapwidth, phxmapheight;
 extern int *phxmapbuffer;
 
diff --git a/libphysics/test/Makefile b/libphysics/test/Makefile
index 1d60fc8..a628f6b 100644
--- a/libphysics/test/Makefile
+++ b/libphysics/test/Makefile
@@ -1,5 +1,5 @@
 CFLAGS=-I../../include -g 
-LDFLAGS=-L../ -lphysics `pkg-config --libs sdl3`
+LDFLAGS=-L../ -lphysics `pkg-config --libs sdl3` -lm
 
 all: vis
 
diff --git a/libphysics/test/vis.c b/libphysics/test/vis.c
index 3111005..ad3cef8 100644
--- a/libphysics/test/vis.c
+++ b/libphysics/test/vis.c
@@ -49,7 +49,7 @@ main()
 		assert(b != -1);
 
 		phxsetpos(b, rand() % 800, rand() % 600);
-		phxsetsize(b, 16.0, 16.0);
+		phxsetsize(b, 5.0, 5.0);
 		phxapplyaccel(b, (float[]){ 30000 * FLOAT_RAND, 30000 * FLOAT_RAND });
 	}
 
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;
+	}
+}