Fast Marching Method to Generate Distance Over 2D Grid

Hello all!

I am trying to build a distance map for a 2D grid using the Fast Marching Method and I'm hoping to get feedback/suggestions for my algorithm in case I'm missing something to speed it up (whether the calculation itself or the way I am storing/referencing the data). I do understand this isn't a "cheap" process, but I am trying to make it as efficient as possible. I know Djikstra's is a bit faster, but I need the Eikonal solutions because I am building a vector field from this and Djikstra's only creates 8 discrete directions when creating the vector field (really only 4 except for the Cartesian directions around the goal cell).

Here is my process (with timer benchmarks for building the cell grid and for calculating the distance map):
GML:
//timer/benchmarking variables
var _t,_start,_stop;
_start = get_timer()

//initialize grid variables
width = 100
height = 100
cell_size = 8 //just for drawing purposes
cell_grid = ds_grid_create(width,height) //contains cell data
distance_grid = ds_grid_create(width,height) //grid to hold distance values
start_x = 0 //starting point for wave propagation
start_y = 0

//establish enum for cell arrays
enum cell
    {
    x = 0,
    y = 1,
    cost = 2,
    neighbors = 3
    }

//create cells and fill cell_grid with data
var i,j;
for (i=0;i<width;i++)
    {
    for (j=0;j<height;j++)
        {
        var c = array_create(4)
        c[cell.x] = i
        c[cell.y] = j
        c[cell.cost] = 1
        c[cell.neighbors] = ds_list_create()
        ds_grid_set(cell_grid,i,j,c)
        }
    }

