broad phase for colliders via aabb spatial hash

src/collider.c

@@ -96,7 +96,7 @@ INTERNAL struct collider_menkowski_point get_menkowski_point(struct collider_sha
  * AABB
  * ========================== */
 
-struct aabb collider_get_aabb(struct collider_shape *shape, struct xform xf)
+struct aabb collider_aabb_from_collider(struct collider_shape *shape, struct xform xf)
 {
     struct aabb res;
     res.p0.x = collider_get_support_point(shape, xf, V2(-1, 0)).p.x - COLLISION_TOLERANCE;

src/collider.h

@@ -60,7 +60,7 @@ struct collider_closest_points_result {
 
 struct collider_support_point collider_get_support_point(struct collider_shape *shape, struct xform xf, struct v2 dir);
 
-struct aabb collider_get_aabb(struct collider_shape *shape, struct xform xf);
+struct aabb collider_aabb_from_collider(struct collider_shape *shape, struct xform xf);
 
 b32 collider_test_aabb(struct aabb box0, struct aabb box1);
 

src/common.h

@@ -496,6 +496,11 @@ struct entity_handle {
     u64 gen;
 };
 
+struct space_client_handle {
+    u64 idx;
+    u64 gen;
+};
+
 /* ========================== *
  * Tag structs
  * ========================== */

src/config.h

@@ -29,6 +29,10 @@
 
 #define IMAGE_PIXELS_PER_UNIT 256.0
 
+/* 256^2 = 65536 buckets */
+#define SPACE_CELL_BUCKETS_SQRT (256)
+#define SPACE_CELL_SIZE 1.0f
+
 #define GAME_FPS 50.0
 #define GAME_TIMESCALE 1
 

src/entity.c

@@ -5,9 +5,7 @@
 #define STORE_ENTITIES_OFFSET (sizeof(struct entity_store) + (sizeof(struct entity_store) % alignof(struct entity)))
 
 /* Accessed via entity_store_nil() */
-READONLY struct entity_store _g_entity_store_nil = {
+READONLY struct entity_store _g_entity_store_nil = ZI;
-    0
-};
 
 /* Accessed via entity_nil() */
 /* TODO: Allocate nil entity in nil store */
@@ -105,7 +103,7 @@ INTERNAL struct entity *entity_alloc_internal(struct entity_store *store)
 struct entity *entity_alloc(struct entity *parent)
 {
     ASSERT(parent->valid);
-    struct entity_store *store = entity_get_store(parent);
+    struct entity_store *store = entity_store_from_entity(parent);
     struct entity *e = entity_alloc_internal(store);
     entity_link_parent(e, parent);
     return e;
@@ -141,7 +139,7 @@ void entity_release(struct entity_store *store, struct entity *ent)
  * Query
  * ==========================  */
 
-struct entity_store *entity_get_store(struct entity *ent)
+struct entity_store *entity_store_from_entity(struct entity *ent)
 {
     if (ent->valid) {
         u64 first_entity_addr = (u64)(ent - ent->handle.idx);
@@ -244,7 +242,7 @@ struct xform entity_get_xform(struct entity *ent)
         if (ent->is_top) {
             xf = ent->local_xform;
         } else {
-            struct entity_store *store = entity_get_store(ent);
+            struct entity_store *store = entity_store_from_entity(ent);
             struct entity *parent = entity_from_handle(store, ent->parent);
             xf = entity_get_xform_w_store(store, parent);
             xf = xform_mul(xf, ent->local_xform);
@@ -267,7 +265,7 @@ struct xform entity_get_local_xform(struct entity *ent)
 void entity_set_xform(struct entity *ent, struct xform xf)
 {
     if (!xform_eq(xf, ent->cached_global_xform)) {
-        struct entity_store *store = entity_get_store(ent);
+        struct entity_store *store = entity_store_from_entity(ent);
         /* Update local xform */
         if (ent->is_top) {
             ent->local_xform = xf;
@@ -287,7 +285,7 @@ void entity_set_local_xform(struct entity *ent, struct xform xf)
     if (!xform_eq(xf, ent->local_xform)) {
         ent->local_xform = xf;
         ent->cached_global_xform_dirty = true;
-        entity_mark_child_xforms_dirty(entity_get_store(ent), ent);
+        entity_mark_child_xforms_dirty(entity_store_from_entity(ent), ent);
     }
 }
 
@@ -341,7 +339,7 @@ void entity_apply_torque(struct entity *ent, f32 torque)
 
 void entity_link_parent(struct entity *ent, struct entity *parent)
 {
-    struct entity_store *store = entity_get_store(ent);
+    struct entity_store *store = entity_store_from_entity(ent);
 
     if (ent->parent.gen) {
         /* Unlink from current parent */
@@ -374,7 +372,7 @@ void entity_link_parent(struct entity *ent, struct entity *parent)
 /* NOTE: Entity will be dangling after calling this, should re-link to root entity. */
 void entity_unlink_from_parent(struct entity *ent)
 {
-    struct entity_store *store = entity_get_store(ent);
+    struct entity_store *store = entity_store_from_entity(ent);
 
     struct entity_handle parent_handle = ent->parent;
     struct entity *parent = entity_from_handle(store, parent_handle);

src/entity.h

@@ -133,6 +133,8 @@ struct entity {
     struct phys_collision_debug collision_debug_data;
 #endif
 
+    struct space_client_handle space_handle;
+
     /* ====================================================================== */
     /* Contact constraint */
 
@@ -390,7 +392,7 @@ void entity_apply_angular_impulse(struct entity *ent, f32 impulse);
 void entity_apply_torque(struct entity *ent, f32 torque);
 
 /* Query */
-struct entity_store *entity_get_store(struct entity *ent);
+struct entity_store *entity_store_from_entity(struct entity *ent);
 struct entity *entity_from_handle(struct entity_store *store, struct entity_handle handle);
 struct entity *entity_find_first_match_one(struct entity_store *store, enum entity_prop prop);
 struct entity *entity_find_first_match_all(struct entity_store *store, struct entity_prop_array props);

src/game.c

@@ -13,11 +13,14 @@
 #include "phys.h"
 #include "collider.h"
 #include "rng.h"
+#include "space.h"
 
 GLOBAL struct {
     struct atomic_i32 game_thread_shutdown;
     struct sys_thread game_thread;
 
+    u64 last_phys_iteration;
+
     b32 paused;
     struct sprite_scope *sprite_frame_scope;
 
@@ -38,6 +41,7 @@ GLOBAL struct {
 #if COLLIDER_DEBUG
     struct entity_lookup collision_debug_lookup;
 #endif
+    struct space *space;
 
     /* Ticks */
     struct sys_mutex prev_tick_mutex;
@@ -140,6 +144,7 @@ INTERNAL void reset_world(void)
 #if COLLIDER_DEBUG
     G.collision_debug_lookup = entity_lookup_alloc(4096);
 #endif
+    G.space = space_alloc(SPACE_CELL_SIZE, SPACE_CELL_BUCKETS_SQRT);
 
     /* Re-create world */
     world_alloc(&G.tick);
@@ -498,6 +503,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
     struct entity *root = G.root;
     struct entity_store *store = G.tick.entity_store;
+    struct space *space = G.space;
     struct sprite_scope *sprite_frame_scope = G.sprite_frame_scope;
 
     (UNUSED)dt;
@@ -1065,6 +1071,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         struct phys_ctx ctx = ZI;
         ctx.tick_id = G.tick.tick_id;
         ctx.store = store;
+        ctx.space = space;
         ctx.contact_lookup = &G.contact_lookup;
         ctx.pre_solve_callback = on_collision;
 #if COLLIDER_DEBUG
@@ -1085,7 +1092,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         }
 
         /* Step */
-        phys_step(&ctx, dt);
+        G.last_phys_iteration = phys_step(&ctx, dt, G.last_phys_iteration);
     }
 
     /* ========================== *

src/phys.c

@@ -2,6 +2,7 @@
 #include "math.h"
 #include "scratch.h"
 #include "entity.h"
+#include "space.h"
 
 GLOBAL struct {
     /* Constants */
@@ -39,7 +40,7 @@ struct phys_startup_receipt phys_startup(void)
  * Contact
  * ========================== */
 
-struct phys_collision_data_array phys_create_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt)
+struct phys_collision_data_array phys_create_and_update_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt, u64 phys_iteration)
 {
     __prof;
     struct phys_collision_data_array res = ZI;
@@ -47,6 +48,7 @@ struct phys_collision_data_array phys_create_contacts(struct arena *arena, struc
     u64 tick_id = ctx->tick_id;
     struct entity_lookup *contact_lookup = ctx->contact_lookup;
     struct entity_lookup *debug_lookup = ctx->debug_lookup;
+    struct space *space = ctx->space;
     struct entity_store *store = ctx->store;
     struct entity *root = entity_from_handle(store, store->root);
 
@@ -58,22 +60,25 @@ struct phys_collision_data_array phys_create_contacts(struct arena *arena, struc
 
         struct xform check0_xf = entity_get_xform(check0);
         struct collider_shape check0_collider = check0->local_collider;
+        struct aabb aabb = collider_aabb_from_collider(&check0_collider, check0_xf);
 
-        for (u64 check1_index = 0; check1_index < store->reserved; ++check1_index) {
+        struct space_iter iter = space_iter_begin_aabb(space, aabb);
-            struct entity *check1 = &store->entities[check1_index];
+        struct space_client *client;
+        while ((client = space_iter_next(&iter))) {
+            struct entity *check1 = entity_from_handle(store, client->ent);
             if (check1 == check0) continue;
             if (!entity_is_valid_and_active(check1)) continue;
             if (!(entity_has_prop(check1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(check1, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue;
             if (check1->local_collider.count <= 0) continue;
 
-            /* Deterministic entity order based on index */
+            /* Deterministic order based on entity index */
             struct entity *e0;
             struct entity *e1;
             struct xform e0_xf;
             struct xform e1_xf;
             struct collider_shape e0_collider;
             struct collider_shape e1_collider;
-            if (check0_index < check1_index) {
+            if (check0_index < check1->handle.idx) {
                 e0 = check0;
                 e1 = check1;
                 e0_xf = check0_xf;
@@ -96,11 +101,11 @@ struct phys_collision_data_array phys_create_contacts(struct arena *arena, struc
             if (entry) {
                 constraint_ent = entity_from_handle(store, entry->entity);
                 if (entity_is_valid_and_active(constraint_ent)) {
-                    if (constraint_ent->contact_constraint_data.last_iteration >= tick_id) {
+                    if (constraint_ent->contact_constraint_data.last_phys_iteration >= phys_iteration) {
                         /* Already processed constraint this iteration */
                         continue;
                     } else {
-                        ++constraint_ent->contact_constraint_data.last_iteration;
+                        constraint_ent->contact_constraint_data.last_phys_iteration = phys_iteration;
                     }
                 } else {
                     /* Constraint ent no longer valid, delete entry */
@@ -270,11 +275,12 @@ struct phys_collision_data_array phys_create_contacts(struct arena *arena, struc
             (UNUSED)debug_lookup;
 #endif
         }
+        space_iter_end(&iter);
     }
     return res;
 }
 
-void phys_prepare_contacts(struct phys_ctx *ctx)
+void phys_prepare_contacts(struct phys_ctx *ctx, u64 phys_iteration)
 {
     __prof;
     struct entity_lookup *contact_lookup = ctx->contact_lookup;
@@ -290,7 +296,7 @@ void phys_prepare_contacts(struct phys_ctx *ctx)
         u32 num_points = constraint->num_points;
         struct entity *e0 = entity_from_handle(store, constraint->e0);
         struct entity *e1 = entity_from_handle(store, constraint->e1);
-        if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
+        if (constraint->last_phys_iteration >= phys_iteration && num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) {
             struct v2 normal = constraint->normal;
             struct v2 tangent = v2_perp(normal);
 
@@ -945,15 +951,6 @@ void phys_solve_mouse_joints(struct phys_ctx *ctx, f32 dt)
                 f32 mass_scale = softness.mass_scale;
                 f32 impulse_scale = softness.impulse_scale;
 
-#if 0
-                struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
-                struct v2 b = v2_mul(xform_basis_mul_v2(xform_invert(joint->linear_mass_xf), v2_add(vel, bias)), mass_scale);
-
-                struct v2 old_impulse = joint->linear_impulse;
-                struct v2 impulse = v2_sub(v2_mul(old_impulse, -impulse_scale), b);
-                joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse);
-                impulse = v2_sub(joint->linear_impulse, old_impulse);
-#else
                 struct v2 vel = v2_add(v, v2_perp_mul(vcp, w));
                 struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias));
 
@@ -969,7 +966,6 @@ void phys_solve_mouse_joints(struct phys_ctx *ctx, f32 dt)
 
                 impulse.x = joint->linear_impulse.x - old_impulse.x;
                 impulse.y = joint->linear_impulse.y - old_impulse.y;
-#endif
 
                 v = v2_add(v, v2_mul(impulse, inv_m));
                 w += v2_wedge(vcp, impulse) * inv_i;
@@ -1048,7 +1044,13 @@ void phys_integrate_velocities(struct phys_ctx *ctx, f32 dt)
         if (!entity_is_valid_and_active(ent)) continue;
         if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL_DYNAMIC) && !entity_has_prop(ent, ENTITY_PROP_PHYSICAL_KINEMATIC)) continue;
 
-        entity_set_xform(ent, get_derived_xform(ent, dt));
+        struct xform xf = get_derived_xform(ent, dt);
+        entity_set_xform(ent, xf);
+
+        struct space_client *space_client = space_client_from_handle(ctx->space, ent->space_handle);
+        if (space_client->valid) {
+            space_client_update_aabb(space_client, collider_aabb_from_collider(&ent->local_collider, xf));
+        }
     }
 }
 
@@ -1101,19 +1103,46 @@ f32 phys_determine_earliest_toi_for_bullets(struct phys_ctx *ctx, f32 step_dt, f
     return smallest_t;
 }
 
+/* ========================== *
+ * Space
+ * ========================== */
+
+void phys_update_aabbs(struct phys_ctx *ctx)
+{
+    struct entity_store *store = ctx->store;
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *ent = &store->entities[entity_index];
+        if (!entity_is_valid_and_active(ent)) continue;
+        if (ent->local_collider.count <= 0) continue;
+
+        struct xform xf = entity_get_xform(ent);
+        struct space_client *space_client = space_client_from_handle(ctx->space, ent->space_handle);
+        if (!space_client->valid) {
+            space_client = space_client_alloc(ctx->space, ent->handle);
+            ent->space_handle = space_client->handle;
+        }
+        space_client_update_aabb(space_client, collider_aabb_from_collider(&ent->local_collider, xf));
+    }
+}
+
 /* ========================== *
  * Step
  * ========================== */
 
-void phys_step(struct phys_ctx *ctx, f32 timestep)
+u64 phys_step(struct phys_ctx *ctx, f32 timestep, u64 last_phys_iteration)
 {
     __prof;
     phys_integrate_forces(ctx, timestep);
+    u64 phys_iteration = last_phys_iteration;
 
     f32 remaining_dt = timestep;
     while (remaining_dt > 0) {
         __profscope(step_part);
+        ++phys_iteration;
         struct temp_arena scratch = scratch_begin_no_conflict();
+
+        phys_update_aabbs(ctx);
+
         /* TOI */
         f32 step_dt = remaining_dt;
         {
@@ -1130,10 +1159,10 @@ void phys_step(struct phys_ctx *ctx, f32 timestep)
 #endif
         }
 
-        struct phys_collision_data_array collision_data = phys_create_contacts(scratch.arena, ctx, timestep - remaining_dt);
+        struct phys_collision_data_array collision_data = phys_create_and_update_contacts(scratch.arena, ctx, timestep - remaining_dt, phys_iteration);
         phys_create_mouse_joints(ctx);
 
-        phys_prepare_contacts(ctx);
+        phys_prepare_contacts(ctx, phys_iteration);
         phys_prepare_motor_joints(ctx);
         phys_prepare_mouse_joints(ctx);
 
@@ -1176,4 +1205,6 @@ void phys_step(struct phys_ctx *ctx, f32 timestep)
 
         scratch_end(scratch);
     }
+
+    return phys_iteration;
 }

src/phys.h

@@ -4,6 +4,7 @@
 #include "collider.h"
 #include "math.h"
 
+struct space;
 struct entity_store;
 struct entity_lookup;
 
@@ -30,6 +31,7 @@ typedef PHYS_COLLISION_CALLBACK_FUNC_DEF(phys_collision_callback_func, data);
 /* Structure containing data used for a single physics step */
 struct phys_ctx {
     u64 tick_id;
+    struct space *space;
     struct entity_store *store;
     struct entity_lookup *contact_lookup;
 
@@ -72,7 +74,7 @@ struct phys_contact_point {
 };
 
 struct phys_contact_constraint {
-    u64 last_updated_tick;  /* To avoid checking collisions for the same constraint twice in one tick */
+    u64 last_phys_iteration;  /* To avoid checking collisions for the same constraint twice in one tick */
     b32 skip_solve;
     struct entity_handle e0;
     struct entity_handle e1;
@@ -107,8 +109,8 @@ struct phys_collision_debug {
     struct xform xf1;
 };
 
-struct phys_collision_data_array phys_create_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt);
+struct phys_collision_data_array phys_create_and_update_contacts(struct arena *arena, struct phys_ctx *ctx, f32 elapsed_dt, u64 phys_iteration);
-void phys_prepare_contacts(struct phys_ctx *ctx);
+void phys_prepare_contacts(struct phys_ctx *ctx, u64 phys_iteration);
 void phys_warm_start_contacts(struct phys_ctx *ctx);
 void phys_solve_contacts(struct phys_ctx *ctx, f32 dt, b32 apply_bias);
 
@@ -190,10 +192,17 @@ void phys_integrate_velocities(struct phys_ctx *ctx, f32 dt);
 
 f32 phys_determine_earliest_toi_for_bullets(struct phys_ctx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations);
 
+/* ========================== *
+ * Space
+ * ========================== */
+
+void phys_update_aabbs(struct phys_ctx *ctx);
+
 /* ========================== *
  * Step
  * ========================== */
 
-void phys_step(struct phys_ctx *ctx, f32 timestep);
+/* Returns phys iteration to be fed into next step. Supplied iteration must be > 0. */
+u64 phys_step(struct phys_ctx *ctx, f32 timestep, u64 phys_iteration);
 
 #endif

src/space.c

@@ -0,0 +1,413 @@
+#include "space.h"
+#include "math.h"
+#include "arena.h"
+#include "collider.h"
+
+/* Offset in bytes from start of space struct to start of client array (assume adjacently allocated) */
+#define SPACE_CLIENTS_OFFSET (sizeof(struct space) + (sizeof(struct space) % alignof(struct space_client)))
+
+/* Accessed via entity_nil() */
+READONLY struct space_client _g_space_client_nil = { .valid = false };
+READONLY struct space_cell _g_space_cell_nil = { .valid = false };
+READONLY struct space _g_space_nil = { .valid = false };
+
+/* ========================== *
+ * Space alloc
+ * ========================== */
+
+struct space *space_alloc(f32 cell_size, u32 num_buckets_sqrt)
+{
+    struct arena arena = arena_alloc(GIGABYTE(64));
+    struct space *space = arena_push_zero(&arena, struct space);
+
+    space->valid = true;
+    space->client_arena = arena;
+    space->clients = arena_dry_push(&space->client_arena, struct space_client);
+
+    space->cell_arena = arena_alloc(GIGABYTE(64));
+    space->cell_size = cell_size;
+    space->num_buckets = num_buckets_sqrt * num_buckets_sqrt;
+    space->num_buckets_sqrt = num_buckets_sqrt;
+    space->buckets = arena_push_array_zero(&space->cell_arena, struct space_cell_bucket, space->num_buckets);
+
+    return space;
+}
+
+void space_release(struct space *space)
+{
+    arena_release(&space->cell_arena);
+    arena_release(&space->client_arena);
+}
+
+struct space *space_from_client(struct space_client *client)
+{
+    if (client->valid) {
+        u64 first_client_addr = (u64)(client - client->handle.idx);
+        struct space *space = (struct space *)(first_client_addr - SPACE_CLIENTS_OFFSET);
+        ASSERT(space->clients == (struct space_client *)first_client_addr);
+        return space;
+    } else {
+        return space_nil();
+    }
+}
+
+/* ========================== *
+ * Cell
+ * ========================== */
+
+INTERNAL struct v2i32 world_to_cell_coords(f32 cell_size, struct v2 world_pos)
+{
+    f32 x = world_pos.x;
+    f32 y = world_pos.y;
+    x = (x + ((x >= 0) - (x < 0)) * cell_size) / cell_size;
+    y = (y + ((y >= 0) - (y < 0)) * cell_size) / cell_size;
+    return V2I32((i32)x, (i32)y);
+}
+
+INTERNAL i32 cell_coords_to_bucket_index(struct space *space, struct v2i32 cell_pos)
+{
+    u32 num_buckets_sqrt = space->num_buckets_sqrt;
+
+    i32 index_x = cell_pos.x;
+    i32 index_y = cell_pos.y;
+    /* Offset cell index by -1 since cell pos of 0 is invalid */
+    index_x -= (index_x >= 0);
+    index_y -= (index_y >= 0);
+    /* Un-mirror coords to prevent collisions between cells near the axes. (e.g. <3, 1> & <3, -1> should not collide) */
+    index_x += (index_x < 0) * (num_buckets_sqrt * ((index_x / -num_buckets_sqrt) + 1));
+    index_y += (index_y < 0) * (num_buckets_sqrt * ((index_y / -num_buckets_sqrt) + 1));
+
+    i32 bucket_index = (index_x % num_buckets_sqrt) + (index_y % num_buckets_sqrt) * num_buckets_sqrt;
+    ASSERT(bucket_index >= 0 && bucket_index < (i32)space->num_buckets);
+
+    return bucket_index;
+}
+
+struct space_cell *space_get_cell(struct space *space, struct v2i32 cell_pos)
+{
+    i32 bucket_index = cell_coords_to_bucket_index(space, cell_pos);
+    struct space_cell_bucket *bucket = &space->buckets[bucket_index];
+    struct space_cell *res = space_cell_nil();
+    for (struct space_cell *n = bucket->first_cell; n; n = n->next_in_bucket) {
+        if (v2i32_eq(n->pos, cell_pos)) {
+            res = n;
+            break;
+        }
+    }
+    return res;
+}
+
+INTERNAL void space_cell_node_alloc(struct v2i32 cell_pos, struct space_client *client)
+{
+    struct space *space = space_from_client(client);
+    i32 bucket_index = cell_coords_to_bucket_index(space, cell_pos);
+    struct space_cell_bucket *bucket = &space->buckets[bucket_index];
+
+    /* Find existing cell */
+    struct space_cell *cell = NULL;
+    for (struct space_cell *n = bucket->first_cell; n; n = n->next_in_bucket) {
+        if (v2i32_eq(n->pos, cell_pos)) {
+            cell = n;
+            break;
+        }
+    }
+
+    /* Allocate new cell if necessary */
+    if (!cell) {
+        if (space->first_free_cell) {
+            cell = space->first_free_cell;
+            space->first_free_cell = cell->next_free;
+        } else {
+            cell = arena_push(&space->cell_arena, struct space_cell);
+        }
+        MEMZERO_STRUCT(cell);
+        if (bucket->last_cell) {
+            bucket->last_cell->next_in_bucket = cell;
+            cell->prev_in_bucket = bucket->last_cell;
+        } else {
+            bucket->first_cell = cell;
+        }
+        bucket->last_cell = cell;
+        cell->pos = cell_pos;
+        cell->bucket = bucket;
+        cell->valid = true;
+    }
+
+    /* Allocate node */
+    struct space_cell_node *node;
+    {
+        if (space->first_free_cell_node) {
+            node = space->first_free_cell_node;
+            space->first_free_cell_node = node->next_free;
+        } else {
+            node = arena_push(&space->cell_arena, struct space_cell_node);
+        }
+        MEMZERO_STRUCT(node);
+    }
+
+    /* Insert into cell list */
+    node->cell = cell;
+    if (cell->last_node) {
+        cell->last_node->next_in_cell = node;
+        node->prev_in_cell = cell->last_node;
+    } else {
+        cell->first_node = node;
+    }
+    cell->last_node = node;
+
+    /* Insert into client list */
+    node->client = client;
+    if (client->last_node) {
+        client->last_node->next_in_client = node;
+        node->prev_in_client = client->last_node;
+    } else {
+        client->first_node = node;
+    }
+    client->last_node = node;
+}
+
+INTERNAL void space_cell_node_release(struct space_cell_node *n)
+{
+    struct space_cell *cell = n->cell;
+    struct space_client *client = n->client;
+    struct space *space = space_from_client(client);
+    struct space_cell_bucket *bucket = cell->bucket;
+
+    /* Remove from client list */
+    {
+        struct space_cell_node *prev = n->prev_in_client;
+        struct space_cell_node *next = n->next_in_client;
+        if (prev) {
+            prev->next_in_client = next;
+        } else {
+            client->first_node = next;
+        }
+        if (next) {
+            next->prev_in_client = prev;
+        } else {
+            client->last_node = prev;
+        }
+    }
+
+    /* Remove from cell list */
+    {
+        struct space_cell_node *prev = n->prev_in_cell;
+        struct space_cell_node *next = n->next_in_cell;
+        if (prev) {
+            prev->next_in_cell = next;
+        } else {
+            cell->first_node = next;
+        }
+        if (next) {
+            next->prev_in_cell = prev;
+        } else {
+            cell->last_node = prev;
+        }
+    }
+
+    /* If cell is now empty, release it */
+    if (!cell->first_node && !cell->last_node) {
+        /* Remove from bucket */
+        struct space_cell *prev = cell->prev_in_bucket;
+        struct space_cell *next = cell->next_in_bucket;
+        if (prev) {
+            prev->next_in_bucket = next;
+        } else {
+            bucket->first_cell = next;
+        }
+        if (next) {
+            next->prev_in_bucket = prev;
+        } else {
+            bucket->last_cell = prev;
+        }
+        cell->valid = false;
+
+        /* Insert into free list */
+        cell->next_free = space->first_free_cell;
+        space->first_free_cell = cell;
+    }
+
+    /* Insert into free list */
+    n->next_free = space->first_free_cell_node;
+    space->first_free_cell_node = n;
+}
+
+/* ========================== *
+ * Client
+ * ========================== */
+
+struct space_client *space_client_from_handle(struct space *space, struct space_client_handle handle)
+{
+    struct space_client *client = space_client_nil();
+
+    if (handle.gen > 0 && handle.idx < space->num_clients_reserved) {
+        struct space_client *tmp = &space->clients[handle.idx];
+        if (tmp->handle.gen == handle.gen) {
+            client = tmp;
+        }
+    }
+
+    return client;
+}
+
+struct space_client *space_client_alloc(struct space *space, struct entity_handle entity)
+{
+    struct space_client *client = NULL;
+    struct space_client_handle handle = ZI;
+    if (space->first_free_client) {
+        client = space->first_free_client;
+        space->first_free_client = client->next_free;
+        handle = client->handle;
+    } else {
+        client = arena_push(&space->client_arena, struct space_client);
+        handle.idx = space->num_clients_reserved;
+        handle.gen = 1;
+        ++space->num_clients_reserved;
+    }
+    MEMZERO_STRUCT(client);
+    client->valid = true;
+    client->handle = handle;
+    client->ent = entity;
+    return client;
+}
+
+void space_client_release(struct space_client *client)
+{
+    /* Release nodes */
+    struct space_cell_node *n = client->first_node;
+    while (n) {
+        struct space_cell_node *next = n->next_in_client;
+        /* TODO: More efficient batch release that doesn't care about maintaining client list */
+        space_cell_node_release(n);
+        n = next;
+    }
+
+    struct space *space = space_from_client(client);
+    client->next_free = space->first_free_client;
+    client->valid = false;
+    ++client->handle.gen;
+    space->first_free_client = client;
+}
+
+void space_client_update_aabb(struct space_client *client, struct aabb new_aabb)
+{
+
+    struct space *space = space_from_client(client);
+    f32 cell_size = space->cell_size;
+
+    struct aabb old_aabb = client->aabb;
+    struct v2i32 old_cell_p0 = world_to_cell_coords(cell_size, old_aabb.p0);
+    struct v2i32 old_cell_p1 = world_to_cell_coords(cell_size, old_aabb.p1);
+    struct v2i32 new_cell_p0 = world_to_cell_coords(cell_size, new_aabb.p0);
+    struct v2i32 new_cell_p1 = world_to_cell_coords(cell_size, new_aabb.p1);
+
+    /* Release outdated nodes */
+    struct space_cell_node *n = client->first_node;
+    while (n) {
+        struct space_cell *cell = n->cell;
+        struct v2i32 cell_pos = cell->pos;
+        if (cell_pos.x < new_cell_p0.x || cell_pos.x > new_cell_p1.x || cell_pos.y < new_cell_p0.y || cell_pos.y > new_cell_p1.y) {
+            /* Cell is outside of new AABB */
+            struct space_cell_node *next = n->next_in_client;
+            space_cell_node_release(n);
+            n = next;
+        } else {
+            n = n->next_in_client;
+        }
+    }
+
+    /* Insert new nodes */
+    for (i32 y = new_cell_p0.y; y <= new_cell_p1.y; ++y) {
+        for (i32 x = new_cell_p0.x; x <= new_cell_p1.x; ++x) {
+            if (x < old_cell_p0.x || x > old_cell_p1.x || y < old_cell_p0.y || y > old_cell_p1.y) {
+                /* Cell is outside of old AABB */
+                space_cell_node_alloc(V2I32(x, y), client);
+            }
+        }
+    }
+
+    client->aabb = new_aabb;
+}
+
+/* ========================== *
+ * Iter
+ * ========================== */
+
+struct space_iter space_iter_begin_aabb(struct space *space, struct aabb aabb)
+{
+    struct space_iter iter = ZI;
+    f32 cell_size = space->cell_size;
+
+    iter.space = space;
+    iter.cell_start = world_to_cell_coords(cell_size, aabb.p0);
+    iter.cell_end = world_to_cell_coords(cell_size, aabb.p1);
+    if (iter.cell_start.x > iter.cell_end.x || iter.cell_start.y > iter.cell_end.y) {
+        /* Swap cell_start & cell_end */
+        struct v2i32 tmp = iter.cell_start;
+        iter.cell_start = iter.cell_end;
+        iter.cell_end = tmp;
+    }
+
+    iter.aabb = aabb;
+    iter.cell_cur = iter.cell_start;
+    iter.cell_cur.x -= 1;
+    iter.cell_cur.y -= 1;
+
+    return iter;
+}
+
+struct space_client *space_iter_next(struct space_iter *iter)
+{
+    struct space *space = iter->space;
+    struct aabb iter_aabb = iter->aabb;
+    struct v2i32 cell_start = iter->cell_start;
+    struct v2i32 cell_end = iter->cell_end;
+    struct v2i32 cell_cur = iter->cell_cur;
+
+    struct space_cell_node *next_node = NULL;
+    if (cell_cur.x >= cell_start.x && cell_cur.x <= cell_end.x && cell_cur.y >= cell_start.y && cell_cur.y <= cell_end.y) {
+        /* Started */
+        ASSERT(iter->prev != NULL);
+        next_node = iter->prev->next_in_cell;
+    }  else if (cell_cur.x < cell_start.x || cell_cur.y < cell_start.y) {
+        /* Unstarted */
+        iter->cell_cur = iter->cell_start;
+        iter->cell_cur.x -= 1;
+        iter->cell_cur.y -= 1;
+    } else if (cell_cur.x > cell_end.x || cell_cur.y > cell_end.y) {
+        /* Ended */
+        return NULL;
+    }
+
+    while (true) {
+        if (next_node) {
+            struct space_client *client = next_node->client;
+            struct aabb client_aabb = client->aabb;
+            if (collider_test_aabb(client_aabb, iter_aabb)) {
+                break;
+            } else {
+                next_node = next_node->next_in_cell;
+            }
+        } else {
+            /* Reached end of cell, find next cell */
+            b32 nextx = (cell_cur.x + 1) <= cell_end.x;
+            b32 nexty = (cell_cur.y + 1) <= cell_end.y;
+            if (nextx || nexty) {
+                cell_cur.x += 1 * nextx;
+                cell_cur.y += 1 * nexty;
+                iter->cell_cur = cell_cur;
+                struct space_cell *cell = space_get_cell(space, cell_cur);
+                next_node = cell->first_node;
+            } else {
+                /* Reached end of iter */
+                cell_cur.x += 1;
+                cell_cur.y += 1;
+                iter->cell_cur = cell_cur;
+                break;
+            }
+        }
+    }
+    iter->prev = next_node;
+
+    return next_node ? next_node->client : NULL;
+}

src/space.h

@@ -0,0 +1,134 @@
+#ifndef SPACE_H
+#define SPACE_H
+
+struct space_cell_bucket;
+
+struct space_client {
+    b32 valid;
+    struct space_client_handle handle;
+
+    struct space_cell_node *first_node;
+    struct space_cell_node *last_node;
+
+    struct aabb aabb;
+    struct entity_handle ent;
+
+    struct space_client *next_free;
+};
+
+/* Links a cell to a client.
+ * Acts as both a list of clients contained by cell & a list of cells containing client. */
+struct space_cell_node {
+    struct space_client *client;
+    struct space_cell *cell;
+
+    /* For list of all clients contained by cell */
+    struct space_cell_node *prev_in_cell;
+    struct space_cell_node *next_in_cell;
+
+    /* For list of all cells containing client */
+    struct space_cell_node *prev_in_client;
+    struct space_cell_node *next_in_client;
+
+    struct space_cell_node *next_free;
+};
+
+struct space_cell {
+    b32 valid;
+    struct v2i32 pos;
+
+    struct space_cell_node *first_node;
+    struct space_cell_node *last_node;
+
+    struct space_cell_bucket *bucket;
+    struct space_cell *prev_in_bucket;
+    struct space_cell *next_in_bucket;
+
+    struct space_cell *next_free;
+};
+
+struct space_cell_bucket {
+    struct space_cell *first_cell;
+    struct space_cell *last_cell;
+};
+
+struct space {
+    b32 valid;
+    f32 cell_size;
+
+    struct arena cell_arena;
+    struct space_cell_bucket *buckets;
+    u64 num_buckets;
+    u64 num_buckets_sqrt;
+    struct space_cell *first_free_cell;
+    struct space_cell_node *first_free_cell_node;
+
+    struct arena client_arena;
+    u64 num_clients_reserved;
+    struct space_client *clients;
+    struct space_client *first_free_client;
+};
+
+struct space_iter {
+    struct aabb aabb;
+    struct space *space;
+    struct v2i32 cell_start;
+    struct v2i32 cell_end;
+    struct v2i32 cell_cur;
+    struct space_cell_node *prev;
+};
+
+/* ========================== *
+ * Nil
+ * ========================== */
+
+INLINE struct space_client *space_client_nil(void)
+{
+    extern READONLY struct space_client _g_space_client_nil;
+    return &_g_space_client_nil;
+}
+
+INLINE struct space_cell *space_cell_nil(void)
+{
+    extern READONLY struct space_cell _g_space_cell_nil;
+    return &_g_space_cell_nil;
+}
+
+INLINE struct space *space_nil(void)
+{
+    extern READONLY struct space _g_space_nil;
+    return &_g_space_nil;
+}
+
+/* ========================== *
+ * Space
+ * ========================== */
+
+struct space *space_alloc(f32 cell_size, u32 num_buckets_sqrt);
+void space_release(struct space *space);
+struct space *space_from_client(struct space_client *client);
+
+/* ========================== *
+ * Cell
+ * ========================== */
+
+struct space_cell *space_get_cell(struct space *space, struct v2i32 cell_pos);
+
+/* ========================== *
+ * Client
+ * ========================== */
+
+struct space_client *space_client_from_handle(struct space *space, struct space_client_handle handle);
+struct space_client *space_client_alloc(struct space *space, struct entity_handle entity);
+void space_client_release(struct space_client *client);
+void space_client_update_aabb(struct space_client *client, struct aabb new_aabb);
+
+/* ========================== *
+ * Iter
+ * ========================== */
+
+struct space_iter space_iter_begin_aabb(struct space *space, struct aabb aabb);
+struct space_client *space_iter_next(struct space_iter *iter);
+#define space_iter_end(i)
+
+#endif

src/user.c

@@ -1008,7 +1008,7 @@ INTERNAL void user_update(void)
 
                 /* Draw AABB */
                 if (ent->local_collider.count > 0) {
-                    struct aabb aabb = collider_get_aabb(&ent->local_collider, xf);
+                    struct aabb aabb = collider_aabb_from_collider(&ent->local_collider, xf);
                     f32 thickness = 1;
                     u32 color = RGBA_32_F(1, 0, 1, 0.5);
                     struct quad quad = quad_from_aabb(aabb);