Ordered heaps and the fast marching method

A talk notebook prepared for presentation to the Japanese Mathematica User's Group (JMUG) on June 7, 2003

Daniel Lichtblau
danl@wolfram.com
Wolfram Research, Inc.
©  May  2003
Japanese translation provided courtesy of JMUG members

Abstract:

At our 1999 Developer Conference I spoke about implementation and use in Mathematica of several basic data structures. This talk will explore in some detail a particular structure that seems to receive scant attention within the Mathematica community.

Ordered heaps are data structures one uses when rapid access is needed to a "first" element, and one must also be able to add new elements. A classical application is to sorting a set according to some ordering on a the domain in which the elements live. Another application is to solving a type of numerical PDE via Sethian's "fast marching method". A simple example, shape offsetting, is to find equidistant curves to a given planar curve. We illustrate these two applications in this talk.

Ordered heap and narrow band illustrations provided by Eric Weisstein. The former comes from his MathWorld page on that topic while the latter is from work of James Sethian. Graphics help provided by Michael Trott. Some of this code is based on related work by Husain Ali Al-Mohssen, whom I thank for introducing me to the topic of moving front construction and in particular the fast marching method. I thank Chikara Miyaji and Andrzej Kozlowski of JMUG for inviting me to give this presentation via Web interface, and JMUG members for providing a translation from English to Japanese.

Ordered Heaps

An ordered heap is a data structure used in computer science for purposes of rapid access to a "smallest" element.

Definition: A binary tree is a tree where every node has zero, one, or two children. A node with no children is called a leaf.

Definition: A complete binary tree is a binary tree such that leaves are located only on the bottom two levels, and leaves at the bottom level are all in the leftmost positions.

Definition: An ordered heap is a complete binary tree such that a total ordering exists on the node values, and every node value is less than that of its children. [But no ordering is imposed on the children with repect to one another!]

<< DiscreteMath`Combinatorica`;
HeapQ[a_List?PermutationQ] := 
      Module[{i, n = Length[a]}, 
        And @@ Table[
            a[[Floor[i/2]]] < a[[i]], {i, 2, 
              n}]]
heap = Heapify[RandomPermutation[10]];
ShowLabeledGraph[CompleteBinaryTree[10], heap];

Practical implementation remark: Such a tree is typically represented as a flat array such that the children of the element in position j are located at positions 2  j and 2  j + 1. Among other things this means that a Mathematica implementation might take advantage of packed arrays for amenable data domains.

Proposition: Given a list of n values for which a total order is defined (e.g. the reals under ' < '), one can construct an ordered heap in O(n) steps.

Method: Iteratively place a new element in the bottom-most position (and increment the counter that marks that position). While it is not the root and is less than its parent, swap them.

Classical example:  We sort a set of elements in a domain for which there is a total order. This is "heap sort".

Related application: Working with what are called "priority queues". One requires
(i)  Find "first" element in constant time.
(ii) Add new elements "fast" e.g. in O(log(n)) for a queue containing n elements.

HeapSort

Below is some code to implement a heap sort. We work with the special case where the input is a list of real values. Clearly this can be generalized to take a list from other domains so long as a comparison function is provided that defines a total order on the domain in question. We assume we know in advance the maximal size of the heap; again, this is not necessary but makes the coding simpler and moreover is often a reasonable assumption (e.g. for this particular algorithm).

Proposition: Given an ordered heap containing n values, one may sort the values in O(n log(n)) steps (our logarithms are base 2).

Method 1: Iteratively remove the top-most element. Now replace it with the smaller of its children, and do likewise for the child that moved in to replace it, moving downward until we reach a leaf. We now have a "hole" in the heap (unless it happens that the last child moved came from the last position in the heap; all we are guaranteed is that it was a leaf). So move the last element in the heap into the hole position. From there move it upwards, that is, swap with its parent provided that parent is larger (this is just as in adding an element). It is fairly clear that we have at most 2  log(n) swaps from the initial removal step and the hole filling step combined. As we do this once per root node, we obtain the stated complexity.

Method 2: Iteratively remove the top-most element. Replace it with the bottom-most element. Now compare that element with its new children and iteratively swap with the smaller child, moving downward, until it is smaller than both children. Here we have at most log(n) swaps per iteration. This seems a bit faster than the first method but, curiously, my implementation of this was reliably about 25% slower than that first approach. Hence we will work with an implementation of the first method.

Remark: As the data structure is a complete binary tree, the above complexities apply in the worst as well as average cases. This is not true of all sorting algorithms.

heapbottompointer = 1;
heaptoppointer = 2. ;
makeHeap[len_] := Table[0. , {len + 1}]