//add neighbors to cells
for (i=0;i<width;i++)
    {
    for (j=0;j<height;j++)
        {
        c = cell_grid[# i,j]
        if i > 0        ds_list_add(c[cell.neighbors],cell_grid[# i-1,j])
        if j > 0        ds_list_add(c[cell.neighbors],cell_grid[# i,j-1])
        if i < width-1    ds_list_add(c[cell.neighbors],cell_grid[# i+1,j])
        if j < height-1    ds_list_add(c[cell.neighbors],cell_grid[# i,j+1])
        }
    }

//timer/benchmarking
_stop = get_timer()
_t = (_stop - _start) / 1000
show_debug_message(string(_t)+"ms to fill grid and assign neighbors to cells")
_start = get_timer()

//create a priority queue for frontier and a list for already calculated cells
var frontier = ds_priority_create()
var closed = ds_list_create()

ds_grid_clear(distance_grid,infinity) //clear distance grid
distance_grid[# start_x,start_y] = 0 //set start location distance to 0

//add starting location to closed list
var start = cell_grid[# start_x,start_y]
ds_list_add(closed,start)

//add neighbors of start location to frontier queue
var neighbor;
for (i=0;i<ds_list_size(start[cell.neighbors]);i++)
    {
    neighbor = ds_list_find_value(start[cell.neighbors],i)
    ds_priority_add(frontier,neighbor,neighbor[cell.cost])
    }

var i,current,distance,dx,dy,delta;
var left,right,top,bot;
while !ds_priority_empty(frontier) //loop through all cells in frontier until empty
    {
    current = ds_priority_delete_min(frontier) //get lowest cost cell in frontier queue
    
    //get distances of neighboring cells
    left = distance_grid[# max(0,current[cell.x]-1),current[cell.y]]
    right = distance_grid[# min(width-1,current[cell.x]+1),current[cell.y]]
    top = distance_grid[# current[cell.x],max(0,current[cell.y]-1)]
    bot = distance_grid[# current[cell.x],min(height-1,current[cell.y]+1)]
    
    //calculate Eikonal distance
    dx = min(left,right)
    dy = min(top,bot)
    delta = 2*current[cell.cost] - sqr(dx-dy)
    if delta >= 1
        {
        distance = (dx+dy+sqrt(delta))/2
        }
        else
        {
        distance = min(dx+current[cell.cost],dy+current[cell.cost])
        }
    
    //update distance grid
    distance_grid[# current[cell.x],current[cell.y]] = distance
    //add neighbors to frontier queue if they are not already calculated (part of closed list)
    for (i=0;i<ds_list_size(current[cell.neighbors]);i++)
        {
        neighbor = ds_list_find_value(current[cell.neighbors],i)
        if ds_list_find_index(closed,neighbor) = -1
            {
            ds_priority_add(frontier,neighbor,distance+neighbor[cell.cost])
            //add neighbor cell to closed list so we don't add to frontier again
            ds_list_add(closed,neighbor)
            }
        }
    }

//destroy frontier and closed to avoid memory leak
ds_priority_destroy(frontier)
ds_list_destroy(closed)

//timer/benchmarking
_stop = get_timer()
_t = (_stop - _start) / 1000
show_debug_message(string(_t)+"ms calculate fast marching distance")
Here is my code to visualize the results:
GML:
var i,j,value,color,size,half_grid;
size = cell_size
draw_set_font(fnt_main)
for (i=0;i<width;i++)
    {
    for (j=0;j<height;j++)
        {
        value = ds_grid_get(distance_grid,i,j)
        color = merge_color(c_black,c_red,abs(sin(value/2)))
        draw_set_color(color)
        draw_rectangle(i*size,j*size,i*size+size,j*size+size,0)
        }
    }
And finally, here is the resulting representation of the distance grid:


It appears to be working just fine, but I would be incredibly grateful for any suggested performance boost suggestions. This is a 100x100 grid and it is taking roughly 50-60ms to build the grid of cells and populate neighbors and then 700-750ms to perform the distance calculations. In practice, I'll be chunking these calculations so I only have to calculate on a few chunks when the map changes, but I'm trying to weed out any stupid mistakes in my method up front.

YYC only provides negligible improvements.

Implemented from: http://www.numerical-tours.com/matlab/fastmarching_0_implementing/
and: https://canal.uned.es/uploads/material/Video/49784/Presentaci__n_Luis_Moreno.pdf
and: https://jvgomez.github.io/files/pubs/fm2star.pdf (source paper)

If anyone smarter than me can understand this "heat method," this may be a better choice...http://www.cs.cmu.edu/~kmcrane/Projects/HeatMethod/index.html

Thank you for your time!
 
Last edited:

Juju

Member
Total time is 43ms in VM, 14.5ms in YYC. I've noticed YYC can sometimes jump up to 30ms though so I wonder if the lower value is the result of sort of caching behaviour. Regarless, I'm confident that, if distributed over a handful of frames, this method can be reliably run in realtime on a 100x100 grid.

Edit: Took another look at this for some micro-optimisations 🤪

GML:
//Start benchmarking timer
var _start = get_timer()

//Set basic grid definitions
width     = 100;
height    = 100;
cell_size =   8;
start_x   =   0;
start_y   =   0;

//Build two grids, one for the per-cell cost and another for the distance from the start point
cell_cost_grid = ds_grid_create(width, height);
distance_grid  = ds_grid_create(width, height);
ds_grid_clear(cell_cost_grid, 1);
ds_grid_clear(distance_grid,  infinity);

//Create two temporary data structures
var _frontier  = ds_priority_create();
var _open_grid = ds_grid_create(width, height);
ds_grid_clear(_open_grid, true);

//Benchmarking
show_debug_message(string((get_timer() - _start) / 1000) + "ms to fill grids / create data structures");
_start = get_timer();

//Set initial algorithm state
_open_grid[#  start_x, start_y] = false;
distance_grid[# start_x, start_y] = 0;

//Cache some instance variables cos instance variable lookup does have a small associated cost
var _cell_cost_grid = cell_cost_grid;
var _distance_grid  = distance_grid;
var _width_minus_1  = width-1;
var _height_minus_1 = height-1;

//Queue our immediate neighbours
if (start_x > 0)
{
    _open_grid[# start_x-1, start_y] = false;
    ds_priority_add(_frontier,
                    (start_y << 32) | (start_x-1),
                    _cell_cost_grid[# start_x-1, start_y]);
}

if (start_x < _width_minus_1)
{
    _open_grid[# start_x+1, start_y] = false;
    ds_priority_add(_frontier,
                    (start_y << 32) | (start_x+1),
                    _cell_cost_grid[# start_x+1, start_y]);
}

if (start_y > 0)
{
    _open_grid[# start_x, start_y-1] = false;
    ds_priority_add(_frontier,
                    ((start_y-1) << 32) | start_x,
                    _cell_cost_grid[# start_x, start_y-1]);
}

if (start_y < _height_minus_1)
{
    _open_grid[# start_x, start_y+1] = false;
    ds_priority_add(_frontier,
                    ((start_y+1) << 32) | start_x,
                    _cell_cost_grid[# start_x, start_y+1]);
}

//Iterate until we run out of cells to process
while(!ds_priority_empty(_frontier))
{
    //Grab the cell with the lowest distance from the priority queue
    var _current_code = ds_priority_delete_min(_frontier);
    //Unpack the x/y coordinates for the cell
    var _current_x = _current_code & 0xFFFFFFFF;
    var _current_y = _current_code >> 32;
 
    //Cache the cost and distance for this cell
    var _current_cost = _cell_cost_grid[# _current_x, _current_y];
 
    //Get cell distances immediately around ourselves
    //If we're at the edge of the grid, use our current distance instead of a neighbour's distance
    //Also cache whether we can look left/right/up/down to avoid calling the same if-statements twice
    if (_current_x > 0)
    {
        var _l_look = true;
        var _l = _distance_grid[# _current_x-1, _current_y];
    }
    else
    {
        var _l_look = false;
        var _l = _distance_grid[# _current_x, _current_y];
    }
 
    if (_current_x < _width_minus_1)
    {
        var _r_look = true;
        var _r = _distance_grid[# _current_x+1, _current_y];
    }
    else
    {
        var _r_look = false;
        var _r = _distance_grid[# _current_x, _current_y];
    }
 
    if (_current_y > 0)
    {
        var _t_look = true;
        var _t = _distance_grid[# _current_x, _current_y-1];
    }
    else
    {
        var _t_look = false;
        var _t = _distance_grid[# _current_x, _current_y];
    }
 
    if (_current_y < _height_minus_1)
    {
        var _b_look = true;
        var _b = _distance_grid[# _current_x, _current_y+1];
    }
    else
    {
        var _b_look = false;
        var _b = _distance_grid[# _current_x, _current_y];
    }
 
    //Eikonal distance
    if (_l < _r)
    {
        var _dx = _l;
    }
    else
    {
        var _dx = _r;
    }
 
    if (_t < _b)
    {
        var _dy = _t;
    }
    else
    {
        var _dy = _b;
    }
 
    var _delta = 2*_current_cost - (_dx - _dy)*(_dx - _dy);
    if (_delta >= 1)
    {
        var _distance = 0.5*(_dx + _dy + sqrt(_delta));
    }
    else
    {
        if (_dx < _dy)
        {
            var _distance = _dx + _current_cost;
        }
        else
        {
            var _distance = _dy + _current_cost;
        }
    }
 
    _distance_grid[# _current_x, _current_y] = _distance;
 
    //Queue up our neighbours for processing
    --_current_x;
    if (_l_look && _open_grid[# _current_x, _current_y])
    {
        _open_grid[# _current_x, _current_y] = false;
        ds_priority_add(_frontier,
                       _current_code - 0x000000001,
                       _distance + _cell_cost_grid[# _current_x, _current_y]);
    }
 
    ++_current_x;
    ++_current_x; // = original x + 1
    if (_r_look && _open_grid[# _current_x, _current_y])
    {
        _open_grid[# _current_x, _current_y] = false;
        ds_priority_add(_frontier,
                        _current_code + 0x000000001,
                        _distance + _cell_cost_grid[# _current_x, _current_y]);
    }
 
    --_current_x;
    --_current_y;
    if (_t_look && _open_grid[# _current_x, _current_y])
    {
        _open_grid[# _current_x, _current_y] = false;
        ds_priority_add(_frontier,
                        _current_code - 0x100000000, // (1 << 32)
                        _distance + _cell_cost_grid[# _current_x, _current_y]);
    }
 
    ++_current_y;
    ++_current_y; // = original y + 1
    if (_b_look && _open_grid[# _current_x, _current_y])
    {
        _open_grid[# _current_x, _current_y] = false;
        ds_priority_add(_frontier,
                        _current_code + 0x100000000, // (1 << 32)
                        _distance + _cell_cost_grid[# _current_x, _current_y]);
    }
}

//Clean up data structures
ds_priority_destroy(_frontier);
ds_grid_destroy(_open_grid);

//Benchmarking
show_debug_message(string((get_timer() - _start) / 1000) + "ms calculate fast marching distance");
 
Last edited:
Wow - this is great! After an initial look through your code, I'm guessing the biggest speed up was the storing of cell positions in a single grid using the bitwise operations (which I have never used before!) instead of storing cell data in an array (with a nested neighbor list). I am testing and implementing now!

Thank you for your time and effort on this!

EDIT: I am floored at the processing time increase (getting roughly the same numbers as you on VM/YYC). Excellent job on this! I will learn a lot from your code. Many thanks!
 
Last edited:
Top