Skip to content

Instantly share code, notes, and snippets.

@zloedi
Created Oct 5, 2019
Embed
What would you like to do?
Tiny single header flood fill pathfinder
// #define FLOOD_NAV_IMPLEMENTATION before #include "nav_flood.h" to unwrap the implementation
// #define NAV_FOUR_WAY before #include "nav_flood.h" for four-way flood implementation
#define NAV_FREE 0x0fffffff
#define NAV_BLOC 0x40000000
// Internal struct used to hold flood front
typedef struct {
int capMask;
int *buf;
int head;
int tail;
} navFront_t;
#ifndef FLOOD_NAV_IMPLEMENTATION
// Get nice round number for allocators
int NAV_FrontBufSizeInBytes( int gridW, int gridH );
// Assumes buffer has NAV_FrontBufSizeInBytes size
// Call once to set nav front buffer for multiple calls to flood
// Front internals get reset on each flood
navFront_t NAV_CreateFront( int gridW, int gridH, int *buffer );
// Assumes impassable (NAV_BLOC) map borders
// Front can be reused in the same thread
// Create nav map i.e. like this: navMap[i] = map[i] == wall ? NAV_BLOC : NAV_FREE;
void NAV_FloodMap( int origin, int maxRange, navFront_t *front, int gridW, int gridH,
const int *navMap, int *floodMap );
// Call this to get a path between two nodes
int NAV_TracePath( int origin, int target, int gridW, const int *floodMap, int resultSize,
int *ioResult, int *outNumElemsWritten );
#else
#include <mem.h>
static inline int Min( int a, int b ) {
return a < b ? a : b;
}
static inline void NAV_FrontReset( navFront_t *front, int origin, int gridW, int gridH ) {
front->head = 0;
front->tail = 1;
front->buf[front->head] = origin;
}
static inline int NAV_FrontPop( navFront_t *front ) {
int coord = front->buf[front->head & front->capMask];
front->head++;
return coord;
}
static inline void NAV_FrontPush( navFront_t *front, int coord ) {
front->buf[front->tail & front->capMask] = coord;
front->tail++;
}
static inline int NAV_FrontIsEmpty( navFront_t *front ) {
return front->head == front->tail;
}
static inline void NAV_TryExpand( navFront_t *front, int *floodMap, int nbr, int newScore ) {
if ( ( floodMap[nbr] & ~NAV_BLOC ) > newScore ) {
floodMap[nbr] = newScore;
NAV_FrontPush( front, nbr );
}
}
static int NAV_FrontCapacity( int gridW, int gridH ) {
int sz = ( gridW + gridH ) * 2;
int c = sz; // compute the next highest power of 2 of 32-bit c
c--;
c |= c >> 1;
c |= c >> 2;
c |= c >> 4;
c |= c >> 8;
c |= c >> 16;
c++;
return c;
}
void NAV_FloodMap( int origin, int maxRange, navFront_t *front, int gridW, int gridH,
const int *navMap, int *floodMap ) {
const int prims[] = {
// keep them ordered
-1, -gridW, 1, gridW,
#ifndef NAV_FOUR_WAY
-1 - gridW, 1 - gridW, 1 + gridW, -1 + gridW,
#endif
};
int sz = gridW * gridH;
memcpy( floodMap, navMap, sz * sizeof( *floodMap ) );
if ( floodMap[origin] == NAV_BLOC ) {
return;
}
floodMap[origin] = 1;
NAV_FrontReset( front, origin, gridW, gridH );
do {
int c = NAV_FrontPop( front );
int headScore = floodMap[c];
if ( headScore < maxRange * 10 ) {
int newScore = headScore + 10;
int nbrs[8] = {
c + prims[0], c + prims[1], c + prims[2], c + prims[3],
#ifndef NAV_FOUR_WAY
c + prims[4], c + prims[5], c + prims[6], c + prims[7],
#endif
};
NAV_TryExpand( front, floodMap, nbrs[0], newScore );
NAV_TryExpand( front, floodMap, nbrs[1], newScore );
NAV_TryExpand( front, floodMap, nbrs[2], newScore );
NAV_TryExpand( front, floodMap, nbrs[3], newScore );
#ifndef NAV_FOUR_WAY
int diagMask = 0;
diagMask |= ( floodMap[nbrs[0]] >> 30 ) << 0;
diagMask |= ( floodMap[nbrs[1]] >> 30 ) << 1;
diagMask |= ( floodMap[nbrs[2]] >> 30 ) << 2;
diagMask |= ( floodMap[nbrs[3]] >> 30 ) << 3;
newScore = headScore + 14;
if ( diagMask ) {
const int ed[] = {
( 1 << 1 ) | ( 1 << 0 ),
( 1 << 2 ) | ( 1 << 1 ),
( 1 << 3 ) | ( 1 << 2 ),
( 1 << 0 ) | ( 1 << 3 ),
};
if ( ! ( ed[0] & diagMask ) ) NAV_TryExpand( front, floodMap, nbrs[4], newScore );
if ( ! ( ed[1] & diagMask ) ) NAV_TryExpand( front, floodMap, nbrs[5], newScore );
if ( ! ( ed[2] & diagMask ) ) NAV_TryExpand( front, floodMap, nbrs[6], newScore );
if ( ! ( ed[3] & diagMask ) ) NAV_TryExpand( front, floodMap, nbrs[7], newScore );
} else {
NAV_TryExpand( front, floodMap, nbrs[4], newScore );
NAV_TryExpand( front, floodMap, nbrs[5], newScore );
NAV_TryExpand( front, floodMap, nbrs[6], newScore );
NAV_TryExpand( front, floodMap, nbrs[7], newScore );
}
#endif
}
} while ( ! NAV_FrontIsEmpty( front ) );
}
int NAV_TracePath( int origin, int target, int gridW, const int *floodMap,
int resultSize, int *ioResult, int *outNumElemsWritten ) {
if ( resultSize == 0
|| floodMap[target] == NAV_BLOC
|| floodMap[target] == NAV_FREE
|| floodMap[origin] == NAV_BLOC ) {
*outNumElemsWritten = 0;
return 0;
}
const int prims[] = {
-1, -gridW, 1, gridW,
};
// explictly push target, then start at 1
ioResult[0] = target;
if ( floodMap[target] == NAV_BLOC ) {
*outNumElemsWritten = 0;
return 0;
}
int numElems;
for ( numElems = 1; numElems < resultSize; numElems++ ) {
int neighbours[4] = {
target + prims[0],
target + prims[1],
target + prims[2],
target + prims[3],
};
int floods[4] = {
floodMap[neighbours[0]],
floodMap[neighbours[1]],
floodMap[neighbours[2]],
floodMap[neighbours[3]],
};
int scores[4] = {
( floods[0] & NAV_BLOC ) | ( ( floods[0] & 0xfffffff ) << 2 ) | 0,
( floods[1] & NAV_BLOC ) | ( ( floods[1] & 0xfffffff ) << 2 ) | 1,
( floods[2] & NAV_BLOC ) | ( ( floods[2] & 0xfffffff ) << 2 ) | 2,
( floods[3] & NAV_BLOC ) | ( ( floods[3] & 0xfffffff ) << 2 ) | 3,
};
int min = Min( scores[3], Min( scores[2], Min( scores[1], scores[0] ) ) );
if ( ( min >> 2 ) >= floodMap[target]) {
break;
}
target = neighbours[min & 3];
ioResult[numElems] = target;
}
*outNumElemsWritten = numElems;
return target == origin;
}
int NAV_FrontBufSizeInBytes( int gridW, int gridH ) {
return NAV_FrontCapacity( gridW, gridH ) * sizeof( *( ( navFront_t* ) 0 )->buf );
}
navFront_t NAV_CreateFront( int gridW, int gridH, int *buffer ) {
navFront_t f = {0};
f.capMask = NAV_FrontCapacity( gridW, gridH ) - 1;
f.buf = buffer;
return f;
}
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment