grid.h
#pragma once
#include "../fn_engine/fn_arrayutils.h"
#include "../fn_engine/p_entity.h"
typedef struct
{
p_Entity** entities;// array of aabb pointers to collide with
int entityCount;
}fn_GridSection;
typedef struct
{
p_Entity* entities;
int entityCount;
int** sectionsTouched;
fn_GridSection* sections;
fn_vec3 offset;
int sectionCount;
fn_vec3 cellsize;
int cellcount;
}fn_Grid;
void fn_createGrid(fn_Grid* grid,p_Entity* entities,int entityCount,fn_vec3 cellsize,int cellcount,bool rebuild);
p_Entity** fn_getCollidableGrid(fn_Grid* grid,int entity,int* entities);
void fn_updateEntityGrid(fn_Grid* grid,int entity);
grid.c
#include "fn_grid.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
static int MAX_COLLIDABLE;
static void getAABBPoints(fn_AABB aabb,fn_vec3* points)
{
points[0] = fn_addVec3(aabb.position,fn_createVec3(aabb.hwidth.x,aabb.hwidth.y,aabb.hwidth.z)); // 1 1 1
points[1] = fn_addVec3(aabb.position,fn_createVec3(-aabb.hwidth.x,-aabb.hwidth.y,-aabb.hwidth.z));// -1 -1 -1
points[2] = fn_addVec3(aabb.position,fn_createVec3(-aabb.hwidth.x,aabb.hwidth.y,aabb.hwidth.z)); // -1 1 1
points[3] = fn_addVec3(aabb.position,fn_createVec3(-aabb.hwidth.x,-aabb.hwidth.y,aabb.hwidth.z)); // -1 -1 1
points[4] = fn_addVec3(aabb.position,fn_createVec3(-aabb.hwidth.x,aabb.hwidth.y,-aabb.hwidth.z)); // -1 1 -1
points[5] = fn_addVec3(aabb.position,fn_createVec3(aabb.hwidth.x,aabb.hwidth.y,-aabb.hwidth.z)); // 1 1 -1
points[6] = fn_addVec3(aabb.position,fn_createVec3(aabb.hwidth.x,-aabb.hwidth.y,aabb.hwidth.z)); // 1 -1 1
points[7] = fn_addVec3(aabb.position,fn_createVec3(aabb.hwidth.x,-aabb.hwidth.y,-aabb.hwidth.z)); // 1 -1 -1
}
static bool contains(int* indices,int index)
{
int i;
bool ret = false;
for (i = 1 ;i <= indices[0];i++)
{
if (indices[i] == index)
{
ret = true;
}
}
return ret;
}
int fn_getGridID (int x,int y,int z,int w, int h)
{
return (z * w * h) + (y * w) + x;
}
fn_vec3 fn_getGridPos(int idx,int w,int h)
{
int z = idx / (w * h);
idx -= (z * w * h);
return fn_createVec3(idx % w,idx / w,z);
}
int fn_worldToGrid(fn_Grid* grid,fn_vec3 pos)
{
pos = fn_addVec3(pos,grid->offset);
fn_vec3 gpos = fn_createVec3(floor(pos.x / grid->cellsize.x) ,floor(pos.y / grid->cellsize.y) ,floor(pos.z / grid->cellsize.z) );
return fn_getGridID(gpos.x,gpos.y,gpos.z,grid->cellcount,grid->cellcount);
}
void fn_createGrid(fn_Grid* grid,p_Entity* entities,int entityCount,fn_vec3 cellsize,int cellcount,bool rebuild)
{
int i,j;
if (rebuild)
{
for (i = 0;i < entityCount;i++)
free(grid->sectionsTouched[i]);
free(grid->sectionsTouched);
for ( i =0;i < grid->sectionCount;i++)
{
if (grid->sections[i].entities != NULL)
free(grid->sections[i].entities);
}
free(grid->sections);
}
MAX_COLLIDABLE = 0;
grid->entities = entities;
grid->entityCount = entityCount;
grid->cellsize = cellsize;
grid->cellcount = cellcount;
grid->sectionsTouched = calloc(entityCount,sizeof(int*));
for (i = 0;i < entityCount;i++)
{
grid->sectionsTouched[i] = calloc(9,sizeof(int));
// 0 is sections touched, 1-8 is ID of nth touched section
}
grid->sectionCount = cellcount*cellcount*cellcount;
grid->sections = malloc(sizeof(fn_GridSection)*grid->sectionCount);
fn_vec3 hsize = fn_multVec3s(cellsize,0.5);
fn_vec3 offset = fn_multVec3s(hsize,cellcount);
grid->offset = offset;
#pragma omp parallel for private(i)
for ( i =0;i < grid->sectionCount;i++)
{
grid->sections[i].entityCount = 0;
grid->sections[i].entities = NULL;
}
int count = 0;
// #pragma omp parallel for private(i)
for ( i =0;i < entityCount;i++)
{
fn_vec3 points[8];
getAABBPoints(entities[i].aabb,points);
for (j = 0;j < 8;j++)
{
int gridIndex = fn_worldToGrid(grid,points[j]);
if (gridIndex < 0 || gridIndex >= grid->sectionCount)
{
printf("%s\n","Spatial hash Not big enough!" );
}
if (!contains(grid->sectionsTouched[i],gridIndex))
{
grid->sectionsTouched[i][0]++;
grid->sectionsTouched[i][grid->sectionsTouched[i][0]] = gridIndex;
grid->sections[gridIndex].entityCount++;
if (grid->sections[gridIndex].entityCount > MAX_COLLIDABLE)
MAX_COLLIDABLE = grid->sections[gridIndex].entityCount;
grid->sections[gridIndex].entities = realloc(grid->sections[gridIndex].entities,sizeof(p_Entity*)*grid->sections[gridIndex].entityCount);
grid->sections[gridIndex].entities[grid->sections[gridIndex].entityCount - 1] = &entities[i];
}
}
}
printf("%i\n",count );
}
//get list of entities that an entity in the grid can collide with
p_Entity** fn_getCollidableGrid(fn_Grid* grid,int entity,int* entities)
{
p_Entity** ret = malloc(sizeof(p_Entity*)*MAX_COLLIDABLE*8);
*entities = 0;
int i;
// printf("O %i\n",grid->sectionsTouched[entity][0] );
if ( grid->sectionsTouched[entity][0] != 0)
{
for (i = 1 ;i <= grid->sectionsTouched[entity][0];i++)
{
// printf("I %p\n",grid->sections[grid->sectionsTouched[entity][i]].entities );
if (grid->sections[grid->sectionsTouched[entity][i]].entityCount != 0)
{
memcpy(&ret[*entities],grid->sections[grid->sectionsTouched[entity][i]].entities,grid->sections[grid->sectionsTouched[entity][i]].entityCount*sizeof(p_Entity*));//grid->sectionsTouched[entity][i]
*entities = *entities + grid->sections[grid->sectionsTouched[entity][i]].entityCount;
}
}
}
return ret;
}
void fn_updateEntityGrid(fn_Grid* grid,int entity)
{
int i,j;
fn_vec3 points[8];
getAABBPoints(grid->entities[entity].aabb,points);
for (i = 1 ;i <= grid->sectionsTouched[entity][0];i++)
{
for (j = 0;j < grid->sections[grid->sectionsTouched[entity][i]].entityCount;j++)
{
if (grid->sections[grid->sectionsTouched[entity][i]].entities[j] == &grid->entities[entity])
{
grid->sections[grid->sectionsTouched[entity][i]].entities = (p_Entity**)remove_elementv((void**)grid->sections[grid->sectionsTouched[entity][i]].entities,j,grid->sections[grid->sectionsTouched[entity][i]].entityCount);
grid->sections[grid->sectionsTouched[entity][i]].entityCount--;
break;
}
}
}
grid->sectionsTouched[entity][0] = 0;
for (j = 0;j < 8;j++)
{
int gridIndex = fn_worldToGrid(grid,points[j]);
if (gridIndex < 0 || gridIndex >= grid->sectionCount)
{
printf("%s\n","Spatial hash Not big enough!" );
}
if (!contains(grid->sectionsTouched[entity],gridIndex))
{
grid->sectionsTouched[entity][0]++;
grid->sectionsTouched[entity][grid->sectionsTouched[entity][0]] = gridIndex;
grid->sections[gridIndex].entityCount++;
if (grid->sections[gridIndex].entityCount > MAX_COLLIDABLE)
MAX_COLLIDABLE = grid->sections[gridIndex].entityCount;
grid->sections[gridIndex].entities = realloc(grid->sections[gridIndex].entities,sizeof(p_Entity*)*grid->sections[gridIndex].entityCount);
grid->sections[gridIndex].entities[grid->sections[gridIndex].entityCount - 1] = &grid->entities[entity];
}
}
}
The function remove_elementv removes an element from an array of pointers. The struct p_Entity contains an AABB(Represented as position and half length) and a velocity vector.
If you don't get the sectionsTouched array, it is an array the same size as the array of total entities and each index in it matches up to an index in the array of entities. Within this it stores the count and ID's of all the sections each entity collides with for fast retrieval.
The create grid function is called once and the getCollidableGrid and updateGrid functions are called once for each entity in the array of entities.