We start with a function to add elements to an existing heap. We make the "heap" argument held so as to emulate call-by-reference (we don't want to physically copy the heap every time we add an element!).

SetAttributes[addToHeap, HoldFirst];
addToHeap[heap_, elem_Real] := 
      Module[
{j1, 
          j2}, 
heap[[heapbottompointer]] += 
          1; 
j1 = 
          heap[[heapbottompointer]]; 
heap[[j1 + 
                1]] = 
          elem; 
While[(j2 = 
                  Floor[j1/2]) ≥ 1 && 
            heap[[j2 + 1]] > 
              heap[[j1 + 1]], 
heap[[{j1 + 1, 
                  j2 + 1}]] = 
            heap[[{j2 + 1, j1 + 1}]]; 
j1 = 
            j2;
];
]

We now show a function that will remove the top-most element from a heap and then reshuffle elements to maintain the ordered heap property.

SetAttributes[removeFromHeap, 
        HoldFirst];
removeFromHeap[heap_] := 
      Module[
{prev = 2, j1 = 2, j2 = 3, 
          top = heap[[heaptoppointer]], last, 
          next}, 
last = 
          heap[[heapbottompointer]]; 
While[
          j1 <= last, 
If[
            j2 ≤ last, 
next = 
              If[heap[[j1 + 1]] ≤ heap[[j2 + 1]], j1, 
                j2], 
next = 
              j1
]; 
heap[[prev]] = 
            heap[[next + 1]]; 
prev = 
            next + 1; 
{j1, j2} = 
            2*next + {0, 
                1};
]; 
heap[[heapbottompointer]] -= 
          1; 
heap[[prev]] = 
          heap[[last + 1]]; 
j1 = 
          prev - 1; 
While[(j2 = 
                  Floor[j1/2]) ≥ 1 && 
            heap[[j2 + 1]] > 
              heap[[j1 + 1]], 
heap[[{j1 + 1, 
                  j2 + 1}]] = 
            heap[[{j2 + 1, j1 + 1}]]; 
j1 = 
            j2;
]; 
top
]

With these functions the implementation of our heap sort algorithm is quite straightforward.

heapSort[ll_] := 
      Module[
{len = Length[ll], heap, res, j1, 
          j2}, 
heap = 
          makeHeap[len]; 
Map[addToHeap[heap, #] &, 
          ll]; 
res = 
          Table[removeFromHeap[
              heap], {len}]; 
res
]

Here is a simple example.

tt = Table[Random[], {10000}];

Timing[hh = heapSort[tt];]

We check that our result agrees with that given by Sort.

hh === Sort[tt]

We see that this is not terribly fast (though it is not hard to also show that the implementation at least seems to have the correct algorithmic complexity). To improve on this we will use Compile. This is a bit tricky because we need to avoid function calls, global variables, and so forth if we want the internal byte code to work as effectively as possible. We will do this in two separate steps (sacrificing a small amount of efficiency in order to gain in clarity of code).

The first procedure creates a heap and adds all elements of the list to it. It is in essence the entire loop of addToHeap invocations above.

heapSortPrepareC = 
        Compile[{{ll, _Real, 1}}, 
          Module[
{len = Length[ll], heap, res, j1, 
              j2}, 
heap = 
              Table[0. , {len + 
                    1}]; 
Do[
heap[[1]] += 1. ; 
j1 = 
                Round[heap[[1]]]; 
heap[[j1 + 1]] = 
                ll[[j]]; 
While[(j2 = 
                        Floor[j1/2]) ≥ 1 && 
                  heap[[j2 + 1]] > 
                    heap[[j1 + 1]], 
heap[[{j1 + 1, 
                        j2 + 1}]] = 
                  heap[[{j2 + 1, j1 + 1}]]; 
j1 = 
                  j2;
];
, {j, 
                len}
]; 
heap
]];

The next procedure iterates over an ordered heap, removing elements one by one until it is empty. So it is in effect the entire loop of removeFromHeap invocations.

heapEmptyC = 
        Compile[{{oheap, _Real, 1}}, 
          Module[
{heap = oheap, prev, j1, j2, top, last, 
              next, len}, 
len = 
              Length[heap] - 
                1; 
Table[
last = 
                Round[heap[[1]]]; 
top = 
                heap[[2]]; 
prev = 
                2; 
j1 = 2; 
j2 = 
                3; 
While[
                j1 <= last, 
If[
                  j2 ≤ last, 
next = 
                    If[heap[[j1 + 1]] ≤ heap[[j2 + 1]], 
                      j1, j2], 
next = 
                    j1
]; 
heap[[prev]] = 
                  heap[[next + 1]]; 
prev = 
                  next + 1; 
{j1, j2} = 
                  2*next + {0, 
                      1};
]; 
heap[[1]] -= 
                1. ; 
heap[[prev]] = 
                heap[[last + 1]]; 
j1 = 
                prev - 1; 
While[(j2 = 
                        Floor[j1/2]) ≥ 1 && 
                  heap[[j2 + 1]] > 
                    heap[[j1 + 1]], 
heap[[{j1 + 1, 
                        j2 + 1}]] = 
                  heap[[{j2 + 1, j1 + 1}]]; 
j1 = 
                  j2;
]; 
top, 
{len}
]
]];

The actual heap sort code is again just a pair of calls to the procedures above.

heapSortC[ll_] := 
      Module[
{len = Length[ll], heap, res, j1, 
          j2}, 
heap = 
          heapSortPrepareC[ll]; 
res = 
          heapEmptyC[
            heap]; 
res
]

Timing[hhC = heapSortC[tt];]

We see that this was around 15 times faster than the first version. We check that the results agree.

hhC === hh

One should note that the built-in Sort is another factor of 20 or so faster still. Possibly this indicates room for improvement in the heapSortC code above. Regardless, we have illustrated that the ordered heap is a data structure that is fast to manipulate. We will next apply it to an interesting engineering application.

The fast marching method for solving certain types of numerical differential equations

Many differential equations formulated to track a "moving front" have the form |∇φ(x, 
              y)| = F(x, y) (we use two variables for notational convenience but this applies in higher dimensions). For example, this might apply to the evolution of a curve where the local change is a function of curvature. A very simple example, which we illustrate below, is when the function F is constant. This can be used to obtain curves equidistant to a given curve in the plane. Another way to view this is as an surface evolving in time, whose cross sections at specified times give equidistant curves.

We will follow the approach of Sethian and his various collaborators and solve this via the "fast marching method". This is applicable to finding "moving fronts" (or interfaces) that travel in one direction only (that is, never fold back on themselves). For more general cases, e.g. to model the motion of a flame, one might instead use Sethian's "level set method" which is not quite as fast but is still quite effective. The idea is to formulate an "upwind scheme" wherein points on the advancing front are only influenced by those that have already been processed (roughly speaking, this enforces that "flow of information" only proceed forward in time).

Applications of fast marching

Fast marching has been used for shape offsetting, robot motion planning, geodesic computation on manifolds, time-of-arrival computations for moving fronts (e.g. in seismic modelling), shape reconstruction from raw data (e.g. in medical imaging), and many other applications in computational science and engineering.

Outline of the fast marching algorithm

Divide space into a lattice of grid points.

Find those that give the initial curve. We regard them as "frozen". Their neighbors comprise what is called the "narrow band". All others are considered "far".

Compute their distances to the initial curve. There are various finite difference formulations appropriate for approximating this; we use one from Sethian's books and articles. Intitially distances will be 1 if next to or between lattice points that are on opposite sides, and (1/(Sqrt[2])) if adjacent to a pair of lattice points that meet at a corner.

Put the narrow band points into an ordered heap.

Iteratively remove the first one, as it is closest to the set under construction. Change its state to "frozen". Recompute distances of its neighbors. Those that are already in the narrow band may now need to be moved to maintain the heap ordering property  (this requires an added data structure to store pointers from lattice points to the corresponding position in the band heap). Those neighbors that were not on the band now get put there as they now neighbor a "frozen" lattice point. Again, restore the heap ordering property.

Continue this removal, recomputation of distances, and addition of elements to the heap until it is empty.

The picture below will serve to illustrate,with frozen points in black, far points in red, and the band points in grey.

Off[General::spell, General::spell1];
<< Graphics`Colors`
upwind = {{7, 1}, {7, 1}, {6, 2}, {6, 2}, {4, 3}, {3, 2}, {2, 2}, {0, 
            3}, {0, 0}};
x = 13; 
y = Length[upwind]; 
colors = {Black, GrayLevel[ .9], Red}; 
padded = Append[#, x - Plus @@ #] & /@ upwind; 
d = 
      Flatten /@ (MapIndexed[Table[Last[#2], {#1}] &, 
            padded, {2}]); 
disk[x__] := {Disk[x], {Black, Circle[x]}}
Show[
        Graphics[{Table[Line[{{1, i}, {x, i}}], {i, y}], 
            Table[Line[{{i, 1}, {i, y}}], {i, x}], 
            MapIndexed[{colors[[#1]], 
                  disk[{#2[[2]], 
                      1 + y - #2[[1]]},  .25]} &, d, {2}]}, 
          AspectRatio -> Automatic]];

It is important to remove the "least" element in the iteration. The idea is that that is the only element that we know cannot be affected by values of other elements (the unidirectionality-of-movement assumption guarantees this). This is what makes an ordered heap implementation viable.

Further details of why this is a good method may be found in various articles and texts, in particular

J. Sethian (1996). Level Set Methods: Evolving interfaces in geometry, fluid mechanics, computer vision, and materials science. Cambridge Monographs on Applied and Computational Mathematics volume 3. Cambridge University Press.

One advantage, which will be quite visible in our example, is that singularities do not get smoothed away and also do not give the sort of numerical grief one might have in a standard finite difference approach. Moreover level curves need not be continuous. Hence we have a method that will handle sharp changes both in geometry and topology.

(I am being intentionally vague. I confess I do not understand the differential equation aspects terribly well. My main interest was in seeing how Mathematica might be made to work with ordered heaps to implement an algorithm of obvious application to many engineering and scientific disciplines.)

The code for fast marching

Now we define several utility constants and functions. One that we treat as "global" is a distance metric. A more complete implementation would make this part of the data we provide as input, but what we have below suffices for pedagogy.

frozen = -1. ;
far = 300. ;
band = 0. ;
outofbounds = 1000. ;
nostate = 1000;
maxPixelValue = 255. ;
minPixelValue = 0. ;
midPixelValue = (maxPixelValue - minPixelValue)/2;

Attributes[state] = HoldFirst;
state[states_, x_, y_] := 
        If[x > Length[states] || x < 1 || y > Length[states[[x]]] || 
            y < 1, 1000, states[[x, y]]];

Attributes[distance] = HoldFirst; 
    distance[distances_, x_, y_] := 
      If[x > Length[distances] || x < 1 || y > Length[distances[[x]]] || 
          y < 1, 1000. , distances[[x, y]]];

neighborValue[l1_, l2_, states_, distances_] := 
      Module[{s1 = state[states, Apply[Sequence, l1]], 
          s2 = state[states, 
              Apply[Sequence, 
                l2]]}, 
Which[
s1 ≠ frozen && s2 ≠ frozen, 
          outofbounds, 
s1 == frozen && 
            s2 == frozen, 
          Min[distance[distances, Apply[Sequence, l1]], 
            distance[distances, 
              Apply[Sequence, l2]]], 
s1 == frozen, 
          distance[distances, 
            Apply[Sequence, l1]], 
s2 == frozen, 
          distance[distances, 
            Apply[Sequence, l2]]]
]

distanceToBoundary2 = 
        Compile[{{v1, _Real}, {v2, _Real}, {f, _Real}}, (Sqrt[(-f^2)*(-2 + f^2*(v1 - v2)^2)] + 
                f^2*(v1 + v2))/(2*f^2)];

distanceToBoundary1[v1_Real, f_Real] := v1 + 1/f

newDistance[x_Integer, y_Integer, states_, distances_] := 
        Module[{up, down, left, right, f, 
            res}, 
up = {x, y + 1}; down = {x, y - 1}; 
          left = {x - 1, y}; 
          right = {x + 1, y}; 
f = 
            F[x, y]; 
xvalue = 
            neighborValue[right, left, states, 
              distances]; 
yvalue = 
            neighborValue[up, down, states, 
              distances]; 
res = 
            Which[
xvalue == yvalue == 
                outofbounds, 
              far, 
xvalue ≠ outofbounds && 
                yvalue ≠ outofbounds, 
              distanceToBoundary2[xvalue, yvalue, 
                f], 
xvalue ≠ outofbounds, 
              distanceToBoundary1[xvalue, 
                f], 
yvalue ≠ outofbounds, 
              distanceToBoundary1[yvalue, 
                f]
]; 
res
];

Now for the main algorithm.

findDistances[ll_]:=Module[
{bandheap, len = Length[ll], wid = Length[ll[[1]]], 
            hsize, hindex = 0, distancetable, statetable, frozen = -1. , 
            far = 300. , band = 0. , outofbounds = 1000. , dist, 
            maxpixval = 255. , minpixval = 0. , midpixval, heaptable, j1, j2, 
            nbrs, pt, x, y, x1, y1, x2, y2, next, prev, done},

(*Initializations.*)
midpixval = (maxpixval - minpixval)/2;
distancetable  = 
              Map[If[# > midpixval, maxpixval, minpixval] &, ll, {2}];
statetable = 
              Map[If[#1 > 1, far, frozen] &, distancetable, {2}];
heaptable = Table[0, {len}, {wid}];
hsize = len*wid;
bandheap = 
              Table[{0. , 0. , 0. }, {hsize}];

(*Initialize the band by placing all neighbors of frozen cells thereon.*)
Do[
If[statetable[[ii, jj]] =!= frozen, 
                    Continue[]];
nbrs = {{ii, jj + 1}, {ii, 
                        jj - 1}, {ii - 1, jj}, {ii + 1, jj}};

Do[
{x, y} = nbrs[[kk]];
If[!  (0 < 
                                  x ≤ len && 
                                0 < y ≤ wid && 
                                statetable[[x, y]] == far), 
                          Continue[]];
hindex++;
statetable[[x, y]] = 
                          band;
dist = 
                          newDistance[x, y, statetable, distancetable];
distancetable[[x, y]] = 
                          dist;
bandheap[[hindex]] = {dist, N[x], N[y]};
j1 = hindex;

(*Place each neighbor to the frozen cell on the end of the band heap .  
                                Then move it upward to the appropriate location.*)
While[
(j2 = Floor[j1/2]) ≥ 1 && 
                            bandheap[[j2, 1]] > 
                              bandheap[[j1, 
                                  1]], 
bandheap[[{j1, 
                                  j2}]] = 
                            bandheap[[{j2, j1}]]; 
{x1, 
                              y1} = Round[
                              Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                            j2;
];
heaptable[[x, y]] = 
                          j1,
{kk, Length[nbrs]}
],
{ii, len},{jj, wid}
];

While[hindex > 0,
pt = bandheap[[1]];
(*Get smallest element in heap and freeze it.*)
{x, y} = Round[Rest[pt]];
statetable[[x, y]] = frozen;

(*Replace with last element in heap, 
                      then rearrange by moving it downward to restore the heap ordering property.*)
bandheap[[1]] = 
                    bandheap[[hindex]];
done = False;
prev = 1;
{j1, j2} = 2*prev + {0, 1};

While[j1 < hindex && ! done,
If[
                          j2 < hindex, 
next = 
                            If[bandheap[[j1, 1]] ≤ 
                                bandheap[[j2, 1]], j1, 
                              j2], 
next = 
                            j1
];

If[bandheap[[prev, 1]] > 
                              bandheap[[next, 1]],(*Swap? If not,  
                                we are done.*)
bandheap[[{prev, 
                                    next}]] = 
                              bandheap[[{next, 
                                    prev}]]; 
{x1, y1} = 
                              Round[Rest[
                                  bandheap[[prev]]]]; 
heaptable[[x1, y1]] = prev; 
prev = 
                              next; 
{j1, j2} = 
                              2*prev + {0, 1}
,  
done = True,done = True
];
];
{x1, y1} = 
                    Round[Rest[bandheap[[prev]]]];
heaptable[[x1, y1]] = prev;

(*Find neighbors of the removed point that were  already in the band,  update their distances,  
                      and move them to new locations in the band heap as needed.*)
nbrs = {{x, y + 1}, {x, 
                        y - 1}, {x - 1, y}, {x + 1, y}};
Do[
{x2, y2} = nbrs[[kk]];
If[!  (0 < 
                                  x2 ≤ len && 
                                0 < y2 ≤ wid && 
                                statetable[[x2, y2]] == band), 
                          Continue[]];
dist = 
                          newDistance[x2, y2, statetable, distancetable];
distancetable[[x2, 
                              y2]] = dist;
(*Find position of neighbor in heap .  Change its value to reflect new distance.*)
j1 = 
                          heaptable[[x2, y2]];
bandheap[[j1]] = {dist, 
                            N[x2], N[y2]};

(*The new distance is no larger than the old,  so it can only move upward (that is,  
                                  we iteratively compare to parent and swap if it is smaller).*)
While[(j2 = 
                                  Floor[j1/2]) ≥ 1 && 
                            bandheap[[j2, 1]] > 
                              bandheap[[j1, 
                                  1]], 
bandheap[[{j1, 
                                  j2}]] = 
                            bandheap[[{j2, j1}]]; 
{x1, 
                              y1} = Round[
                              Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                            j2;
];
heaptable[[x2, y2]] = 
                          j1
,
{kk, Length[nbrs]}
];

hindex--;

(*Find neighbors of the removed point that are not already in the band,  and place them there.*)
Do[
{x2, y2} = nbrs[[kk]];
If[!  (0 < 
                                  x2 ≤ len && 
                                0 < y2 ≤ wid && 
                                statetable[[x2, y2]] == far), 
                          Continue[]];
hindex++;
statetable[[x2, y2]] = 
                          band;
dist = 
                          newDistance[x2, y2, statetable, distancetable];
distancetable[[x2, 
                              y2]] = dist;
bandheap[[hindex]] = {dist, N[x2], N[y2]};
j1 = hindex;

(*Place each neighbor on the band heap and move upward to the appropriate location.*)
While[
(j2 = Floor[j1/2]) ≥ 1 && 
                            bandheap[[j2, 1]] > 
                              bandheap[[j1, 
                                  1]], 
bandheap[[{j1, 
                                  j2}]] = 
                            bandheap[[{j2, j1}]]; 
{x1, 
                              y1} = Round[
                              Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                            j2;
];
heaptable[[x2, y2]] = 
                          j1,
{kk, Length[nbrs]}
];

];
distancetable
]

An example

We will use a simple example of what is called "shape ofsetting" in two dimensions. For illustration I will start with my initials in rasterized form.

Load a black and white image showing the initial borders of the curve.

gr = 
        Show[Graphics[
            Text[StyleForm["D L", FontSize -> 60, 
                FontFamily -> "Times", 
                FontWeight -> "Bold"], {0, 0}]]];

DLRasterized = 
        ImportString[
          ExportString[Graphics[gr], "PNG", 
            ImageResolution -> 125], "PNG"];
{Head[DLRasterized], Head[First@DLRasterized], 
      DLRasterized[[1, 1]] // Dimensions}

We will take the centered part to work on a 256x256 grid.

x1 = 
        Take[DLRasterized[[1, 1]], {29, 29 + 255}, {122, 
            122 + 255}];
Dimensions[x1]
x2  =  x1[[All, All, 1]];

We run this on the rasterized initials and then show a density plot of the resulting grid values. Visually we see that corners and connectivity of level sets are as we would expect.

F[x_, y_] = 1. 
Timing[distances = findDistances[x2];]
ListContourPlot[distances, ColorFunction -> (Hue[ .78*#] &), 
        ImageSize -> 500];

For variety we will redo this example with a nonuniform metric function.

Clear[F]
F[x_, y_] = 
        N[1/(x^2 + 1) + Abs[y] + 
            Abs[Sin[x*y]]];
Timing[distances2 = findDistances[x2];]
ListContourPlot[distances2, 
        ColorFunction -> (Hue[1 -  .9*#] &), 
        ImageSize -> 500];

One would use a nonuniform function, for example, to model movement of seismic fronts through inhomogeneous media (though this would involve three  dimensions, not two).

As a final illustration we will use a piecewise constant metric where motion into the upper right corner proceeds at speed close to zero (we do not use exact zero because the code is not prepared to handle infinities). This is how one might model motion planning wherein obstacles cannot be penetrated; the impenetrability is enforced by making obstacle areas in effect infinitely far away.

Clear[F]
F[x_, y_] = 
        If[Abs[x] > 200 && Abs[y] > 200, 0.001, 
          1. ];
Timing[distances3 = findDistances[x2];]
ListContourPlot[distances3, 
        ColorFunction -> (Hue[ .78*#] &), 
        ImageSize -> 500];

Making it run faster

It should be noted that these times, while acceptable for many purposes, are not exactly blinding. It turns out that with some effort that mostly amounts to "in-lining" the various procedures called from findDistances, we can get something that will be understandable to the Compile capabilities of Mathematica. While I do not intend to actually show that code (at least not in public!), it is available in the notebook in the cell below. To make it visible one may select that cell and then in the menu click on Cell>Cell Properties>Open. Note that we must encode the metric directly in the findDistances function in order to best take advantage of Compile. This would generally be regarded as poor software design but it is necessary if we wish to get the best possible run time speed. If one considers running similar examples on finer grids, or in three dimensions, one quickly sees the importance of a speed improvement factor of ten or so.

findDistancesC=Compile[{{ll, _Real, 2}},Module[
{bandheap, len = Length[ll], 
                  wid = Length[ll[[1]]], hsize, hindex = 0, 
                  distancetable, statetable, band = 0. , maxpixval = 255. , 
                  minpixval = 0. , midpixval, heaptable, j1, j2, nbrs, pt, x, 
                  y, x1, y1, x2, y2, next, prev, done, res, xval = 1.0, 
                  yval = 1.0, dist = 1.0, out1, out2, s1 = 1. , s2 = 1. , 
                  d1 = 1. , d2 = 1. , f = 1. },

(*Initializations.*)
f = 1. ;
midpixval = (maxpixval - 
                          minpixval)/2;
distancetable  = 
                    Map[If[# > midpixval, maxpixval, minpixval] &, ll, {2}];
statetable = 
                    Map[If[#1 > 1. , 300. , -1. ] &, 
                      distancetable, {2}];
heaptable = 
                    Table[0, {len}, {wid}];
hsize = len*wid;
bandheap = 
                    Table[{0. , 0. , 0. }, {hsize}];

(*Initialize the band by placing all neighbors of frozen cells thereon.*)
Do[
If[statetable[[ii, jj]] != -1. , 
                          Continue[]];
nbrs = {{ii, jj + 1}, {ii, 
                              jj - 1}, {ii - 1, jj}, {ii + 1, jj}};

Do[
{x, y} = nbrs[[kk]];
If[!  (0 < 
                                        x ≤ len && 
                                      0 < y ≤ wid && 
                                      statetable[[x, y]] == 
                                        300. ), Continue[]];
hindex++;
statetable[[x, 
                                    y]] = band;

  
out1 = 
                                x == Length[statetable];
out2 = x == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x + 1, y]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x - 1, y]]];
Which[
s1 ≠ -1.  && 
                                  s2 ≠ -1. , 
xval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x + 1, y]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x - 1, 
                                        y]]]; 
xval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
xval = If[out1, 1000. , 
                                    distancetable[[x + 1, 
                                        y]]], 
s2 == -1. , 
xval = 
                                  If[out2, 1000. , 
                                    distancetable[[x - 1, 
                                        y]]]
];

out1 = 
                                y == Length[statetable];
out2 = y == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x, y + 1]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x, y - 1]]];
Which[
s1 ≠ -1.  && 
                                  s2 ≠ -1. , 
yval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x, y + 1]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x, 
                                        y - 1]]]; 
yval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
yval = If[out1, 1000. , 
                                    distancetable[[x, 
                                        y + 1]]], 
s2 == -1. , 
yval = 
                                  If[out2, 1000. , 
                                    distancetable[[x, 
                                        y - 1]]]
];

Which[
xval == yval == 
                                  1000. , 
dist = 
                                  300. , 
xval ≠ 
                                    1000.  && 
                                  yval ≠ 
                                    1000. , 
dist = (Sqrt[(-f^2)*(-2 + f^2*(xval - yval)^2)] + 
                                        f^2*(xval + yval))/(2*
                                        f^2), 
xval ≠ 1000. , 
dist = 
                                  xval + 
                                    1/f, 
yval ≠ 
                                  1000. , 
dist = 
                                  yval + 1/f
];

distancetable[[x, 
                                    y]] = dist;
bandheap[[hindex]] = {dist, N[x], N[y]};
j1 = hindex;

(*Place each neighbor to the frozen cell on the end of the band heap .  
                                      Then move it upward to the appropriate location.*)
While[
(j2 = Floor[j1/2]) ≥ 1 && 
                                  bandheap[[j2, 1]] > 
                                    bandheap[[j1, 
                                        1]], 
bandheap[[{j1, j2}]] = 
                                  bandheap[[{j2, 
                                        j1}]]; 
{x1, 
                                    y1} = 
                                  Round[
                                    Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                                  j2;
];
heaptable[[x, 
                                    y]] = j1,
{kk, Length[nbrs]}
],
{ii, len},{jj, wid}
];

While[hindex > 0,
pt = bandheap[[1]];
(*Get smallest element in heap and freeze it.*)
{x, y} = Round[Rest[pt]];
statetable[[x, 
                              y]] = -1. ;

(*Replace with last element in heap, 
                            then rearrange by moving it downward to restore the heap ordering property.*)
bandheap[[1]] = 
                          bandheap[[hindex]];
done = False;
prev = 1;
{j1, j2} = 2*prev + {0, 1};

While[j1 < hindex && ! done,
If[
                                j2 < hindex, 
next = 
                                  If[bandheap[[j1, 1]] ≤ 
                                      bandheap[[j2, 1]], j1, 
                                    j2], 
next = 
                                  j1
];

If[bandheap[[prev, 1]] > 
                                    bandheap[[next, 1]],(*Swap? If not,  
                                      we are done.*)
bandheap[[{prev, 
                                        next}]] = 
                                    bandheap[[{next, 
                                        prev}]]; 
{x1, 
                                      y1} = 
                                    Round[
                                      Rest[bandheap[[prev]]]]; 
heaptable[[x1, y1]] = prev; 
prev = 
                                    next; 
{j1, j2} = 
                                    2*prev + {0, 1}
,  
done = True
];
];
{x1, y1} = 
                          Round[Rest[bandheap[[prev]]]];
heaptable[[x1, y1]] = 
                          prev;

(*Find neighbors of the removed point that were  already in the band,  update their distances,  
                            and move them to new locations in the band heap as needed.*)
nbrs = {{x, y + 1}, {x, 
                              y - 1}, {x - 1, y}, {x + 1, y}};
Do[
{x2, y2} = nbrs[[kk]];
If[!  (0 < 
                                        x2 ≤ len && 
                                      0 < y2 ≤ wid && 
                                      statetable[[x2, y2]] == 
                                        band), Continue[]];

  
out1 = 
                                x2 == Length[statetable];
out2 = x2 == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x2 + 1, y2]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x2 - 1, y2]]];

Which[
s1 ≠ -1.  && 
                                  s2 ≠ -1. , 
xval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x2 + 1, y2]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x2 - 1, 
                                        y2]]]; 
xval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
xval = If[out1, 1000. , 
                                    distancetable[[x2 + 1, 
                                        y2]]], 
s2 == -1. , 
xval = 
                                  If[out2, 1000. , 
                                    distancetable[[x2 - 1, 
                                        y2]]]
];

out1 = 
                                y2 == Length[statetable];
out2 = y2 == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x2, y2 + 1]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x2, y2 - 1]]];
Which[
s1 ≠ -1 && 
                                  s2 ≠ -1, 
yval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x2, y2 + 1]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x2, 
                                        y2 - 
                                        1]]]; 
yval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
yval = If[out1, 1000. , 
                                    distancetable[[x2, 
                                        y2 + 
                                        1]]], 
s2 == -1. , 
yval = 
                                  If[out2, 1000. , 
                                    distancetable[[x2, 
                                        y2 - 1]]]
];

Which[
xval == yval == 
                                  1000. , 
dist = 
                                  300. , 
xval ≠ 
                                    1000.  && 
                                  yval ≠ 
                                    1000. , 
dist = (Sqrt[(-f^2)*(-2 + f^2*(xval - yval)^2)] + 
                                        f^2*(xval + yval))/(2*
                                        f^2), 
xval ≠ 1000. , 
dist = 
                                  xval + 
                                    1/f, 
yval ≠ 
                                  1000. , 
dist = 
                                  yval + 1/f
];

distancetable[[x2, 
                                    y2]] = dist;
(*Find position of neighbor in heap .  Change its value to reflect new distance.*)
j1 = 
                                heaptable[[x2, y2]];
bandheap[[j1]] = {dist, N[x2], N[y2]};

(*The new distance is no larger than the old,  so it can only move upward (that is,  
                                        we iteratively compare to parent and swap if it is smaller).*)
While[(j2 = 
                                        Floor[j1/2]) ≥ 1 && 
                                  bandheap[[j2, 1]] > 
                                    bandheap[[j1, 
                                        1]], 
bandheap[[{j1, j2}]] = 
                                  bandheap[[{j2, 
                                        j1}]]; 
{x1, 
                                    y1} = 
                                  Round[
                                    Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                                  j2;
];
heaptable[[x2, 
                                    y2]] = j1
,
{kk, 
                              Length[nbrs]}
];

hindex--;

(*Find neighbors of the removed point that are not already in the band,  
                            and place them there.*)
Do[
{x2, y2} = nbrs[[kk]];
If[!  (0 < 
                                        x2 ≤ len && 
                                      0 < y2 ≤ wid && 
                                      statetable[[x2, y2]] == 
                                        300. ), Continue[]];
hindex++;
statetable[[x2, 
                                    y2]] = band;

  
out1 = 
                                x2 == Length[statetable];
out2 = x2 == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x2 + 1, y2]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x2 - 1, y2]]];

