2d spatial hashing

spatial_hash.c

/**
 * 2D spatial hash implementation.
 *
 * The spatial hash is really good at representing grids in games. It only stores elements
 * of the grid where items are inserted into the spacial hash, making it a memory-efficient
 * option for grid games with lots of empty space between tiles/entities.
 *
 * Spatial hash queries are extremely fast for small queries covering low surface areas.
 *
 * Spatial hashes breakdown when queries cover large surface areas. This is because each
 * query has to perform a hashtable lookup for each grid cell overlapping the query. This could
 * be potentially sped up by adding in kind of tree structure, or multi-level grid with
 * varying resolutions, but at the cost of added implementation complexity and constant-time
 * overhead on each query. At this point a different kind of data structure would be better.
 */

#pragma once

typedef struct SH_Grid SH_Grid;

SH_Grid* sh_make(int grid_cell_size, int initial_pool_size);
void sh_destroy(SH_Grid* grid);
void sh_add(SH_Grid* grid, uint64_t id, CF_Aabb bb);
CF_Aabb sh_get(SH_Grid* grid, uint64_t id); // Slow. Mainly intended for debugging.
void sh_update(SH_Grid* grid, uint64_t id, CF_Aabb bb);
void sh_del(SH_Grid* grid, uint64_t id);
void sh_clear(SH_Grid* grid);
htbl uint64_t* sh_query(SH_Grid* grid, CF_Aabb bb);
htbl uint64_t* sh_query_id(SH_Grid* grid, uint64_t id);

//--------------------------------------------------------------------------------------------------
// Implementation.

typedef struct SH_Object
{
	uint64_t id;
	CF_Aabb bb;
	struct SH_Object* next;
} SH_Object;

struct SH_Grid
{
	int grid_size;
	int initial_pool_size;
	htbl SH_Object** cells;
	htbl CF_Aabb* bbs;
	htbl uint64_t* id_query;
	CF_MemoryPool* pool;
};

SH_Grid* sh_make(int grid_cell_size, int initial_pool_size)
{
	SH_Grid* grid = CALLOC(SH_Grid);
	grid->grid_size = grid_cell_size;
	grid->initial_pool_size = initial_pool_size;
	grid->pool = cf_make_memory_pool(sizeof(SH_Object), initial_pool_size, 8);
	return grid;
}

void sh_destroy(SH_Grid* grid)
{
	hfree(grid->cells);
	hfree(grid->bbs);
	hfree(grid->id_query);
	cf_destroy_memory_pool(grid->pool);
	FREE(grid);
}

#define SH_CELL_RANGE(grid, bb, min_x, min_y, max_x, max_y) \
	do { \
		(min_x) = (int)floorf((bb).min.x / (grid)->grid_size); \
		(min_y) = (int)floorf((bb).min.y / (grid)->grid_size); \
		(max_x) = (int)ceilf((bb).max.x / (grid)->grid_size); \
		(max_y) = (int)ceilf((bb).max.y / (grid)->grid_size); \
	} while(0)

#define sh_hash(cell_x, cell_y) ((uint64_t)(cell_x) + (((uint64_t)cell_y) << 32))

void sh_add(SH_Grid* grid, uint64_t id, CF_Aabb bb)
{
	if (hhas(grid->bbs, id)) {
		return;
	}

	int min_x, min_y, max_x, max_y;
	SH_CELL_RANGE(grid, bb, min_x, min_y, max_x, max_y);

	for (int x = min_x; x <= max_x; ++x) {
		for (int y = min_y; y <= max_y; ++y) {
			uint64_t hash = sh_hash(x, y);
			SH_Object* cell = grid->cells ? hfind(grid->cells, hash) : NULL;

			// If the cell doesn't exist, create a new one.
			if (!cell) {
				cell = cf_memory_pool_alloc(grid->pool);
				cell->id = id;
				cell->bb = bb;
				cell->next = NULL;
				hadd(grid->cells, hash, cell);
			} else {
				SH_Object* obj = cf_memory_pool_alloc(grid->pool);
				obj->id = id;
				obj->bb = bb;
				obj->next = cell->next;
				cell->next = obj;
			}
		}
	}

	hadd(grid->bbs, id, bb);
}

CF_Aabb sh_get(SH_Grid* grid, uint64_t id)
{
	return hget(grid->bbs, id);
}

void sh_update(SH_Grid* grid, uint64_t id, CF_Aabb bb)
{
	CF_Aabb old_bb = grid->bbs ? hget(grid->bbs, id) : (CF_Aabb){ 0 };
	int old_min_x, old_min_y, old_max_x, old_max_y;
	int new_min_x, new_min_y, new_max_x, new_max_y;
	SH_CELL_RANGE(grid, old_bb, old_min_x, old_min_y, old_max_x, old_max_y);
	SH_CELL_RANGE(grid, bb, new_min_x, new_min_y, new_max_x, new_max_y);

	// Remove from cells that are no longer overlapping.
	for (int x = old_min_x; x <= old_max_x; ++x) {
		for (int y = old_min_y; y <= old_max_y; ++y) {
			if (x < new_min_x || x > new_max_x || y < new_min_y || y > new_max_y) {
				uint64_t hash = sh_hash(x, y);
				SH_Object* cell = grid->cells ? hget(grid->cells, hash) : NULL;

				SH_Object* prev = NULL;
				SH_Object* curr = cell;

				while (curr) {
					if (curr->id == id) {
						if (prev) {
							prev->next = curr->next;
						} else {
							hadd(grid->cells, hash, curr->next);
						}
						cf_memory_pool_free(grid->pool, curr);
						break;
					}
					prev = curr;
					curr = curr->next;
				}
			}
		}
	}

	// Update the object in cells where it still overlaps.
	for (int x = new_min_x; x <= new_max_x; ++x) {
		for (int y = new_min_y; y <= new_max_y; ++y) {
			if (x >= old_min_x && x <= old_max_x && y >= old_min_y && y <= old_max_y) {
				uint64_t hash = sh_hash(x, y);
				SH_Object* cell = grid->cells ? hget(grid->cells, hash) : NULL;
				while (cell) {
					if (cell->id == id) {
						cell->bb = bb;
						break;
					}
					cell = cell->next;
				}
			}
		}
	}

	// Add to new cells that were not covered by the old bounding box.
	for (int x = new_min_x; x <= new_max_x; ++x) {
		for (int y = new_min_y; y <= new_max_y; ++y) {
			if (x < old_min_x || x > old_max_x || y < old_min_y || y > old_max_y) {
				uint64_t hash = sh_hash(x, y);
				SH_Object* cell = grid->cells ? hget(grid->cells, hash) : NULL;

				// If the cell doesn't exist, create a new one.
				if (!cell) {
					cell = cf_memory_pool_alloc(grid->pool);
					cell->id = id;
					cell->bb = bb;
					cell->next = NULL;
					hadd(grid->cells, hash, cell);
				} else {
					// Insert the new object at the head of the list.
					SH_Object* obj = cf_memory_pool_alloc(grid->pool);
					obj->id = id;
					obj->bb = bb;
					obj->next = cell->next;
					cell->next = obj;
				}
			}
		}
	}

	hadd(grid->bbs, id, bb);
}

void sh_del(SH_Grid* grid, uint64_t id)
{
	CF_Aabb bb = hget(grid->bbs, id);
	int min_x, min_y, max_x, max_y;
	SH_CELL_RANGE(grid, bb, min_x, min_y, max_x, max_y);

	for (int x = min_x; x <= max_x; ++x) {
		for (int y = min_y; y <= max_y; ++y) {
			uint64_t hash = sh_hash(x, y);
			SH_Object* cell = grid->cells ? hget(grid->cells, hash) : NULL;

			SH_Object* prev = NULL;
			SH_Object* curr = cell;

			while (curr) {
				if (curr->id == id) {
					if (prev) {
						prev->next = curr->next;
					} else {
						hadd(grid->cells, hash, curr->next);
					}

					cf_memory_pool_free(grid->pool, curr);
					break;
				}
				prev = curr;
				curr = curr->next;
			}
		}
	}

	hdel(grid->bbs, id);
}

void sh_clear(SH_Grid* grid)
{
	hclear(grid->cells);
	hclear(grid->bbs);
	hclear(grid->id_query);
	cf_destroy_memory_pool(grid->pool);
	grid->pool = cf_make_memory_pool(sizeof(SH_Object), grid->initial_pool_size, 8);
}

htbl uint64_t* sh_query(SH_Grid* grid, CF_Aabb bb)
{
	int min_x, min_y, max_x, max_y;
	SH_CELL_RANGE(grid, bb, min_x, min_y, max_x, max_y);
	hclear(grid->id_query); // Use hashtable to report only unique ids.

	for (int x = min_x; x <= max_x; ++x) {
		for (int y = min_y; y <= max_y; ++y) {
			uint64_t hash = sh_hash(x, y);
			SH_Object* cell = grid->cells ? hget(grid->cells, hash) : NULL;

			while (cell) {
				if (cf_overlaps(cell->bb, bb)) {
					hadd(grid->id_query, cell->id, cell->id);
				}
				cell = cell->next;
			}
		}
	}

	return grid->id_query;
}

htbl uint64_t* sh_query_id(SH_Grid* grid, uint64_t id)
{
	CF_Aabb bb = hget(grid->bbs, id);
	htbl uint64_t* result = sh_query(grid, bb);
	hdel(result, id); // Don't report this id colliding against itself.
	return result;
}

void sh_unit_test()
{
	SH_Grid* grid = sh_make(17, 5000);

	sh_add(grid, 0, (CF_Aabb){ 0,0, 10,10 });
	sh_add(grid, 1, (CF_Aabb){ 20,5, 30,10 });
	sh_add(grid, 2, (CF_Aabb){ -30,-30, -5,-5 });

	uint64_t* ids = sh_query(grid, (CF_Aabb){ 0,0, 30,30 });
	CF_ASSERT(hcount(ids) == 2);
	for (uint64_t i = 0; i < hcount(ids); ++i) {
		uint64_t id = ids[i];
		CF_ASSERT(id == i);
	}

	sh_del(grid, 0);
	sh_del(grid, 1);

	ids = sh_query(grid, (CF_Aabb){ 0,0, 30,30 });
	CF_ASSERT(hcount(ids) == 0);

	ids = sh_query(grid, (CF_Aabb){ -30,-30, 30,30 });
	CF_ASSERT(hcount(ids) == 1);
	CF_ASSERT(ids[0] == 2);

	sh_update(grid, 2, (CF_Aabb){ -5,-5, 5,5 });
	ids = sh_query(grid, (CF_Aabb){ -10,-10, -6,-6 });
	CF_ASSERT(hcount(ids) == 0);

	sh_destroy(grid);
}