broad phase for colliders via aabb spatial hash
This commit is contained in:
parent
c200a618b0
commit
0836eec851
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -496,6 +496,11 @@ struct entity_handle {
|
||||
u64 gen;
|
||||
};
|
||||
|
||||
struct space_client_handle {
|
||||
u64 idx;
|
||||
u64 gen;
|
||||
};
|
||||
|
||||
/* ========================== *
|
||||
* Tag structs
|
||||
* ========================== */
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
18
src/entity.c
18
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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
/* ========================== *
|
||||
|
||||
77
src/phys.c
77
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;
|
||||
}
|
||||
|
||||
17
src/phys.h
17
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
|
||||
|
||||
413
src/space.c
Normal file
413
src/space.c
Normal file
@ -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;
|
||||
}
|
||||
134
src/space.h
Normal file
134
src/space.h
Normal file
@ -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
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user