Skip to content

Instantly share code, notes, and snippets.

@zloedi
Last active October 25, 2021 14:30
Show Gist options
  • Save zloedi/7b46c5a6d32cb8516b46f92acd06e3e2 to your computer and use it in GitHub Desktop.
Save zloedi/7b46c5a6d32cb8516b46f92acd06e3e2 to your computer and use it in GitHub Desktop.
Flood fill pather in C#
//#define NAV_FOUR_WAY
public static class Nav {
public const int FREE = ( int )0x0fffffff;
public const int BLOC = ( int )0x40000000;
static int Min( int a, int b ) {
return a < b ? a : b;
}
static int Max( int a, int b ) {
return a > b ? a : b;
}
static int Clamp( int a, int min, int max ) {
return Min( max, Max( a, min ) );
}
static void Reset( Context ctx, int origin ) {
ctx.head = 0;
ctx.tail = 1;
ctx.front[ctx.head] = origin;
}
static int Pop( Context ctx ) {
int coord = ctx.front[ctx.head & ( ctx.front.Length - 1 )];
ctx.head++;
return coord;
}
// ! silently overflows !
static void Push( Context ctx, int coord ) {
ctx.front[ctx.tail & ( ctx.front.Length - 1 )] = coord;
ctx.tail++;
}
static bool FrontIsEmpty( Context ctx ) {
return ctx.head == ctx.tail;
}
static void TryExpand( Context ctx, int nbr, int newScore ) {
if ( ( ctx.floodMap[nbr] & ~BLOC ) > newScore ) {
ctx.floodMap[nbr] = newScore;
Push( ctx, nbr );
}
}
// front size should be power of two, use this for getting a nice front buffer
static int [] CreateFront( int navMapSize ) {
// front size picked totally arbitrary
int c = navMapSize / 4;
// 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 new int[c];
}
static int [] CreateFloodMap( int navMapSize ) {
return new int[navMapSize];
}
// == PUBLIC API ==
public class Context {
// internal state
public int head;
public int tail;
public int [] front;
// the filled map, passed to i.e. trace path
public int [] floodMap;
}
public static Context CreateContext( int navMapSize ) {
return new Context {
front = CreateFront( navMapSize ),
floodMap = CreateFloodMap( navMapSize ),
};
}
// before being able to trace paths, you need to flood the map
// reuse the context for tracing multiple paths
public static int [] FloodMap( int origin, int maxRange, int navMapPitch,
byte [] navMap, Context ctx ) {
if ( navMap[origin] != 0 ) {
return ctx.floodMap;
}
int [] prims = {
// keep them ordered
-1, -navMapPitch, 1, navMapPitch,
#if ! NAV_FOUR_WAY
-1 - navMapPitch, 1 - navMapPitch, 1 + navMapPitch, -1 + navMapPitch,
#endif
};
int [] ed = {
( 1 << 1 ) | ( 1 << 0 ),
( 1 << 2 ) | ( 1 << 1 ),
( 1 << 3 ) | ( 1 << 2 ),
( 1 << 0 ) | ( 1 << 3 ),
};
for ( int i = 0; i < navMap.Length; i++ ) {
ctx.floodMap[i] = navMap[i] != 0 ? BLOC : FREE;
}
int max = ctx.floodMap.Length - 1;
ctx.floodMap[origin] = 1;
Reset( ctx, origin );
int [] nbrs = new int [8];
do {
int c = Pop( ctx );
int headScore = ctx.floodMap[c];
if ( headScore < maxRange * 10 ) {
int newScore = headScore + 10;
nbrs[0] = Clamp( c + prims[0], 0, max );
nbrs[1] = Clamp( c + prims[1], 0, max );
nbrs[2] = Clamp( c + prims[2], 0, max );
nbrs[3] = Clamp( c + prims[3], 0, max );
#if ! NAV_FOUR_WAY
nbrs[4] = Clamp( c + prims[4], 0, max );
nbrs[5] = Clamp( c + prims[5], 0, max );
nbrs[6] = Clamp( c + prims[6], 0, max );
nbrs[7] = Clamp( c + prims[7], 0, max );
#endif
TryExpand( ctx, nbrs[0], newScore );
TryExpand( ctx, nbrs[1], newScore );
TryExpand( ctx, nbrs[2], newScore );
TryExpand( ctx, nbrs[3], newScore );
#if ! NAV_FOUR_WAY
int diagMask = 0;
diagMask |= ( ctx.floodMap[nbrs[0]] >> 30 ) << 0;
diagMask |= ( ctx.floodMap[nbrs[1]] >> 30 ) << 1;
diagMask |= ( ctx.floodMap[nbrs[2]] >> 30 ) << 2;
diagMask |= ( ctx.floodMap[nbrs[3]] >> 30 ) << 3;
newScore = headScore + 14;
if ( diagMask != 0 ) {
if ( ( ed[0] & diagMask ) == 0 ) TryExpand( ctx, nbrs[4], newScore );
if ( ( ed[1] & diagMask ) == 0 ) TryExpand( ctx, nbrs[5], newScore );
if ( ( ed[2] & diagMask ) == 0 ) TryExpand( ctx, nbrs[6], newScore );
if ( ( ed[3] & diagMask ) == 0 ) TryExpand( ctx, nbrs[7], newScore );
} else {
TryExpand( ctx, nbrs[4], newScore );
TryExpand( ctx, nbrs[5], newScore );
TryExpand( ctx, nbrs[6], newScore );
TryExpand( ctx, nbrs[7], newScore );
}
#endif
}
} while ( ! FrontIsEmpty( ctx ) );
return ctx.floodMap;
}
// call this to get a path between two nodes
public static bool TracePath( int origin, int target, int gridW,
int [] ioFloodMap, int [] ioResult,
out int numWritten ) {
if ( ioResult.Length == 0
|| ioFloodMap[target] == BLOC
|| ioFloodMap[target] == FREE
|| ioFloodMap[origin] == BLOC ) {
numWritten = 0;
return false;
}
int [] prims = {
-1, -gridW, 1, gridW,
};
// explictly push target, then start at 1
ioResult[0] = target;
if ( ioFloodMap[target] == BLOC ) {
numWritten = 0;
return false;
}
int max = ioFloodMap.Length - 1;
int numElems;
for ( numElems = 1; numElems < ioResult.Length; numElems++ ) {
int [] neighbours = {
Clamp( target + prims[0], 0, max ),
Clamp( target + prims[1], 0, max ),
Clamp( target + prims[2], 0, max ),
Clamp( target + prims[3], 0, max ),
};
int [] floods = {
ioFloodMap[neighbours[0]],
ioFloodMap[neighbours[1]],
ioFloodMap[neighbours[2]],
ioFloodMap[neighbours[3]],
};
int [] scores = {
( int )( floods[0] & BLOC ) | ( ( floods[0] & 0xfffffff ) << 2 ) | 0,
( int )( floods[1] & BLOC ) | ( ( floods[1] & 0xfffffff ) << 2 ) | 1,
( int )( floods[2] & BLOC ) | ( ( floods[2] & 0xfffffff ) << 2 ) | 2,
( int )( floods[3] & BLOC ) | ( ( floods[3] & 0xfffffff ) << 2 ) | 3,
};
int min = Min( scores[3], Min( scores[2], Min( scores[1], scores[0] ) ) );
if ( ( min >> 2 ) >= ioFloodMap[target]) {
break;
}
target = neighbours[min & 3];
ioResult[numElems] = target;
}
numWritten = numElems;
return target == origin;
}
}
@zloedi
Copy link
Author

zloedi commented Oct 25, 2021

Usage:

// context can be reused between floods
var ctx = Nav.CreateContext( _grid.Length );

...

// a flooded map could be used for multiple path traces
var floodMap = FloodMap( a, maxDist, _gridSize.x, _grid, ctx );

// get the path as a list of tiles in 'path'
TracePath( a, b, _gridSize.x, floodMap, path, out numWritten );

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment