494 lines
13 KiB
C
494 lines
13 KiB
C
/* FIXME: Default space entry & cell pointers to nil */
|
|
|
|
Readonly PP_SpaceEntry PP_nil_space_entry = ZI;
|
|
Readonly PP_SpaceCell PP_nil_space_cell = ZI;
|
|
Readonly PP_Space PP_nil_space = ZI;
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Space
|
|
|
|
/* NOTE:
|
|
* The number of bins determines how often tiles will collide in the spatial hash.
|
|
* For example, at `num_bins_sqrt` = 256 (65536 bins), tiles <1, 1>, <1, 257>, and <257, 257> will collide. */
|
|
PP_Space *PP_AcquireSpace(f32 cell_size, u32 num_bins_sqrt)
|
|
{
|
|
PP_Space *space;
|
|
{
|
|
Arena *arena = AcquireArena(Gibi(64));
|
|
space = PushStruct(arena, PP_Space);
|
|
space->entry_arena = arena;
|
|
}
|
|
|
|
space->valid = 1;
|
|
space->entries = PushDry(space->entry_arena, PP_SpaceEntry);
|
|
|
|
space->cell_arena = AcquireArena(Gibi(64));
|
|
space->cell_size = cell_size;
|
|
space->num_bins = num_bins_sqrt * num_bins_sqrt;
|
|
space->num_bins_sqrt = num_bins_sqrt;
|
|
space->bins = PushStructs(space->cell_arena, PP_SpaceCellBin, space->num_bins);
|
|
|
|
return space;
|
|
}
|
|
|
|
void PP_ReleaseSpace(PP_Space *space)
|
|
{
|
|
ReleaseArena(space->cell_arena);
|
|
ReleaseArena(space->entry_arena);
|
|
}
|
|
|
|
void PP_ResetSpace(PP_Space *space)
|
|
{
|
|
PopTo(space->entry_arena, (u64)space->entries - (u64)ArenaBase(space->entry_arena));
|
|
ResetArena(space->cell_arena);
|
|
space->bins = PushStructs(space->cell_arena, PP_SpaceCellBin, space->num_bins);
|
|
space->num_entries_reserved = 0;
|
|
space->first_free_cell = 0;
|
|
space->first_free_cell_node = 0;
|
|
space->first_free_entry = 0;
|
|
}
|
|
|
|
PP_Space *PP_SpaceFromEntry(PP_SpaceEntry *entry)
|
|
{
|
|
if (entry->valid)
|
|
{
|
|
u64 first_entry_addr = (u64)(entry - entry->handle.idx);
|
|
PP_Space *space = (PP_Space *)(first_entry_addr - PP_SpaceEntriesOffset);
|
|
Assert(space->entries == (PP_SpaceEntry *)first_entry_addr);
|
|
return space;
|
|
}
|
|
else
|
|
{
|
|
return PP_NilSpace();
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Cell
|
|
|
|
Vec2I32 PP_SpaceCellCoordsFromWorldCoords(f32 cell_size, Vec2 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 VEC2I32((i32)x, (i32)y);
|
|
}
|
|
|
|
i32 PP_SpaceBinIndexFromCellCoords(PP_Space *space, Vec2I32 cell_pos)
|
|
{
|
|
i32 num_bins_sqrt = space->num_bins_sqrt;
|
|
|
|
/* Cell pos of 0 is not valid and will be converted to -1 */
|
|
Assert(cell_pos.x != 0 && cell_pos.y != 0);
|
|
|
|
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_bins_sqrt * ((index_x / -num_bins_sqrt) + 1));
|
|
index_y += (index_y < 0) * (num_bins_sqrt * ((index_y / -num_bins_sqrt) + 1));
|
|
|
|
i32 bin_index = (index_x % num_bins_sqrt) + (index_y % num_bins_sqrt) * num_bins_sqrt;
|
|
Assert(bin_index >= 0 && bin_index < (i32)space->num_bins);
|
|
|
|
return bin_index;
|
|
}
|
|
|
|
PP_SpaceCell *PP_SpaceCellFromCellPos(PP_Space *space, Vec2I32 cell_pos)
|
|
{
|
|
i32 bin_index = PP_SpaceBinIndexFromCellCoords(space, cell_pos);
|
|
PP_SpaceCellBin *bin = &space->bins[bin_index];
|
|
PP_SpaceCell *result = PP_NilSpaceCell();
|
|
for (PP_SpaceCell *n = bin->first_cell; n; n = n->next_in_bin)
|
|
{
|
|
if (EqVec2I32(n->pos, cell_pos))
|
|
{
|
|
result = n;
|
|
break;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void PP_AcquireSpaceCellNode(Vec2I32 cell_pos, PP_SpaceEntry *entry)
|
|
{
|
|
PP_Space *space = PP_SpaceFromEntry(entry);
|
|
i32 bin_index = PP_SpaceBinIndexFromCellCoords(space, cell_pos);
|
|
PP_SpaceCellBin *bin = &space->bins[bin_index];
|
|
|
|
/* Find existing cell */
|
|
PP_SpaceCell *cell = 0;
|
|
for (PP_SpaceCell *n = bin->first_cell; n; n = n->next_in_bin)
|
|
{
|
|
if (EqVec2I32(n->pos, cell_pos))
|
|
{
|
|
cell = n;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* Acquire 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 = PushStructNoZero(space->cell_arena, PP_SpaceCell);
|
|
}
|
|
ZeroStruct(cell);
|
|
if (bin->last_cell)
|
|
{
|
|
bin->last_cell->next_in_bin = cell;
|
|
cell->prev_in_bin = bin->last_cell;
|
|
}
|
|
else
|
|
{
|
|
bin->first_cell = cell;
|
|
}
|
|
bin->last_cell = cell;
|
|
cell->pos = cell_pos;
|
|
cell->bin = bin;
|
|
cell->valid = 1;
|
|
}
|
|
|
|
/* Acquire node */
|
|
PP_SpaceCellNode *node;
|
|
{
|
|
if (space->first_free_cell_node)
|
|
{
|
|
node = space->first_free_cell_node;
|
|
space->first_free_cell_node = node->next_free;
|
|
}
|
|
else
|
|
{
|
|
node = PushStructNoZero(space->cell_arena, PP_SpaceCellNode);
|
|
}
|
|
ZeroStruct(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 entry list */
|
|
node->entry = entry;
|
|
if (entry->last_node)
|
|
{
|
|
entry->last_node->next_in_entry = node;
|
|
node->prev_in_entry = entry->last_node;
|
|
}
|
|
else
|
|
{
|
|
entry->first_node = node;
|
|
}
|
|
entry->last_node = node;
|
|
}
|
|
|
|
void PP_ReleaseSpaceCellNode(PP_SpaceCellNode *n)
|
|
{
|
|
PP_SpaceCell *cell = n->cell;
|
|
PP_SpaceEntry *entry = n->entry;
|
|
PP_Space *space = PP_SpaceFromEntry(entry);
|
|
PP_SpaceCellBin *bin = cell->bin;
|
|
|
|
/* Remove from entry list */
|
|
{
|
|
PP_SpaceCellNode *prev = n->prev_in_entry;
|
|
PP_SpaceCellNode *next = n->next_in_entry;
|
|
if (prev)
|
|
{
|
|
prev->next_in_entry = next;
|
|
}
|
|
else
|
|
{
|
|
entry->first_node = next;
|
|
}
|
|
if (next)
|
|
{
|
|
next->prev_in_entry = prev;
|
|
}
|
|
else
|
|
{
|
|
entry->last_node = prev;
|
|
}
|
|
}
|
|
|
|
/* Remove from cell list */
|
|
{
|
|
PP_SpaceCellNode *prev = n->prev_in_cell;
|
|
PP_SpaceCellNode *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 bin */
|
|
PP_SpaceCell *prev = cell->prev_in_bin;
|
|
PP_SpaceCell *next = cell->next_in_bin;
|
|
if (prev)
|
|
{
|
|
prev->next_in_bin = next;
|
|
}
|
|
else
|
|
{
|
|
bin->first_cell = next;
|
|
}
|
|
if (next)
|
|
{
|
|
next->prev_in_bin = prev;
|
|
}
|
|
else
|
|
{
|
|
bin->last_cell = prev;
|
|
}
|
|
cell->valid = 0;
|
|
|
|
/* Insert cell into free list */
|
|
cell->next_free = space->first_free_cell;
|
|
space->first_free_cell = cell;
|
|
}
|
|
|
|
/* Insert node into free list */
|
|
n->next_free = space->first_free_cell_node;
|
|
space->first_free_cell_node = n;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Entry
|
|
|
|
PP_SpaceEntry *PP_SpaceEntryFromKey(PP_Space *space, PP_SpaceEntryKey handle)
|
|
{
|
|
PP_SpaceEntry *entry = PP_NilSpaceEntry();
|
|
|
|
if (handle.gen > 0 && handle.idx < space->num_entries_reserved)
|
|
{
|
|
PP_SpaceEntry *tmp = &space->entries[handle.idx];
|
|
if (tmp->handle.gen == handle.gen)
|
|
{
|
|
entry = tmp;
|
|
}
|
|
}
|
|
|
|
return entry;
|
|
}
|
|
|
|
PP_SpaceEntry *PP_AcquireSpaceEntry(PP_Space *space, PP_EntKey ent)
|
|
{
|
|
PP_SpaceEntry *entry = 0;
|
|
PP_SpaceEntryKey handle = ZI;
|
|
if (space->first_free_entry)
|
|
{
|
|
entry = space->first_free_entry;
|
|
space->first_free_entry = entry->next_free;
|
|
handle = entry->handle;
|
|
}
|
|
else
|
|
{
|
|
entry = PushStructNoZero(space->entry_arena, PP_SpaceEntry);
|
|
handle.idx = space->num_entries_reserved;
|
|
handle.gen = 1;
|
|
++space->num_entries_reserved;
|
|
}
|
|
ZeroStruct(entry);
|
|
entry->valid = 1;
|
|
entry->handle = handle;
|
|
entry->ent = ent;
|
|
return entry;
|
|
}
|
|
|
|
void PP_ReleaseSpaceEntry(PP_SpaceEntry *entry)
|
|
{
|
|
/* Release nodes */
|
|
PP_SpaceCellNode *n = entry->first_node;
|
|
while (n)
|
|
{
|
|
PP_SpaceCellNode *next = n->next_in_entry;
|
|
/* TODO: More efficient batch release that doesn't care about maintaining entry list */
|
|
PP_ReleaseSpaceCellNode(n);
|
|
n = next;
|
|
}
|
|
|
|
PP_Space *space = PP_SpaceFromEntry(entry);
|
|
entry->next_free = space->first_free_entry;
|
|
entry->valid = 0;
|
|
++entry->handle.gen;
|
|
space->first_free_entry = entry;
|
|
}
|
|
|
|
void PP_UpdateSpaceEntryAabb(PP_SpaceEntry *entry, Aabb new_aabb)
|
|
{
|
|
PP_Space *space = PP_SpaceFromEntry(entry);
|
|
f32 cell_size = space->cell_size;
|
|
|
|
Vec2I32 old_cell_p0 = VEC2I32(0, 0);
|
|
Vec2I32 old_cell_p1 = VEC2I32(0, 0);
|
|
if (entry->first_node)
|
|
{
|
|
Aabb old_aabb = entry->aabb;
|
|
old_cell_p0 = PP_SpaceCellCoordsFromWorldCoords(cell_size, old_aabb.p0);
|
|
old_cell_p1 = PP_SpaceCellCoordsFromWorldCoords(cell_size, old_aabb.p1);
|
|
}
|
|
|
|
Vec2I32 new_cell_p0 = PP_SpaceCellCoordsFromWorldCoords(cell_size, new_aabb.p0);
|
|
Vec2I32 new_cell_p1 = PP_SpaceCellCoordsFromWorldCoords(cell_size, new_aabb.p1);
|
|
|
|
/* Release outdated nodes */
|
|
PP_SpaceCellNode *n = entry->first_node;
|
|
while (n)
|
|
{
|
|
PP_SpaceCell *cell = n->cell;
|
|
Vec2I32 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 */
|
|
PP_SpaceCellNode *next = n->next_in_entry;
|
|
PP_ReleaseSpaceCellNode(n);
|
|
n = next;
|
|
}
|
|
else
|
|
{
|
|
n = n->next_in_entry;
|
|
}
|
|
}
|
|
|
|
/* 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 != 0 && y != 0 && (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 */
|
|
PP_AcquireSpaceCellNode(VEC2I32(x, y), entry);
|
|
}
|
|
}
|
|
}
|
|
|
|
entry->aabb = new_aabb;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Iter
|
|
|
|
PP_SpaceIter PP_BeginSpaceIterAabb(PP_Space *space, Aabb aabb)
|
|
{
|
|
PP_SpaceIter iter = ZI;
|
|
f32 cell_size = space->cell_size;
|
|
|
|
iter.space = space;
|
|
iter.cell_start = PP_SpaceCellCoordsFromWorldCoords(cell_size, aabb.p0);
|
|
iter.cell_end = PP_SpaceCellCoordsFromWorldCoords(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 */
|
|
Vec2I32 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;
|
|
}
|
|
|
|
PP_SpaceEntry *PP_NextSpaceIterAabb(PP_SpaceIter *iter)
|
|
{
|
|
PP_Space *space = iter->space;
|
|
Aabb iter_aabb = iter->aabb;
|
|
Vec2I32 cell_start = iter->cell_start;
|
|
Vec2I32 cell_end = iter->cell_end;
|
|
Vec2I32 cell_cur = iter->cell_cur;
|
|
i32 span = cell_end.x - cell_start.x;
|
|
|
|
PP_SpaceCellNode *next_node = 0;
|
|
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 != 0);
|
|
next_node = iter->prev->next_in_cell;
|
|
}
|
|
else if (cell_cur.x > cell_end.x || cell_cur.y > cell_end.y)
|
|
{
|
|
/* Ended */
|
|
return 0;
|
|
}
|
|
|
|
for (;;)
|
|
{
|
|
if (next_node)
|
|
{
|
|
PP_SpaceEntry *entry = next_node->entry;
|
|
Aabb entry_aabb = entry->aabb;
|
|
if (CLD_TestAabb(entry_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 = (nextx == 0) && ((cell_cur.y + 1) <= cell_end.y);
|
|
if (nextx || nexty)
|
|
{
|
|
cell_cur.x += nextx - (span * nexty);
|
|
cell_cur.y += nexty;
|
|
cell_cur.x += (cell_cur.x == 0);
|
|
cell_cur.y += (cell_cur.y == 0);
|
|
PP_SpaceCell *cell = PP_SpaceCellFromCellPos(space, cell_cur);
|
|
next_node = cell->first_node;
|
|
}
|
|
else
|
|
{
|
|
/* Reached end of iter */
|
|
cell_cur.x += 1;
|
|
cell_cur.y += 1;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
iter->prev = next_node;
|
|
iter->cell_cur = cell_cur;
|
|
|
|
return next_node ? next_node->entry : 0;
|
|
}
|