diff --git a/src/collider.c b/src/collider.c index 021058d3..a9163f9b 100644 --- a/src/collider.c +++ b/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; diff --git a/src/collider.h b/src/collider.h index 8b7523da..6d277932 100644 --- a/src/collider.h +++ b/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); diff --git a/src/common.h b/src/common.h index 21c52d74..922483e0 100644 --- a/src/common.h +++ b/src/common.h @@ -496,6 +496,11 @@ struct entity_handle { u64 gen; }; +struct space_client_handle { + u64 idx; + u64 gen; +}; + /* ========================== * * Tag structs * ========================== */ diff --git a/src/config.h b/src/config.h index e9118300..7764f31e 100644 --- a/src/config.h +++ b/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 diff --git a/src/entity.c b/src/entity.c index 3f75ac9d..b1e2888d 100644 --- a/src/entity.c +++ b/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 = { - 0 -}; +READONLY struct entity_store _g_entity_store_nil = ZI; /* 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); diff --git a/src/entity.h b/src/entity.h index 376a36fb..72bc0772 100644 --- a/src/entity.h +++ b/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); diff --git a/src/game.c b/src/game.c index 7217ef7f..0087dcc2 100644 --- a/src/game.c +++ b/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); } /* ========================== * diff --git a/src/phys.c b/src/phys.c index f2f325ef..4bda5d4c 100644 --- a/src/phys.c +++ b/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 entity *check1 = &store->entities[check1_index]; + struct space_iter iter = space_iter_begin_aabb(space, aabb); + 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; } diff --git a/src/phys.h b/src/phys.h index 56b2beaa..46f8f21c 100644 --- a/src/phys.h +++ b/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); -void phys_prepare_contacts(struct phys_ctx *ctx); +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, 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 diff --git a/src/space.c b/src/space.c new file mode 100644 index 00000000..23471cd5 --- /dev/null +++ b/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; +} diff --git a/src/space.h b/src/space.h new file mode 100644 index 00000000..a9dcfc81 --- /dev/null +++ b/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 diff --git a/src/user.c b/src/user.c index fb39b162..d7bbbd2c 100644 --- a/src/user.c +++ b/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);