Which[
s1 ≠ -1.  && 
                                  s2 ≠ -1. , 
xval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x2 + 1, y2]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x2 - 1, 
                                        y2]]]; 
xval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
xval = If[out1, 1000. , 
                                    distancetable[[x2 + 1, 
                                        y2]]], 
s2 == -1. , 
xval = 
                                  If[out2, 1000. , 
                                    distancetable[[x2 - 1, 
                                        y2]]]
];

out1 = 
                                y2 == Length[statetable];
out2 = y2 == 1;
s1 = 
                                If[out1, 1000. , 
                                  statetable[[x2, y2 + 1]]];
s2 = 
                                If[out2, 1000. , 
                                  statetable[[x2, y2 - 1]]];
Which[
s1 ≠ -1.  && 
                                  s2 ≠ -1. , 
yval = 1000. , 
s1 == -1.  && 
                                  s2 == -1. , 
d1 = If[out1, 1000. , distancetable[[x2, y2 + 1]]]; 
d2 = 
                                  If[out2, 1000. , 
                                    distancetable[[x2, 
                                        y2 - 
                                        1]]]; 
yval = 
                                  Min[d1, 
                                    d2], 
s1 == -1. , 
yval = If[out1, 1000. , 
                                    distancetable[[x2, 
                                        y2 + 
                                        1]]], 
s2 == -1. , 
yval = 
                                  If[out2, 1000. , 
                                    distancetable[[x2, 
                                        y2 - 1]]]
];

