Flood fill pather in C#

Nav.cs

//#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;
}


}

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 );