Capstone: Dijkstra on a Real Road Network
Module 4 ends where Dijkstra's algorithm began: with a map. In 1956 Dijkstra chose shortest routes between Dutch cities as the demonstration problem for the ARMAC computer because anyone could check the answer. You will do the same thing at a larger scale — load a road network into an adjacency list, route between two coordinates with Dijkstra and then with A*, measure how much work each one does, and turn the resulting list of vertex ids into directions a person could follow.
This is the Graphs overview and the shortest-paths lesson applied to real data. The interesting problems are the ones textbooks skip: snapping a coordinate to the road graph, choosing a heuristic you can prove is admissible, counting work fairly, and grouping a path into "turn left onto Avenue 10".
Goal
Build a command-line router that, given a road graph and two latitude/longitude pairs:
- snaps each coordinate to the nearest intersection,
- computes the shortest route with Dijkstra and with A* using a haversine heuristic,
- reports the route length and the number of vertices each algorithm settled (expanded),
- prints turn-by-turn directions.
A* must always return the same length as Dijkstra and must never settle more vertices. The reference solution runs offline on a synthetic city; the same code runs on real data once you write a loader.
Requirements
- Graph. Adjacency list: for each vertex, a list of
(neighbour, length_in_metres, street_name). Vertex coordinates stored separately as two arrays of latitude and longitude. - Weights. Non-negative integers (metres). Integer weights make results reproducible across languages and avoid floating-point ties.
- Dijkstra. Binary heap with lazy deletion (push on improvement, skip stale pops), stopping as soon as the target is settled, with parent pointers.
- A*. Same loop, ordered by where is the great-circle distance to the target, rounded down.
- Metric.
expanded= number of non-stale pops, counting the target. This is the fair comparison: stale pops are an artefact of the lazy heap and wall-clock time depends on the language. - Directions. Merge consecutive edges with the same street name into one leg; for each leg change, say left or right; first leg gets a compass direction.
- Tests. The acceptance tests below pass.
Getting the data
Pick one real source for the main exercise. The reference solution ships with a third, synthetic source so it runs with no download.
Option A: an OpenStreetMap extract
OpenStreetMap stores roads as ways (ordered lists of node ids, tagged highway=residential, highway=primary and so on) and nodes (points with latitude and longitude). Turning that into a routing graph means: keep the ways with a drivable highway tag; make every node that is shared by two or more ways, or is a way's endpoint, an intersection vertex; and join consecutive intersections along a way with an edge whose length is the sum of the great-circle lengths of the segments between them. Ways tagged oneway=yes produce one directed edge instead of two.
The easiest route in Python is the OSMnx library, which downloads a place through the Overpass API and does the simplification for you. Its graph is a NetworkX MultiDiGraph whose nodes carry y (latitude) and x (longitude) and whose edges carry length in metres and usually name:
# fragment: needs network access and `pip install osmnx`
import osmnx as ox
G = ox.graph_from_place("Piedmont, California, USA", network_type="drive")
ids = {osm_id: i for i, osm_id in enumerate(G.nodes)} # OSM ids are huge: re-index 0..n-1
lat = [G.nodes[k]["y"] for k in G.nodes]
lon = [G.nodes[k]["x"] for k in G.nodes]
adj = [[] for _ in ids]
for u, v, data in G.edges(data=True): # already directed; one-way respected
name = data.get("name", "unnamed road")
if isinstance(name, list): # merged ways can carry several names
name = name[0]
adj[ids[u]].append((ids[v], round(data["length"]), name))
Start with a small town (a few thousand intersections). For larger areas, download a regional .osm.pbf file from a mirror such as Geofabrik and parse it with a PBF library instead of Overpass. OpenStreetMap data is © OpenStreetMap contributors under the ODbL; keep the attribution if you publish results.
Option B: the DIMACS road graphs
The 9th DIMACS Implementation Challenge (2006) published US road networks derived from TIGER/Line data, from New York City (about 264,000 vertices and 734,000 arcs) up to the whole USA (about 24 million vertices). Each region comes as two plain-text files: a .gr graph file and a .co coordinate file. Lines starting with c are comments; p sp n m gives the sizes; a u v w is an arc from u to v with integer weight w; and v id x y gives a vertex's longitude and latitude multiplied by . Vertices are 1-indexed. Each region has a distance graph (USA-road-d.*) and a travel-time graph (USA-road-t.*). Street names are not included, so directions can only say "turn left".
A loader is short:
def load_dimacs(gr_text: str, co_text: str):
"""Parse DIMACS .gr / .co text into 0-indexed lat, lon, adj."""
lat: list[float] = []
lon: list[float] = []
adj: list[list[tuple[int, int, str]]] = []
for line in co_text.splitlines():
if line.startswith("p "):
n = int(line.split()[-1])
lat, lon = [0.0] * n, [0.0] * n
elif line.startswith("v "):
_, i, x, y = line.split()
lon[int(i) - 1], lat[int(i) - 1] = int(x) / 1e6, int(y) / 1e6
for line in gr_text.splitlines():
if line.startswith("p "):
adj = [[] for _ in range(int(line.split()[2]))]
elif line.startswith("a "):
_, u, v, w = line.split()
adj[int(u) - 1].append((int(v) - 1, int(w), ""))
return lat, lon, adj
GR = "c tiny sample\np sp 3 4\na 1 2 803\na 2 1 803\na 2 3 1101\na 3 2 1101\n"
CO = "c tiny sample\np aux sp co 3\nv 1 -73530767 41085396\nv 2 -73530538 41086098\nv 3 -73519366 41048796\n"
lat, lon, adj = load_dimacs(GR, CO)
assert adj[0] == [(1, 803, "")] and len(adj[1]) == 2
assert abs(lat[0] - 41.085396) < 1e-9 and abs(lon[2] + 73.519366) < 1e-9
print("ok")
The weight units are whatever the challenge's documentation says they are — do not assume metres. The heuristic section shows how to calibrate a provably admissible heuristic without knowing the unit at all.
Option C: the synthetic grid city (offline)
The reference solution generates a 60 × 60 grid of intersections about 100 m apart, near latitude 45° N. East-west roads are "Street r", north-south roads "Avenue c". Every fifth street and avenue is straight; the others wind, so their length is 1.0 to 1.5 times the straight-line distance, drawn from a seeded Park-Miller generator so every language builds the same city. A river runs east-west through the middle and only avenues 0, 10, 20, … cross it. That gives 3,600 vertices and 7,026 two-way roads — small enough to run anywhere, irregular enough that A* has to work for its savings.
flowchart LR
SRC["Coordinates in"] --> SNAP["Snap to nearest vertex"]
SNAP --> DJ["Dijkstra: settle by g"]
SNAP --> AS["A*: settle by g + h"]
DJ --> CMP["Compare: length equal? expanded fewer?"]
AS --> CMP
AS --> PATH["Parent pointers to path"]
PATH --> TBT["Group edges by street, compute turns"]
TBT --> OUT["Directions out"]
The staged plan
Part 1 — Load the graph
Write a loader for your chosen source that produces lat, lon and adj with vertices numbered . Print , the number of edges, and the degree distribution; on a road network almost every vertex has degree 2 to 4, and anything wildly different means a parsing bug. Check connectivity with one BFS from vertex 0: real extracts usually contain small disconnected pieces (parking lots, roads cut off at the extract boundary). Either keep only the largest component or make your router report "unreachable" cleanly.
Part 2 — Snap coordinates to the graph
Users give coordinates, not vertex ids. The simplest snap is a linear scan for the vertex with the smallest haversine distance — per query, fine for a few thousand vertices. At city scale use a spatial index (a k-d tree, a grid of buckets or a geohash prefix; see spatial indexing and spatial trees). Production routers snap to the nearest point on an edge and split it, which matters on long rural roads; snapping to a vertex is good enough here.
Part 3 — Point-to-point Dijkstra
Take the Dijkstra from the overview and add three things: stop when the target is popped (its distance is final at that moment — that is the Dijkstra invariant), count non-stale pops, and record parents. With early exit, Dijkstra settles exactly the vertices closer to the source than the target (plus ties): a "ball" around the source whose radius is the answer. That ball is what A* shrinks.
Part 4 — A* with a haversine heuristic
A* is Dijkstra with the priority , where is the best known distance from the source and estimates the remaining distance to the target. The haversine formula gives the great-circle distance between two points on a sphere of radius (use the mean Earth radius, km):
with latitudes and longitudes in radians. Set .
Admissibility. A heuristic is admissible if it never overestimates: for every . Take any path . Every edge is at least as long as the straight line between its endpoints — a road cannot be shorter than the great-circle distance it spans — so . Great-circle distance is a metric, so the triangle inequality gives . Hence every path from to has length at least , and in particular the shortest one does. The synthetic city enforces the edge condition by construction: lengths are with factor .
Consistency. A stronger property holds: for every edge. Since , and is an integer, . Consistency means the reduced weights are non-negative, and A* is exactly Dijkstra on the reduced weights: every path from to has its length shifted by the same constant , so the shortest path is unchanged, and Dijkstra's proof applies verbatim. Each vertex is settled at most once with its final .
Why A settles no more than Dijkstra.* Let . Dijkstra with early exit settles vertices with . A* with a consistent heuristic settles vertices in non-decreasing order of and stops at the target, whose key is ; so it settles only vertices with . Since , that set is contained in Dijkstra's. The containment is exact except for vertices tied at key exactly , which tie-breaking may or may not settle; the reference breaks ties by vertex id in both algorithms, and the tests confirm the inequality on every query.
Real data. OSM edge lengths are summed along the road geometry, so they satisfy the edge condition (up to rounding — floor the heuristic or subtract a metre). DIMACS weights have their own unit and the TIGER geometry is imperfect, so calibrate instead: compute over all arcs with , and use . The same proof goes through with in place of , whatever the unit. On travel-time graphs, is effectively one over the fastest speed in the network, which makes the heuristic much weaker — a well-known limitation of geometric A*.
Part 5 — Compare the work
Run both algorithms on the same pairs and record length, expanded vertices and time. Assert equal lengths and expanded_astar <= expanded_dijkstra on every pair. Then look at where the savings come from: draw (or print as a character grid) which vertices each algorithm settled. Dijkstra's settled set is a disc around the source; A*'s is an elongated region pointing at the target.
Part 6 — Path reconstruction and turn-by-turn
Walk parent[] back from the target and reverse. Group consecutive edges with the same street name into legs and sum their lengths. For the turn between leg and leg , take the direction vector of the last edge of leg and the first edge of leg in a local east/north frame (, ). The sign of the 2-D cross product says which way you turned: positive is counter-clockwise, i.e. left. Real navigation adds "slight" and "sharp" turns by thresholding the angle, and "continue" when the name changes but the direction does not.
Acceptance tests
Your router passes when, on the synthetic 60 × 60 city with seed 42:
| Test | Expected |
|---|---|
| City size | 3,600 vertices, 7,026 roads |
| Snap (45.01083, 7.00957) and (45.03238, 7.02644) | row 12 col 8 (vertex 728) and row 36 col 22 (vertex 2182) |
| Route length, Dijkstra and A* | 3,827 m, both |
| Vertices settled | Dijkstra 1,492; A* 480 |
| Path check | sum of edge weights along each returned path equals 3,827 |
| Long diagonal, vertex 123 to vertex 3347 | 9,566 m; Dijkstra 3,457, A* 2,576 settled |
20 pairs from Lcg(7) | equal lengths on all; total settled Dijkstra 34,694, A* 11,847 |
| Source equals target | length 0, path [s], 1 settled |
The directions for the main query (A*'s path):
Head north on Avenue 8 for 131 m
Turn right onto Street 13 for 225 m
Turn left onto Avenue 10 for 1717 m
Turn right onto Street 30 for 950 m
Turn left onto Avenue 20 for 505 m
Turn right onto Street 35 for 190 m
Turn left onto Avenue 22 for 109 m
Arrive at destination
Avenue 10 is the bridge: the route detours west from avenue 8 to cross the river, then staircases east along the straight avenues and streets (multiples of 5), which have no winding penalty. Dijkstra may return a different path of the same length — many equal-length staircases exist on a grid — so compare lengths, not paths.
Reference solution
Reference solution (Python, Java, C++, TypeScript)
All four build the same city, run the same queries and assert the numbers in the acceptance table. Ties in the heap are broken by vertex id so the settled counts are identical across languages.
import heapq
import math
R_EARTH = 6_371_000.0 # mean Earth radius in metres
DEG = math.pi / 180.0
def haversine(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
"""Great-circle distance in metres."""
p1, p2 = lat1 * DEG, lat2 * DEG
s1 = math.sin((p2 - p1) / 2)
s2 = math.sin((lon2 - lon1) * DEG / 2)
a = s1 * s1 + math.cos(p1) * math.cos(p2) * s2 * s2
return 2 * R_EARTH * math.asin(math.sqrt(a))
class Lcg:
"""Park-Miller 'minimal standard' generator, so all four languages build the same city."""
def __init__(self, seed: int):
self.x = seed
def next(self) -> int:
self.x = self.x * 48271 % 2147483647
return self.x
class City:
"""rows x cols intersections ~100 m apart. Streets run east-west, avenues north-south.
Every 5th street/avenue is straight; the rest wind (length x 1.0-1.5).
A river south of row rows//2 is crossed only by avenues 0, 10, 20, ..."""
def __init__(self, rows: int, cols: int, seed: int = 42):
n = rows * cols
self.cols = cols
self.lat = [45.0 + (i // cols) * 0.0009 for i in range(n)]
self.lon = [7.0 + (i % cols) * 0.0012 for i in range(n)]
self.adj: list[list[tuple[int, int, str]]] = [[] for _ in range(n)]
rng = Lcg(seed)
for r in range(rows):
for c in range(cols):
for dr, dc in ((0, 1), (1, 0)):
nr, nc = r + dr, c + dc
if nr >= rows or nc >= cols:
continue
factor = 1.0 + (rng.next() % 500) / 1000.0
if dr == 0:
name, straight = f"Street {r}", r % 5 == 0
else:
name, straight = f"Avenue {c}", c % 5 == 0
if nr == rows // 2 and c % 10 != 0:
continue # river: no bridge here
if straight:
factor = 1.0
u, v = r * cols + c, nr * cols + nc
# ceil keeps every edge >= its straight-line length (needed for admissibility)
w = math.ceil(haversine(self.lat[u], self.lon[u], self.lat[v], self.lon[v]) * factor)
self.adj[u].append((v, w, name))
self.adj[v].append((u, w, name))
def nearest(self, lat: float, lon: float) -> int:
"""Snap a coordinate to the closest intersection (linear scan; use a k-d tree at scale)."""
return min(range(len(self.adj)), key=lambda v: haversine(lat, lon, self.lat[v], self.lon[v]))
def route(self, src: int, dst: int, astar: bool) -> tuple[int, list[int], int]:
"""Dijkstra (astar=False) or A* with h = floor(haversine to dst).
Returns (metres, path, expanded) where expanded = settled (non-stale) pops."""
def h(v: int) -> int:
return math.floor(haversine(self.lat[v], self.lon[v], self.lat[dst], self.lon[dst])) if astar else 0
g = [math.inf] * len(self.adj)
parent = [-1] * len(self.adj)
g[src] = 0
pq = [(h(src), src, 0)] # (f, vertex, g): ties broken by vertex id
expanded = 0
while pq:
_, u, gu = heapq.heappop(pq)
if gu > g[u]:
continue # stale entry
expanded += 1
if u == dst:
break
for v, w, _ in self.adj[u]:
if gu + w < g[v]:
g[v] = gu + w
parent[v] = u
heapq.heappush(pq, (g[v] + h(v), v, g[v]))
if g[dst] == math.inf:
return -1, [], expanded
path = [dst]
while path[-1] != src:
path.append(parent[path[-1]])
return int(g[dst]), path[::-1], expanded
def directions(self, path: list[int]) -> list[str]:
def edge(u: int, v: int) -> tuple[int, str]:
return next((w, name) for x, w, name in self.adj[u] if x == v)
def vec(u: int, v: int) -> tuple[float, float]: # local east/north components
return ((self.lon[v] - self.lon[u]) * math.cos(self.lat[u] * DEG), self.lat[v] - self.lat[u])
if len(path) < 2:
return ["You are already there"]
legs: list[list] = [] # [name, metres, first edge, last edge]
for u, v in zip(path, path[1:]):
w, name = edge(u, v)
if legs and legs[-1][0] == name:
legs[-1][1] += w
legs[-1][3] = (u, v)
else:
legs.append([name, w, (u, v), (u, v)])
dx, dy = vec(*legs[0][2])
compass = ("north" if dy > 0 else "south") if abs(dy) >= abs(dx) else ("east" if dx > 0 else "west")
out = [f"Head {compass} on {legs[0][0]} for {legs[0][1]} m"]
for prev, leg in zip(legs, legs[1:]):
ax, ay = vec(*prev[3])
bx, by = vec(*leg[2])
turn = "left" if ax * by - ay * bx > 0 else "right" # sign of the 2-D cross product
out.append(f"Turn {turn} onto {leg[0]} for {leg[1]} m")
out.append("Arrive at destination")
return out
def check_path(city: City, path: list[int], metres: int) -> bool:
total = 0
for u, v in zip(path, path[1:]):
total += next(w for x, w, _ in city.adj[u] if x == v)
return total == metres
city = City(60, 60)
assert len(city.adj) == 3600 and sum(map(len, city.adj)) // 2 == 7026
src = city.nearest(45.01083, 7.00957) # snaps to row 12, col 8
dst = city.nearest(45.03238, 7.02644) # snaps to row 36, col 22
assert (src, dst) == (12 * 60 + 8, 36 * 60 + 22)
d_m, d_path, d_exp = city.route(src, dst, astar=False)
a_m, a_path, a_exp = city.route(src, dst, astar=True)
assert d_m == a_m == 3827 # same optimal length
assert (d_exp, a_exp) == (1492, 480) # A* settles about 3x fewer vertices
assert check_path(city, d_path, d_m) and check_path(city, a_path, a_m)
assert city.directions(a_path) == [
"Head north on Avenue 8 for 131 m",
"Turn right onto Street 13 for 225 m",
"Turn left onto Avenue 10 for 1717 m",
"Turn right onto Street 30 for 950 m",
"Turn left onto Avenue 20 for 505 m",
"Turn right onto Street 35 for 190 m",
"Turn left onto Avenue 22 for 109 m",
"Arrive at destination",
]
# A long diagonal trip: the straight-line heuristic is weak on a grid.
far_d = city.route(2 * 60 + 3, 55 * 60 + 47, astar=False)
far_a = city.route(2 * 60 + 3, 55 * 60 + 47, astar=True)
assert far_d[0] == far_a[0] == 9566 and (far_d[2], far_a[2]) == (3457, 2576)
# 20 pseudo-random pairs: identical distances, A* never settles more vertices.
rng, tot_d, tot_a = Lcg(7), 0, 0
for _ in range(20):
s, t = rng.next() % 3600, rng.next() % 3600
rd, ra = city.route(s, t, False), city.route(s, t, True)
assert rd[0] == ra[0] and ra[2] <= rd[2]
tot_d, tot_a = tot_d + rd[2], tot_a + ra[2]
assert (tot_d, tot_a) == (34694, 11847)
assert city.route(100, 100, True) == (0, [100], 1)
print("\n".join(city.directions(a_path)))
print("ok")
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
public class RoadRouting {
static final double R_EARTH = 6_371_000.0;
static final double DEG = Math.PI / 180.0;
static double haversine(double lat1, double lon1, double lat2, double lon2) {
double p1 = lat1 * DEG, p2 = lat2 * DEG;
double s1 = Math.sin((p2 - p1) / 2);
double s2 = Math.sin((lon2 - lon1) * DEG / 2);
double a = s1 * s1 + Math.cos(p1) * Math.cos(p2) * s2 * s2;
return 2 * R_EARTH * Math.asin(Math.sqrt(a));
}
static final class Lcg {
long x;
Lcg(long seed) { x = seed; }
long next() { x = x * 48271 % 2147483647L; return x; }
}
record Edge(int to, int w, String name) {}
record Route(long metres, List<Integer> path, int expanded) {}
static final class City {
final double[] lat, lon;
final List<List<Edge>> adj = new ArrayList<>();
City(int rows, int cols, long seed) {
int n = rows * cols;
lat = new double[n];
lon = new double[n];
for (int i = 0; i < n; i++) {
lat[i] = 45.0 + (i / cols) * 0.0009;
lon[i] = 7.0 + (i % cols) * 0.0012;
adj.add(new ArrayList<>());
}
Lcg rng = new Lcg(seed);
int[][] dirs = {{0, 1}, {1, 0}};
for (int r = 0; r < rows; r++) {
for (int c = 0; c < cols; c++) {
for (int[] d : dirs) {
int nr = r + d[0], nc = c + d[1];
if (nr >= rows || nc >= cols) continue;
double factor = 1.0 + (rng.next() % 500) / 1000.0;
String name;
boolean straight;
if (d[0] == 0) {
name = "Street " + r;
straight = r % 5 == 0;
} else {
name = "Avenue " + c;
straight = c % 5 == 0;
if (nr == rows / 2 && c % 10 != 0) continue; // river
}
if (straight) factor = 1.0;
int u = r * cols + c, v = nr * cols + nc;
int w = (int) Math.ceil(haversine(lat[u], lon[u], lat[v], lon[v]) * factor);
adj.get(u).add(new Edge(v, w, name));
adj.get(v).add(new Edge(u, w, name));
}
}
}
}
int nearest(double la, double lo) {
int best = 0;
double bestD = Double.MAX_VALUE;
for (int v = 0; v < lat.length; v++) {
double d = haversine(la, lo, lat[v], lon[v]);
if (d < bestD) { bestD = d; best = v; }
}
return best;
}
long h(int v, int dst, boolean astar) {
return astar ? (long) Math.floor(haversine(lat[v], lon[v], lat[dst], lon[dst])) : 0;
}
Route route(int src, int dst, boolean astar) {
int n = adj.size();
long[] g = new long[n];
Arrays.fill(g, Long.MAX_VALUE);
int[] parent = new int[n];
Arrays.fill(parent, -1);
// entries {f, vertex, g}; order by f, then vertex id
PriorityQueue<long[]> pq = new PriorityQueue<>(
(a, b) -> a[0] != b[0] ? Long.compare(a[0], b[0]) : Long.compare(a[1], b[1]));
g[src] = 0;
pq.add(new long[] {h(src, dst, astar), src, 0});
int expanded = 0;
while (!pq.isEmpty()) {
long[] top = pq.poll();
int u = (int) top[1];
long gu = top[2];
if (gu > g[u]) continue; // stale entry
expanded++;
if (u == dst) break;
for (Edge e : adj.get(u)) {
if (gu + e.w() < g[e.to()]) {
g[e.to()] = gu + e.w();
parent[e.to()] = u;
pq.add(new long[] {g[e.to()] + h(e.to(), dst, astar), e.to(), g[e.to()]});
}
}
}
if (g[dst] == Long.MAX_VALUE) return new Route(-1, List.of(), expanded);
LinkedList<Integer> path = new LinkedList<>();
for (int v = dst; v != -1; v = parent[v]) path.addFirst(v);
return new Route(g[dst], path, expanded);
}
Edge edge(int u, int v) {
for (Edge e : adj.get(u)) if (e.to() == v) return e;
throw new IllegalArgumentException("no edge");
}
double[] vec(int u, int v) {
return new double[] {(lon[v] - lon[u]) * Math.cos(lat[u] * DEG), lat[v] - lat[u]};
}
List<String> directions(List<Integer> path) {
List<String> out = new ArrayList<>();
if (path.size() < 2) { out.add("You are already there"); return out; }
List<String> names = new ArrayList<>();
List<long[]> legs = new ArrayList<>(); // {metres, firstU, firstV, lastU, lastV}
for (int i = 0; i + 1 < path.size(); i++) {
int u = path.get(i), v = path.get(i + 1);
Edge e = edge(u, v);
if (!names.isEmpty() && names.get(names.size() - 1).equals(e.name())) {
long[] leg = legs.get(legs.size() - 1);
leg[0] += e.w();
leg[3] = u;
leg[4] = v;
} else {
names.add(e.name());
legs.add(new long[] {e.w(), u, v, u, v});
}
}
double[] d0 = vec((int) legs.get(0)[1], (int) legs.get(0)[2]);
String compass = Math.abs(d0[1]) >= Math.abs(d0[0])
? (d0[1] > 0 ? "north" : "south") : (d0[0] > 0 ? "east" : "west");
out.add("Head " + compass + " on " + names.get(0) + " for " + legs.get(0)[0] + " m");
for (int i = 1; i < legs.size(); i++) {
double[] a = vec((int) legs.get(i - 1)[3], (int) legs.get(i - 1)[4]);
double[] b = vec((int) legs.get(i)[1], (int) legs.get(i)[2]);
String turn = a[0] * b[1] - a[1] * b[0] > 0 ? "left" : "right";
out.add("Turn " + turn + " onto " + names.get(i) + " for " + legs.get(i)[0] + " m");
}
out.add("Arrive at destination");
return out;
}
boolean checkPath(List<Integer> path, long metres) {
long total = 0;
for (int i = 0; i + 1 < path.size(); i++) total += edge(path.get(i), path.get(i + 1)).w();
return total == metres;
}
}
static void check(boolean cond, String what) { if (!cond) throw new AssertionError(what); }
public static void main(String[] args) {
City city = new City(60, 60, 42);
int roads = 0;
for (List<Edge> es : city.adj) roads += es.size();
check(city.adj.size() == 3600 && roads / 2 == 7026, "size");
int src = city.nearest(45.01083, 7.00957), dst = city.nearest(45.03238, 7.02644);
check(src == 12 * 60 + 8 && dst == 36 * 60 + 22, "snap");
Route d = city.route(src, dst, false), a = city.route(src, dst, true);
check(d.metres() == 3827 && a.metres() == 3827, "length");
check(d.expanded() == 1492 && a.expanded() == 480, "expanded");
check(city.checkPath(d.path(), d.metres()) && city.checkPath(a.path(), a.metres()), "path");
check(city.directions(a.path()).equals(List.of(
"Head north on Avenue 8 for 131 m",
"Turn right onto Street 13 for 225 m",
"Turn left onto Avenue 10 for 1717 m",
"Turn right onto Street 30 for 950 m",
"Turn left onto Avenue 20 for 505 m",
"Turn right onto Street 35 for 190 m",
"Turn left onto Avenue 22 for 109 m",
"Arrive at destination")), "directions");
Route fd = city.route(2 * 60 + 3, 55 * 60 + 47, false), fa = city.route(2 * 60 + 3, 55 * 60 + 47, true);
check(fd.metres() == 9566 && fa.metres() == 9566 && fd.expanded() == 3457 && fa.expanded() == 2576, "diagonal");
Lcg rng = new Lcg(7);
long totD = 0, totA = 0;
for (int i = 0; i < 20; i++) {
int s = (int) (rng.next() % 3600), t = (int) (rng.next() % 3600);
Route rd = city.route(s, t, false), ra = city.route(s, t, true);
check(rd.metres() == ra.metres() && ra.expanded() <= rd.expanded(), "pair");
totD += rd.expanded();
totA += ra.expanded();
}
check(totD == 34694 && totA == 11847, "totals");
Route same = city.route(100, 100, true);
check(same.metres() == 0 && same.path().equals(List.of(100)) && same.expanded() == 1, "same vertex");
System.out.println(String.join("\n", city.directions(a.path())));
System.out.println("ok");
}
}
#include <cassert>
#include <cmath>
#include <cstdint>
#include <functional>
#include <iostream>
#include <limits>
#include <queue>
#include <stdexcept>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
constexpr double R_EARTH = 6371000.0;
constexpr double PI = 3.14159265358979323846;
constexpr double DEG = PI / 180.0;
double haversine(double lat1, double lon1, double lat2, double lon2) {
double p1 = lat1 * DEG, p2 = lat2 * DEG;
double s1 = std::sin((p2 - p1) / 2);
double s2 = std::sin((lon2 - lon1) * DEG / 2);
double a = s1 * s1 + std::cos(p1) * std::cos(p2) * s2 * s2;
return 2 * R_EARTH * std::asin(std::sqrt(a));
}
struct Lcg {
std::int64_t x;
std::int64_t next() { return x = x * 48271 % 2147483647LL; }
};
struct Edge { int to; int w; std::string name; };
struct Route { long long metres; std::vector<int> path; int expanded; };
struct City {
std::vector<double> lat, lon;
std::vector<std::vector<Edge>> adj;
City(int rows, int cols, std::int64_t seed) : lat(rows * cols), lon(rows * cols), adj(rows * cols) {
for (int i = 0; i < rows * cols; i++) {
lat[i] = 45.0 + (i / cols) * 0.0009;
lon[i] = 7.0 + (i % cols) * 0.0012;
}
Lcg rng{seed};
const int dirs[2][2] = {{0, 1}, {1, 0}};
for (int r = 0; r < rows; r++)
for (int c = 0; c < cols; c++)
for (const auto& d : dirs) {
int nr = r + d[0], nc = c + d[1];
if (nr >= rows || nc >= cols) continue;
double factor = 1.0 + static_cast<double>(rng.next() % 500) / 1000.0;
std::string name;
bool straight;
if (d[0] == 0) {
name = "Street " + std::to_string(r);
straight = r % 5 == 0;
} else {
name = "Avenue " + std::to_string(c);
straight = c % 5 == 0;
if (nr == rows / 2 && c % 10 != 0) continue; // river
}
if (straight) factor = 1.0;
int u = r * cols + c, v = nr * cols + nc;
int w = static_cast<int>(std::ceil(haversine(lat[u], lon[u], lat[v], lon[v]) * factor));
adj[u].push_back({v, w, name});
adj[v].push_back({u, w, name});
}
}
int nearest(double la, double lo) const {
int best = 0;
double bestD = std::numeric_limits<double>::max();
for (int v = 0; v < static_cast<int>(lat.size()); v++) {
double d = haversine(la, lo, lat[v], lon[v]);
if (d < bestD) { bestD = d; best = v; }
}
return best;
}
Route route(int src, int dst, bool astar) const {
auto h = [&](int v) -> long long {
return astar ? static_cast<long long>(std::floor(haversine(lat[v], lon[v], lat[dst], lon[dst]))) : 0;
};
const long long INF = std::numeric_limits<long long>::max();
std::vector<long long> g(adj.size(), INF);
std::vector<int> parent(adj.size(), -1);
using Item = std::tuple<long long, int, long long>; // (f, vertex, g): ties by vertex id
std::priority_queue<Item, std::vector<Item>, std::greater<Item>> pq;
g[src] = 0;
pq.push({h(src), src, 0});
int expanded = 0;
while (!pq.empty()) {
auto [f, u, gu] = pq.top();
pq.pop();
if (gu > g[u]) continue; // stale entry
expanded++;
if (u == dst) break;
for (const Edge& e : adj[u]) {
if (gu + e.w < g[e.to]) {
g[e.to] = gu + e.w;
parent[e.to] = u;
pq.push({g[e.to] + h(e.to), e.to, g[e.to]});
}
}
}
if (g[dst] == INF) return {-1, {}, expanded};
std::vector<int> path;
for (int v = dst; v != -1; v = parent[v]) path.insert(path.begin(), v);
return {g[dst], path, expanded};
}
const Edge& edge(int u, int v) const {
for (const Edge& e : adj[u])
if (e.to == v) return e;
throw std::invalid_argument("no edge");
}
std::pair<double, double> vec(int u, int v) const {
return {(lon[v] - lon[u]) * std::cos(lat[u] * DEG), lat[v] - lat[u]};
}
std::vector<std::string> directions(const std::vector<int>& path) const {
if (path.size() < 2) return {"You are already there"};
struct Leg { std::string name; long long metres; int fu, fv, lu, lv; };
std::vector<Leg> legs;
for (std::size_t i = 0; i + 1 < path.size(); i++) {
int u = path[i], v = path[i + 1];
const Edge& e = edge(u, v);
if (!legs.empty() && legs.back().name == e.name) {
legs.back().metres += e.w;
legs.back().lu = u;
legs.back().lv = v;
} else {
legs.push_back({e.name, e.w, u, v, u, v});
}
}
std::vector<std::string> out;
auto [dx, dy] = vec(legs[0].fu, legs[0].fv);
std::string compass = std::abs(dy) >= std::abs(dx) ? (dy > 0 ? "north" : "south") : (dx > 0 ? "east" : "west");
out.push_back("Head " + compass + " on " + legs[0].name + " for " + std::to_string(legs[0].metres) + " m");
for (std::size_t i = 1; i < legs.size(); i++) {
auto [ax, ay] = vec(legs[i - 1].lu, legs[i - 1].lv);
auto [bx, by] = vec(legs[i].fu, legs[i].fv);
std::string turn = ax * by - ay * bx > 0 ? "left" : "right";
out.push_back("Turn " + turn + " onto " + legs[i].name + " for " + std::to_string(legs[i].metres) + " m");
}
out.push_back("Arrive at destination");
return out;
}
bool checkPath(const std::vector<int>& path, long long metres) const {
long long total = 0;
for (std::size_t i = 0; i + 1 < path.size(); i++) total += edge(path[i], path[i + 1]).w;
return total == metres;
}
};
int main() {
City city(60, 60, 42);
std::size_t roads = 0;
for (const auto& es : city.adj) roads += es.size();
assert(city.adj.size() == 3600 && roads / 2 == 7026);
int src = city.nearest(45.01083, 7.00957), dst = city.nearest(45.03238, 7.02644);
assert(src == 12 * 60 + 8 && dst == 36 * 60 + 22);
Route d = city.route(src, dst, false), a = city.route(src, dst, true);
assert(d.metres == 3827 && a.metres == 3827);
assert(d.expanded == 1492 && a.expanded == 480);
assert(city.checkPath(d.path, d.metres) && city.checkPath(a.path, a.metres));
std::vector<std::string> expected = {
"Head north on Avenue 8 for 131 m", "Turn right onto Street 13 for 225 m",
"Turn left onto Avenue 10 for 1717 m", "Turn right onto Street 30 for 950 m",
"Turn left onto Avenue 20 for 505 m", "Turn right onto Street 35 for 190 m",
"Turn left onto Avenue 22 for 109 m", "Arrive at destination"};
assert(city.directions(a.path) == expected);
Route fd = city.route(2 * 60 + 3, 55 * 60 + 47, false), fa = city.route(2 * 60 + 3, 55 * 60 + 47, true);
assert(fd.metres == 9566 && fa.metres == 9566 && fd.expanded == 3457 && fa.expanded == 2576);
Lcg rng{7};
long long totD = 0, totA = 0;
for (int i = 0; i < 20; i++) {
int s = static_cast<int>(rng.next() % 3600), t = static_cast<int>(rng.next() % 3600);
Route rd = city.route(s, t, false), ra = city.route(s, t, true);
assert(rd.metres == ra.metres && ra.expanded <= rd.expanded);
totD += rd.expanded;
totA += ra.expanded;
}
assert(totD == 34694 && totA == 11847);
Route same = city.route(100, 100, true);
assert(same.metres == 0 && same.path == std::vector<int>{100} && same.expanded == 1);
for (const auto& line : city.directions(a.path)) std::cout << line << "\n";
std::cout << "ok\n";
}
const R_EARTH = 6_371_000;
const DEG = Math.PI / 180;
function haversine(lat1: number, lon1: number, lat2: number, lon2: number): number {
const p1 = lat1 * DEG, p2 = lat2 * DEG;
const s1 = Math.sin((p2 - p1) / 2);
const s2 = Math.sin(((lon2 - lon1) * DEG) / 2);
const a = s1 * s1 + Math.cos(p1) * Math.cos(p2) * s2 * s2;
return 2 * R_EARTH * Math.asin(Math.sqrt(a));
}
class Lcg {
x: number;
constructor(seed: number) { this.x = seed; }
next(): number { this.x = (this.x * 48271) % 2147483647; return this.x; } // exact: < 2^53
}
type Edge = { to: number; w: number; name: string };
type Route = { metres: number; path: number[]; expanded: number };
type Entry = [number, number, number]; // [f, vertex, g]
class EntryHeap {
a: Entry[] = [];
less(x: Entry, y: Entry): boolean { return x[0] !== y[0] ? x[0] < y[0] : x[1] < y[1]; }
push(e: Entry): void {
const a = this.a;
a.push(e);
for (let i = a.length - 1; i > 0; ) {
const p = (i - 1) >> 1;
if (!this.less(a[i], a[p])) break;
[a[i], a[p]] = [a[p], a[i]];
i = p;
}
}
pop(): Entry {
const a = this.a, top = a[0], last = a.pop()!;
if (a.length > 0) {
a[0] = last;
for (let i = 0; ; ) {
const l = 2 * i + 1, r = l + 1;
let m = i;
if (l < a.length && this.less(a[l], a[m])) m = l;
if (r < a.length && this.less(a[r], a[m])) m = r;
if (m === i) break;
[a[i], a[m]] = [a[m], a[i]];
i = m;
}
}
return top;
}
}
class City {
lat: number[] = [];
lon: number[] = [];
adj: Edge[][] = [];
constructor(rows: number, cols: number, seed: number) {
const n = rows * cols;
for (let i = 0; i < n; i++) {
this.lat.push(45.0 + Math.floor(i / cols) * 0.0009);
this.lon.push(7.0 + (i % cols) * 0.0012);
this.adj.push([]);
}
const rng = new Lcg(seed);
for (let r = 0; r < rows; r++) {
for (let c = 0; c < cols; c++) {
for (const [dr, dc] of [[0, 1], [1, 0]]) {
const nr = r + dr, nc = c + dc;
if (nr >= rows || nc >= cols) continue;
let factor = 1.0 + (rng.next() % 500) / 1000.0;
let name: string, straight: boolean;
if (dr === 0) {
name = `Street ${r}`;
straight = r % 5 === 0;
} else {
name = `Avenue ${c}`;
straight = c % 5 === 0;
if (nr === Math.floor(rows / 2) && c % 10 !== 0) continue; // river
}
if (straight) factor = 1.0;
const u = r * cols + c, v = nr * cols + nc;
const w = Math.ceil(haversine(this.lat[u], this.lon[u], this.lat[v], this.lon[v]) * factor);
this.adj[u].push({ to: v, w, name });
this.adj[v].push({ to: u, w, name });
}
}
}
}
nearest(la: number, lo: number): number {
let best = 0, bestD = Infinity;
for (let v = 0; v < this.lat.length; v++) {
const d = haversine(la, lo, this.lat[v], this.lon[v]);
if (d < bestD) { bestD = d; best = v; }
}
return best;
}
route(src: number, dst: number, astar: boolean): Route {
const h = (v: number): number =>
astar ? Math.floor(haversine(this.lat[v], this.lon[v], this.lat[dst], this.lon[dst])) : 0;
const g = new Array<number>(this.adj.length).fill(Infinity);
const parent = new Array<number>(this.adj.length).fill(-1);
const pq = new EntryHeap();
g[src] = 0;
pq.push([h(src), src, 0]);
let expanded = 0;
while (pq.a.length > 0) {
const [, u, gu] = pq.pop();
if (gu > g[u]) continue; // stale entry
expanded++;
if (u === dst) break;
for (const e of this.adj[u]) {
if (gu + e.w < g[e.to]) {
g[e.to] = gu + e.w;
parent[e.to] = u;
pq.push([g[e.to] + h(e.to), e.to, g[e.to]]);
}
}
}
if (g[dst] === Infinity) return { metres: -1, path: [], expanded };
const path: number[] = [];
for (let v = dst; v !== -1; v = parent[v]) path.push(v);
return { metres: g[dst], path: path.reverse(), expanded };
}
edge(u: number, v: number): Edge {
const e = this.adj[u].find((x) => x.to === v);
if (!e) throw new Error("no edge");
return e;
}
vec(u: number, v: number): [number, number] {
return [(this.lon[v] - this.lon[u]) * Math.cos(this.lat[u] * DEG), this.lat[v] - this.lat[u]];
}
directions(path: number[]): string[] {
if (path.length < 2) return ["You are already there"];
const legs: { name: string; metres: number; first: [number, number]; last: [number, number] }[] = [];
for (let i = 0; i + 1 < path.length; i++) {
const u = path[i], v = path[i + 1];
const e = this.edge(u, v);
const prev = legs[legs.length - 1];
if (prev && prev.name === e.name) {
prev.metres += e.w;
prev.last = [u, v];
} else {
legs.push({ name: e.name, metres: e.w, first: [u, v], last: [u, v] });
}
}
const [dx, dy] = this.vec(...legs[0].first);
const compass = Math.abs(dy) >= Math.abs(dx) ? (dy > 0 ? "north" : "south") : dx > 0 ? "east" : "west";
const out = [`Head ${compass} on ${legs[0].name} for ${legs[0].metres} m`];
for (let i = 1; i < legs.length; i++) {
const [ax, ay] = this.vec(...legs[i - 1].last);
const [bx, by] = this.vec(...legs[i].first);
const turn = ax * by - ay * bx > 0 ? "left" : "right";
out.push(`Turn ${turn} onto ${legs[i].name} for ${legs[i].metres} m`);
}
out.push("Arrive at destination");
return out;
}
checkPath(path: number[], metres: number): boolean {
let total = 0;
for (let i = 0; i + 1 < path.length; i++) total += this.edge(path[i], path[i + 1]).w;
return total === metres;
}
}
function check(cond: boolean, what: string): void {
if (!cond) throw new Error(what);
}
const city = new City(60, 60, 42);
check(city.adj.length === 3600 && city.adj.reduce((s, es) => s + es.length, 0) / 2 === 7026, "size");
const src = city.nearest(45.01083, 7.00957), dst = city.nearest(45.03238, 7.02644);
check(src === 12 * 60 + 8 && dst === 36 * 60 + 22, "snap");
const d = city.route(src, dst, false), a = city.route(src, dst, true);
check(d.metres === 3827 && a.metres === 3827, "length");
check(d.expanded === 1492 && a.expanded === 480, "expanded");
check(city.checkPath(d.path, d.metres) && city.checkPath(a.path, a.metres), "path");
check(JSON.stringify(city.directions(a.path)) === JSON.stringify([
"Head north on Avenue 8 for 131 m",
"Turn right onto Street 13 for 225 m",
"Turn left onto Avenue 10 for 1717 m",
"Turn right onto Street 30 for 950 m",
"Turn left onto Avenue 20 for 505 m",
"Turn right onto Street 35 for 190 m",
"Turn left onto Avenue 22 for 109 m",
"Arrive at destination",
]), "directions");
const fd = city.route(2 * 60 + 3, 55 * 60 + 47, false), fa = city.route(2 * 60 + 3, 55 * 60 + 47, true);
check(fd.metres === 9566 && fa.metres === 9566 && fd.expanded === 3457 && fa.expanded === 2576, "diagonal");
const rng = new Lcg(7);
let totD = 0, totA = 0;
for (let i = 0; i < 20; i++) {
const s = rng.next() % 3600, t = rng.next() % 3600;
const rd = city.route(s, t, false), ra = city.route(s, t, true);
check(rd.metres === ra.metres && ra.expanded <= rd.expanded, "pair");
totD += rd.expanded;
totA += ra.expanded;
}
check(totD === 34694 && totA === 11847, "totals");
const same = city.route(100, 100, true);
check(same.metres === 0 && same.path.length === 1 && same.path[0] === 100 && same.expanded === 1, "same vertex");
console.log(city.directions(a.path).join("\n"));
console.log("ok");
Benchmarks and what the numbers should look like
On the synthetic city. The acceptance table is the benchmark: on the main query A* settles 480 vertices to Dijkstra's 1,492, a factor of 3.1; over the 20 random pairs the totals are 11,847 versus 34,694, a factor of 2.9. On the long diagonal the factor drops to 1.3 (2,576 versus 3,457). That is not a bug. On a grid the shortest route has to staircase, so its length (9,566 m) is about 1.4 times the straight line (6,734 m), and the heuristic's underestimate grows with the trip. A* prunes the vertices whose exceeds the route length; the larger the gap between and the truth, the fewer it can prune. Real cities with diagonal arterials are friendlier to the straight-line heuristic than this grid.
On real data, measure it yourself:
- Fix the random seed and draw 100–1,000 source/target pairs, uniformly over vertices of the largest component.
- For each pair, run Dijkstra and A* and record length, settled vertices, heap pushes, and wall-clock time (warm the JIT first in Java and TypeScript; take the median of several runs).
- Report the median and 90th percentile of the settled-vertex ratio, and plot ratio against route length.
What to expect qualitatively. Both algorithms are in the worst case; the difference is the constant fraction of the graph they touch. Dijkstra with early exit settles every vertex closer than the target, so its work grows with the square of the route length on a planar road network. A*'s savings grow with route length on distance-weighted graphs, shrink on travel-time graphs (where the heuristic divides by the top speed), and vanish when the target is unreachable (both explore the whole component). Wall-clock time follows settled vertices closely, but A* pays for a haversine per push — about the cost of a handful of trigonometric calls — so its time ratio is a little worse than its settled ratio. If A* ever settles more vertices than Dijkstra, or returns a different length, your heuristic is not admissible: check edge lengths against straight-line distances.
Stretch goals
- Bidirectional Dijkstra. Run one search forward from and one backward from (on reversed edges), alternating. Keep , the best – length seen so far: whenever either search relaxes an edge where the other search has already reached , update . Stop when the sum of the two queues' minimum keys is at least . The classic bug is stopping when a vertex is settled by both searches and taking the path through it — that vertex need not lie on a shortest path. Expect roughly half of Dijkstra's settled vertices: two balls of radius instead of one of radius .
- One-way streets and turn restrictions. Make some streets directed. Then forbid left turns at some intersections: that changes the state from "vertex" to "(vertex, incoming edge)" — an edge-based graph — and is a good exercise in graph modelling.
- Travel time. Give avenues 50 km/h and streets 30 km/h, weight edges by seconds, and rebuild the heuristic as distance divided by the top speed. Measure how much A*'s advantage shrinks.
- ALT (A, landmarks, triangle inequality).* Pick 8–16 landmarks on the boundary of the map, precompute distances from each landmark with Dijkstra, and use , which is admissible by the triangle inequality. On road networks this is usually a much tighter bound than straight-line distance (Goldberg and Harrelson, 2005).
- Snap to edges. Snap a coordinate to the nearest point on any road segment, split that edge, and route from there. Use a grid or k-d tree over segment bounding boxes.
- Run it on a real city with Option A or B, and write up the benchmark numbers in the format above.
Further reading: how production routers do it
Plain Dijkstra and A* are where routing starts, not where it ends. A continental road network has tens of millions of vertices; even A* settles too many of them per query for an interactive service. Production engines precompute.
flowchart LR
DJ["Dijkstra: ball of radius D"] --> BI["Bidirectional: two balls of radius D/2"]
DJ --> AS["A*: geometric heuristic"]
AS --> ALT["ALT: landmark heuristic"]
BI --> CH["Contraction hierarchies: upward-only search"]
ALT --> CH
CH --> MLD["Multi-level partitions: live traffic"]
- Contraction hierarchies (Geisberger, Sanders, Schultes and Delling, "Contraction Hierarchies: Faster and Simpler Hierarchical Routing in Road Networks", WEA 2008). Order the vertices by "importance", then contract them one by one from least to most important, adding shortcut edges whenever removing a vertex would lengthen some shortest path between its neighbours. A query is a bidirectional Dijkstra that only ever moves upward in the order; it settles a few hundred vertices on a continental graph instead of millions. This is what OSRM's CH mode and GraphHopper's speed mode are built on.
- Multi-level partitioning / customizable route planning. Split the graph into cells, precompute distances across cell boundaries, and keep the metric (traffic, closures) separate from the topology so it can be updated in seconds.
- The survey. Bast, Delling, Goldberg, Müller-Hannemann, Pajor, Sanders, Wagner and Werneck, "Route Planning in Transportation Networks" (2016, arXiv:1504.05140) compares all of these on the same data.
- Background. Hart, Nilsson and Raphael, "A Formal Basis for the Heuristic Determination of Minimum Cost Paths" (1968) introduced A*. The shortest-paths lesson proves A*'s optimality in general; spatial indexing covers the geometry side; network layer shows the same Dijkstra running inside OSPF routers.
Course navigation. Previous: Graph Algorithms in the Real World. Module overview: Graphs. Course: DSA: Zero to Mastery. Next: Module 5 (recursion, backtracking and greedy).