Which[
xval == yval == 
                                  1000. , 
dist = 
                                  300. , 
xval ≠ 
                                    1000.  && 
                                  yval ≠ 
                                    1000. , 
dist = (Sqrt[(-f^2)*(-2 + f^2*(xval - yval)^2)] + 
                                        f^2*(xval + yval))/(2*
                                        f^2), 
xval ≠ 1000. , 
dist = 
                                  xval + 
                                    1/f, 
yval ≠ 
                                  1000. , 
dist = 
                                  yval + 1/f
];

distancetable[[x2, 
                                    y2]] = dist;
bandheap[[hindex]] = {dist, N[x2], N[y2]};
j1 = hindex;

(*Place each neighbor on the band heap and move upward to the appropriate location.*)
While[
(j2 = Floor[j1/2]) ≥ 1 && 
                                  bandheap[[j2, 1]] > 
                                    bandheap[[j1, 
                                        1]], 
bandheap[[{j1, j2}]] = 
                                  bandheap[[{j2, 
                                        j1}]]; 
{x1, 
                                    y1} = 
                                  Round[
                                    Rest[bandheap[[j1]]]]; 
heaptable[[x1, y1]] = j1; 
j1 = 
                                  j2;
];
heaptable[[x2, 
                                    y2]] = j1,
{kk, Length[nbrs]}
];

];
distancetable
]
];

Timing[distancesC = findDistancesC[x2];]

distances === distancesC

Summary

We see that ordered heaps can be a very useful data structure, and that a Mathematica implementation is reasonably straightforward. We use them to implement Sethian's fast marching method for tracking a unidirectionally moving front. One drawback is that the iterative nature of this code causes it to be slow when run by the Mathematica interpreter. Without serious work it is difficult e.g. to take advantage of speed gain offered by Mathematica's Compile capabilities (this points to a useful direction for future enhancement to that function). Regardless, the ability to use code of the appropriate asymptotic complexity is already important because of the relative ease of coding and debugging this algorithm in Mathematica, and because it allows access to a wide range of mathematical functions e.g. for evaluating more complicated functions of gradients than those used in our simple example.