49 lines
1.4 KiB
C
49 lines
1.4 KiB
C
|
|
#include "grid.h"
|
|
|
|
#define BLOCK(p, left, step) (int)((p-left)/step)
|
|
|
|
static inline double pyt_h2(double r, double x) {
|
|
return sqrt(r*r-x*x);
|
|
}
|
|
|
|
static void _insert(grid *g, object *o, double l, double r, double t, double b) {
|
|
double hstep = g->top - g->bottom / GRID_SIZE;
|
|
double wstep = g->right - g->left / GRID_SIZE, wstep2 = wstep/2;
|
|
int y,
|
|
ndx_b = max(0, BLOCK(b, g->bottom, hstep)),
|
|
ndx_t = min(GRID_SPLIT-1, BLOCK(t, g->bottom, hstep));
|
|
for (y = ndx_b; y <= ndx_t; y++) {
|
|
double posy = g->bottom + wstep * (y+0.5);
|
|
if (posy > o->y) posy -= wstep2;
|
|
else posy += wstep2;
|
|
double dx = pyt_h2(o->r, posy - o->y);
|
|
int x,
|
|
ndx_l = max(0, BLOCK(o->x-dx, g->left, wstep)),
|
|
ndx_r = min(GRID_SPLIT-1, BLOCK(o->y-dy, g->left, wstep));
|
|
for (x = ndx_l; x <= ndx_r; x++) {
|
|
if (g->nodes[y][x].grid) {
|
|
_insert(g->nodes[y][x].grid, o, l, r, t, b);
|
|
}
|
|
g_queue_push_back(g->nodes[y][x].objects, o);
|
|
}
|
|
}
|
|
}
|
|
|
|
void grid_insert(grid* g, object *o) {
|
|
object *co = g_slice_new(object);
|
|
memcpy(co, o, sizeof(*co));
|
|
_insert(g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
|
|
g_queue_push_back(g->nodes[y][x].objects, o);
|
|
}
|
|
|
|
void _do_split(gpointer _o, gpointer g) {
|
|
object *o = (object*) o;
|
|
_insert((grid*) g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
|
|
}
|
|
|
|
void grid_split(gridnode *n) {
|
|
n->grid = g_slice_new0(grid);
|
|
g_queue_foreach(n->objects, _do_split, n->grid);
|
|
}
|