# Fast Marching Method to Generate Distance Over 2D Grid

#### ChessMasterRiley

##### Member
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)
}
}

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]

//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)
}

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
{
//add neighbor cell to closed list so we don't add to frontier again
}
}
}

//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://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

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;
(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;
(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;
((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;
((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;
_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;
_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;
_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;
_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:
• ChessMasterRiley

#### ChessMasterRiley

##### Member
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: