Dijkstra’s Algorithm and A* Search

Adam Rumpf, 11/8/2017

Introduction

This demonstration illustrates the process of searching for the shortest path between two yellow cells on a field of black obstacles using either Dijkstra’s algorithm or A* search. The progress of the chosen search algorithm is shown step-by-step, with tiles being highlighted as they are scanned. The final path chosen is shown in red. Controls exist to select the locations of the start and end nodes, the types of obstacles, and the current step (which can be used to animate the search process).

The graph in this example is made up of a 17×17 grid of nodes. Each node is adjacent to the 8 nodes immediately surrounding it. Vertical and horizontal edges have a weight of 1 while diagonal edges have a weight of (Sqrt[2]), representing Euclidean distance on a unit lattice.

The standard version of Dijkstra’s algorithm attempts to find the shortest path from a start vertex to every other vertex. In this program it has been modified to halt early if the finish vertex is removed from the tentative set, since at this point no shorter path can possibly be found. In the animation, unvisited nodes are colored white, tentative nodes are colored light blue, and searched nodes are colored dark blue. Watching the search process unfold displays how the search region slowly expands in all directions from the starting node.

A* search is a modified version of Dijkstra’s algorithm that attempts to preferentially search tentative nodes likely to be closer to the finish. It does this based on heuristic distance, which in this case is simply Euclidean distance from the node to the finish. The animation highlights the search region in the same way as with Dijkstra’s algorithm, but A* tends to attempt to move in a direct line towards the finish when possible, only stopping to expand the search region when it gets caught on an obstacle.

Note that this program is quite computationally intensive. It is best to manipulate the controls slowly and to wait for everything to update before moving on.

Code

Initialization

dim=17;
(* Calculates the coordinates of vertex number k on an m×n grid, assuming that the first element is at (1,1). *)
coords[k_,m_,n_]:={Mod[k-1,m]+1,Quotient[k-1,m]+1}
(* Calculates the vertex number of a given set of coordinates on an m×n grid, assuming that the first vertex is located at (1,1). *)
rcoords[c_,m_,n_]:=c[[1]]+m(c[[2]]-1)
(* Transforms from array coordinates to plotted coordinates. *)
tcoords[c_,dim_]:={dim-c[[2]]+1,c[[1]]}
(* Transforms from path coordinates to plotted coordinates. *)
pcoords[c_,dim_]:={c[[2]]-0.5,dim-c[[1]]+0.5}
(* Calculates the values of the vertices on an m×n grid graph that fall inside a rectangle with a given bottom left and top right coordinate, assuming that the first element is (1,1). *)
rlist[bl_,tr_,m_,n_]:=Table[If[bl[[1]]≤Mod[k-1,m]+1≤tr[[1]]&&bl[[2]]≤Quotient[k-1,m]+1≤tr[[2]],k,Nothing],{k,1,m n}]
(* Calculates the values of the vertices on an m×n grid graph that fall inside a disk with a given center and radius, assuming that the first element is (1,1). *)
dlist[c_,r_,m_,n_]:=Table[If[EuclideanDistance[c,coords[k,m,n]]≤r,k,Nothing],{k,1,m n}]
(* Generates an m×n grid graph, with nodes numbered from {1,2,...,m·n}, with an option to use von Neumann neighborhoods (option 0; adjacent only in cardinal directions) or Moore neighborhoods (option 1; adjacent also in diagonal directions), and an option to include a list of vertices to exclude from the grid. The graph weighs each edge according to Euclidean distance. *)
graphgen[m_,n_,type_:0,exc_:{}]:=Module[{v=Range[m n],e={},w={},c},
(* v,e,w = vertex, edge, and weight lists
c = coordinates of all vertices *)
c=Table[coords[k,m,n],{k,1,Length[v]}];
Do[
If[MemberQ[exc,k]==False,
(* only move ahead if the current vertex is not excluded *)
Switch[type,
0,(* von Neumann *)
If[c[[k,1]]<m,
If[MemberQ[exc,k+1]==False,
e=Append[e,k\[UndirectedEdge](k+1)];
w=Append[w,1];
];
];
If[c[[k,2]]<n,
If[MemberQ[exc,k+m]==False,
e=Append[e,k\[UndirectedEdge](k+m)];
w=Append[w,1];
];
],
1, (* Moore *)
If[c[[k,1]]<m,
If[MemberQ[exc,k+1]==False,
e=Append[e,k\[UndirectedEdge](k+1)];
w=Append[w,1];
];
];
If[c[[k,2]]<n,
If[MemberQ[exc,k+m]==False,
e=Append[e,k\[UndirectedEdge](k+m)];
w=Append[w,1];
];
];
If[c[[k,1]]<m&&c[[k,2]]<n,
If[MemberQ[exc,k+m+1]==False&&MemberQ[exc,k+m]==False&&MemberQ[exc,k+1]==False,
e=Append[e,k\[UndirectedEdge](k+m+1)];
w=Append[w,N[(Sqrt[2])]];
];
];
If[c[[k,1]]<m&&1<c[[k,2]]<n+1,
If[MemberQ[exc,k-m+1]==False&&MemberQ[exc,k-m]==False&&MemberQ[exc,k+1]==False,
e=Append[e,k\[UndirectedEdge](k-m+1)];
w=Append[w,N[(Sqrt[2])]];
];
];
];
],
{k,1,Length[v]}];
Graph[v,e,EdgeWeight->w]
]
(* Conducts Dijkstra's algorithm on a given graph "g" to find the shortest path from vertex "start" to vertex "finish". We assume that the vertices are labeled from {1,2,...,n}. The output is a list of three objects in order: the shortest path (as a list of the vertices visited, in order), a list specifying the step on which each vertex was first opened, and a list specifying the step on which each vertex was first closed. *)
dijkstra[g_,start_,finish_]:=Module[{vl=VertexList[g],el=EdgeList[g],wl=PropertyValue[g,EdgeWeight],path={},opened,closed,q,dist,prev,alt,done=False,u,v,nbh,i=1,temp},
(* vl = vertex list
el = edge list
wl = edge weight list
q = set of vertices yet to be processed
dist = vector of shortest known distances from start to each vertex
prev = vector of predecessors along the shortest path from start to each vertex
alt = alternative distance to compare
done = indicates whether or not we have closed the finish vertex
u,v = labels of vertices currently under examination
nbd = neighborhood of vertex currently under examination
i = iteration *)
(* initialization *)
q=vl;
Do[
opened[(vl[[k]])]=closed[(vl[[k]])]=dist[(vl[[k]])]=prev[(vl[[k]])]=Infinity,
{k,1,Length[vl]}];
dist[start]=0;
prev[start]=start;
opened[start]=1;
(* main loop *)
While[done==False,
i++;
u=Flatten[MinimalBy[q,dist]][[1]];(* pick u as the vertex with the smallest known distance *)
closed[u]=i;(* record when we discovered u *)
q=DeleteCases[q,u];(* remove u from q *)
If[u==finish,
done=True,(* if we've just closed the finish vertex, terminate *)
nbh=Intersection[q,DeleteCases[VertexList[NeighborhoodGraph[g,u]],u]];(* neighborhood of u *)
Do[
v=nbh[[k]];
If[i<opened[v],
opened[v]=i;(* record if we've just opened v *)
];
(* decide whether the edge is u\[UndirectedEdge]v or v\[UndirectedEdge]u, and calculate the distance to v from u *)
If[MemberQ[el,u\[UndirectedEdge]v]==True,
alt=dist[u]+wl[[(Flatten[Position[el,u\[UndirectedEdge]v]][[1]])]],
alt=dist[u]+wl[[(Flatten[Position[el,v\[UndirectedEdge]u]][[1]])]];
];
If[alt<dist[v],
dist[v]=alt;
prev[v]=u;
],
{k,1,Length[nbh]}];
];
];
(* back-tracing to get the path *)
u=finish;
While[u≠start,
path=Prepend[path,u];
u=prev[u];
];
path=Prepend[path,start];
{path,Table[opened[(vl[[k]])],{k,1,Length[vl]}],Table[closed[(vl[[k]])],{k,1,Length[vl]}]}
]
(* Generates the Euclidean distances from all vertices on an m×n grid graph to a specified vertex. *)
edist[m_,n_,finish_]:=N[Module[{c=Table[coords[k,m,n],{k,1,m n}]},
Table[N[EuclideanDistance[(c[[k]]),(c[[finish]])]],{k,1,m n}]]]
(* Conducts the (SuperStar[A]) algorithm. Similar to the Dijkstra's algorithm program above, but also accepts a heuristic vector corresponding to the vertex list. *)
astar[g_,start_,finish_,h_]:=Module[{vl=VertexList[g],el=EdgeList[g],wl=PropertyValue[g,EdgeWeight],path={},opened,closed,q,dist,prev,alt,done=False,u,v,nbh,i=1,temp},
(* vl = vertex list
el = edge list
wl = edge weight list
q = set of vertices yet to be processed
dist = vector of shortest known distances from start to each vertex
prev = vector of predecessors along the shortest path from start to each vertex
alt = alternative distance to compare
done = indicates whether or not we have closed the finish vertex
u,v = labels of vertices currently under examination
nbd = neighborhood of vertex currently under examination
i = iteration *)
(* initialization *)
q=vl;
Do[
opened[(vl[[k]])]=closed[(vl[[k]])]=dist[(vl[[k]])]=prev[(vl[[k]])]=Infinity,
{k,1,Length[vl]}];
dist[start]=0;
prev[start]=start;
opened[start]=1;
(* main loop *)
While[done==False,
i++;
u=Flatten[MinimalBy[q,dist]][[1]];(* pick u as the vertex with the smallest known distance *)
closed[u]=i;(* record when we discovered u *)
q=DeleteCases[q,u];(* remove u from q *)
If[u==finish,
done=True,(* if we've just closed the finish vertex, terminate *)
nbh=Intersection[q,DeleteCases[VertexList[NeighborhoodGraph[g,u]],u]];(* neighborhood of u *)
Do[
v=nbh[[k]];
If[i<opened[v],
opened[v]=i;(* record if we've just opened v *)
];
(* decide whether the edge is u\[UndirectedEdge]v or v\[UndirectedEdge]u, and calculate the distance to v from u *)
If[MemberQ[el,u\[UndirectedEdge]v]==True,
alt=dist[u]+wl[[(Flatten[Position[el,u\[UndirectedEdge]v]][[1]])]]+h[[v]]-h[[u]],
alt=dist[u]+wl[[(Flatten[Position[el,v\[UndirectedEdge]u]][[1]])]]+h[[v]]-h[[u]];
];
If[alt<dist[v],
dist[v]=alt;
prev[v]=u;
],
{k,1,Length[nbh]}];
];
];
(* back-tracing to get the path *)
u=finish;
While[u≠start,
path=Prepend[path,u];
u=prev[u];
];
path=Prepend[path,start];
{path,Table[opened[(vl[[k]])],{k,1,Length[vl]}],Table[closed[(vl[[k]])],{k,1,Length[vl]}]}
]
(* Generates an array of numbers to describe the current state of the search, given dimensions, the excluded vertices, the start vertex, the finish vertex, the shortest path, the vertex open list, the vertex close list, and the step number. *)
steparray[m_,n_,exc_,start_,finish_,path_,opened_,closed_,step_]:=Module[{vl=Range[m n],a=ConstantArray[0,{m,n}],temp},
(* a = array of values corresponding to the current state of each vertex:
0 = empty
1 = start or finish
2 = opened
3 = closed
4 = unexplored *)
Do[
If[MemberQ[exc,k]==False,
(* nonempty vertex *)
temp=coords[k,m,n];
If[k==start||k==finish,
(* start/finish vertex *)
a[[(temp[[1]]),(temp[[2]])]]=1,
If[Length[closed]>0,
If[step≥closed[[k]],
(* vertex is closed *)
a[[(temp[[1]]),(temp[[2]])]]=3,
If[step ≥opened[[k]],
(* vertex is open *)
a[[(temp[[1]]),(temp[[2]])]]=2,
(* vertex is unexplored *)
a[[(temp[[1]]),(temp[[2]])]]=4;
];
],
a[[(temp[[1]]),(temp[[2]])]]=4;
];
];
],
{k,1,Length[vl]}];
a
]
(* Returns the largest finite element of a list. *)
maxfin[list_]:=Max[DeleteCases[list,Infinity]]

Demonstration

Manipulate[Module[{g,exc,path,opened,closed,mat,showpath},
Switch[block,
0,(* no exclusions *)
exc={},
1,(* large center square *)
exc=rlist[{Floor[(dim/2)]-1,Floor[(dim/2)]-1},{Ceiling[(dim/2)]+2,Ceiling[(dim/2)]+2},dim,dim],
2,(* two stacked squares *)
exc=Join[rlist[{Floor[(dim/2)],4},{Ceiling[(dim/2)]+1,6},dim,dim],rlist[{Floor[(dim/2)],dim-5},{Ceiling[(dim/2)]+1,dim-3},dim,dim]],
3,(* three squares in a triangle *)
exc=Join[rlist[{4,4},{6,6},dim,dim],rlist[{Floor[(dim/2)],dim-5},{Ceiling[(dim/2)]+1,dim-3},dim,dim],rlist[{dim-5,4},{dim-3,6},dim,dim]],
4,(* four squares in a 2×2 array *)
exc=Join[rlist[{4,4},{6,6},dim,dim],rlist[{4,dim-5},{6,dim-3},dim,dim],rlist[{dim-5,4},{dim-3,6},dim,dim],rlist[{dim-5,dim-5},{dim-3,dim-3},dim,dim]],
5,(* large circle in middle *)
exc=dlist[{Ceiling[(dim/2)],Ceiling[(dim/2)]},(dim/5),dim,dim],
6,(* wall with wide gap *)
exc=Join[rlist[{Floor[(dim/2)],1},{Ceiling[(dim/2)]+1,Floor[(dim/2)]-1},dim,dim],rlist[{Floor[(dim/2)],Ceiling[(dim/2)]+2},{Ceiling[(dim/2)]+1,dim},dim,dim]],
7,(* wall with narrow gap *)
exc=Join[rlist[{Floor[(dim/2)],1},{Ceiling[(dim/2)]+1,Floor[(dim/2)]},dim,dim],rlist[{Floor[(dim/2)],Ceiling[(dim/2)]+1},{Ceiling[(dim/2)]+1,dim},dim,dim]],
8,(* L-shaped wall *)
exc=Join[rlist[{6,5},{7,dim-5},dim,dim],rlist[{6,dim-6},{dim-4,dim-5},dim,dim]];
];
g=graphgen[dim,dim,1,exc];
If[GraphDistance[g,rcoords[tcoords[sc,dim],dim,dim],rcoords[tcoords[fc,dim],dim,dim]]==Infinity,
(* endpoint in a block *)
showpath=False;
mat=steparray[dim,dim,exc,rcoords[tcoords[sc,dim],dim,dim],rcoords[tcoords[fc,dim],dim,dim],{},{},{},1],
(* endpoints are clear *)
Switch[type,
0,(* Dijkstra *)
{path,opened,closed}=dijkstra[g,rcoords[tcoords[sc,dim],dim,dim],rcoords[tcoords[fc,dim],dim,dim]],
1,(* (SuperStar[A]) *)
{path,opened,closed}=astar[g,rcoords[tcoords[sc,dim],dim,dim],rcoords[tcoords[fc,dim],dim,dim],edist[dim,dim,rcoords[tcoords[fc,dim],dim,dim]]];
];
If[step≥maxfin[closed],
showpath=True,
showpath=False;
];
mat=steparray[dim,dim,exc,rcoords[tcoords[sc,dim],dim,dim],rcoords[tcoords[fc,dim],dim,dim],path,opened,closed,step];
];
Show[{ArrayPlot[mat,ColorRules->{0->Black,1->Yellow,2->RGBColor[0.9,0.9,1.0],3->RGBColor[0.7,0.7,1.0],4->White}],If[showpath==True,Graphics[{Red,Thick,Line[Table[pcoords[(coords[(path[[k]]),dim,dim]),dim],{k,1,Length[path]}]]}],Nothing]},PlotRange->{{0,dim},{0,dim}},AspectRatio->1]
],{{type,0,"search algorithm"},{0->"Dijkstra",1->"(SuperStar[A])"},RadioButton},{{step,1,"algorithm step"},1,(dim^2)+1,1,Animator,AppearanceElements->{"ProgressSlider","PlayPauseButton"},AnimationRunning->False,AnimationRate->20},{{sc,{2,2},"starting point"},{1,1},{dim,dim},1},{{fc,{dim-1,dim-1},"ending point"},{1,1},{dim,dim},1},{{block,1,"obstacles"},{0->"none",1->"1 square",2->"2 squares",3->"3 squares",4->"4 squares",5->"circle",6->"wide gap",7->"narrow gap",8->"L-shaped wall"}},ControlPlacement->Left,SaveDefinitions->True]