Informed Search Lab

# Minimum spanning trees for the muddy city problem (Section 4.2): Kruskal, Prim and Borůvka
import heapq

EDGES = [("A", "B", 10), ("A", "C", 5), ("A", "D", 3), ("A", "E", 4), ("B", "C", 2), ("B", "F", 6), ("C", "E", 11),
         ("C", "F", 3), ("C", "G", 2), ("D", "E", 3), ("D", "H", 5), ("E", "G", 5), ("E", "H", 1), ("F", "G", 2),
         ("F", "I", 7), ("F", "J", 13), ("G", "H", 4), ("G", "I", 3), ("H", "I", 6), ("I", "J", 2)]   # Listing 4.1
NODES = sorted({n for e in EDGES for n in e[:2]})


class DisjointSet:
    def __init__(self, items):
        self.parent = {i: i for i in items}

    def find(self, x):
        while self.parent[x] != x:
            self.parent[x] = self.parent[self.parent[x]]
            x = self.parent[x]
        return x

    def union(self, a, b):
        ra, rb = self.find(a), self.find(b)
        if ra == rb:
            return False                    # same component: the edge would close a cycle
        self.parent[ra] = rb
        return True


def kruskal(nodes, edges):
    """Algorithm 4.1: add edges by increasing weight unless they form a cycle."""
    ds, tree = DisjointSet(nodes), []
    for u, v, w in sorted(edges, key=lambda e: e[2]):
        if ds.union(u, v):
            tree.append((u, v, w))
            print(f"add {u}-{v} ({w})")
        else:
            print(f"skip {u}-{v} ({w}): cycle")
        if len(tree) == len(nodes) - 1:
            break
    return tree


def prim(nodes, edges, root="A"):
    """Jarník-Prim: grow one tree from the root, always taking the cheapest edge that leaves it."""
    adj = {n: [] for n in nodes}
    for u, v, w in edges:
        adj[u].append((w, u, v)); adj[v].append((w, v, u))
    inside, heap, tree = {root}, list(adj[root]), []
    heapq.heapify(heap)
    while heap and len(inside) < len(nodes):
        w, u, v = heapq.heappop(heap)
        if v in inside:
            continue
        inside.add(v); tree.append((u, v, w))
        for e in adj[v]:
            if e[2] not in inside:
                heapq.heappush(heap, e)
    return tree


def boruvka(nodes, edges):
    """Borůvka: every component picks its cheapest outgoing edge; merge; repeat."""
    ds, tree = DisjointSet(nodes), set()
    while len(tree) < len(nodes) - 1:
        cheapest = {}
        for u, v, w in edges:
            ru, rv = ds.find(u), ds.find(v)
            if ru == rv:
                continue
            for r in (ru, rv):
                if r not in cheapest or w < cheapest[r][2]:
                    cheapest[r] = (u, v, w)
        for u, v, w in cheapest.values():
            if ds.union(u, v):
                tree.add((u, v, w))
    return list(tree)


T = kruskal(NODES, EDGES)
print("Kruskal:", sum(w for *_, w in T))          # total cost 22
print("Prim:   ", sum(w for *_, w in prim(NODES, EDGES)))
print("Borůvka:", sum(w for *_, w in boruvka(NODES, EDGES)))

# Hill climbing and A* (Sections 4.3.1 and 4.3.3)
import heapq
import math
import random

# 1. Hill climbing on a function with a local and a global maximum (like Figure 4.12)
f = lambda x: 14 * math.exp(-(x - 1.8) ** 2 / 1.2) + 12 * math.exp(-(x + 2.3) ** 2 / 0.5) - 2.6 * x ** 2 + 4


def steepest_ascent(x, step=0.05):
    while True:
        best = max([x - step, x + step], key=f)
        if f(best) <= f(x) or not -4 <= best <= 4:
            return x
        x = best


def random_restart(n=10, seed=0):
    rng = random.Random(seed)
    return max((steepest_ascent(rng.uniform(-4, 4)) for _ in range(n)), key=f)


print(f"start -3.5 -> x = {steepest_ascent(-3.5):.2f}, f = {f(steepest_ascent(-3.5)):.2f}   (local maximum)")
print(f"start  0.5 -> x = {steepest_ascent(0.5):.2f}, f = {f(steepest_ascent(0.5)):.2f}   (global maximum)")
print(f"random restart: x = {random_restart():.2f}")

# 2. The 8-puzzle with heuristics; the blank (0) moves left, up, right or down
GOAL = (0, 1, 2, 3, 4, 5, 6, 7, 8)


def expand(s):
    i = s.index(0)
    for name, d in (("Left", -1), ("Up", -3), ("Right", 1), ("Down", 3)):
        j = i + d
        if name == "Left" and i % 3 == 0 or name == "Right" and i % 3 == 2 or not 0 <= j < 9:
            continue
        t = list(s); t[i], t[j] = t[j], t[i]
        yield tuple(t)


misplaced = lambda s: sum(1 for i, v in enumerate(s) if v and v != i)          # excluding the blank
manhattan = lambda s: sum(abs(i // 3 - v // 3) + abs(i % 3 - v % 3) for i, v in enumerate(s) if v)


def hill_climbing(s, h):
    """Steepest descent on h (Listing 4.3): move to the best neighbor while it improves."""
    path = [s]
    while s != GOAL:
        best = min(expand(s), key=h)
        if h(best) >= h(s):
            break                                   # stuck in a local minimum or on a plateau
        s = best; path.append(s)
    return path


def a_star(s, h):
    """A*: expand the state with the lowest f = g + h; with an admissible h the first goal popped is optimal."""
    pq, g, parent, closed, k = [(h(s), 0, s)], {s: 0}, {s: None}, set(), 0
    while pq:
        _, _, u = heapq.heappop(pq)
        if u in closed:
            continue
        closed.add(u)
        if u == GOAL:
            path = []
            while u:
                path.append(u); u = parent[u]
            return path[::-1], len(closed)
        for v in expand(u):
            if g[u] + 1 < g.get(v, math.inf):
                g[v], parent[v] = g[u] + 1, u
                k += 1; heapq.heappush(pq, (g[v] + h(v), k, v))


start = (1, 4, 2, 3, 7, 5, 6, 0, 8)                 # Listing 4.3
path = hill_climbing(start, misplaced)
print("hill climbing, misplaced tiles:", len(path) - 1, "moves, h along the way:", [misplaced(p) for p in path])
hard = (8, 6, 7, 2, 5, 4, 3, 0, 1)                 # a hard board: 27 moves
print("hill climbing on a hard board stops at h =", misplaced(hill_climbing(hard, misplaced)[-1]))
for name, h in (("misplaced tiles", misplaced), ("Manhattan distance", manhattan), ("h = 0 (UCS)", lambda s: 0)):
    p, n = a_star(hard, h)
    print(f"A* with {name}: {len(p) - 1} moves, {n} states expanded")

# Informed search on the 8 points-of-interest road network (Figures 4.14 to 4.22, Listing 4.4)
import heapq

EDGES = [("A", "B", 4), ("A", "C", 3), ("A", "S", 2), ("B", "D", 3), ("B", "S", 4), ("C", "E", 4), ("C", "D", 5),
         ("D", "E", 6), ("D", "F", 5), ("E", "F", 7), ("E", "G", 3), ("F", "G", 3)]
H = {"S": 11.3, "A": 9.5, "B": 10.2, "C": 5.6, "D": 6.2, "E": 4.4, "F": 3.8, "G": 0.0}   # straight-line distance to G
G = {}
for u, v, w in EDGES:
    G.setdefault(u, {})[v] = w
    G.setdefault(v, {})[u] = w
cost = lambda path: sum(G[a][b] for a, b in zip(path, path[1:]))


def best_first(start, goal, f):
    """Generic best-first search on partial paths. f(path) gives the priority; returns the route and the expansion log."""
    pq, log = [(f([start]), [start])], []
    while pq:
        val, path = heapq.heappop(pq)
        node = path[-1]
        log.append(("->".join(path), cost(path), val))
        if node == goal:
            return path, log
        for nb in sorted(G[node]):
            if nb not in path:                       # no cycles within a route
                heapq.heappush(pq, (f(path + [nb]), path + [nb]))


def show(name, f, w=1):
    path, log = best_first("S", "G", f)
    print(f"{name}: {'-'.join(path)} (cost {cost(path)}), {len(log)} expansions")
    for p, g, v in log:
        print(f"   {p:12s} g = {g:2d}  priority = {v:.1f}")


show("Uniform-cost search, f = g", lambda p: cost(p))
show("Greedy best-first, f = h", lambda p: H[p[-1]])
show("A*, f = g + h", lambda p: cost(p) + H[p[-1]])
show("Weighted A*, f = g + 2h", lambda p: cost(p) + 2 * H[p[-1]])

# Admissibility check: h(n) must never exceed the true remaining cost h*(n)
dist = {"G": 0}
pq = [(0, "G")]
while pq:
    d, n = heapq.heappop(pq)
    for m, w in G[n].items():
        if d + w < dist.get(m, 1e9):
            dist[m] = d + w; heapq.heappush(pq, (d + w, m))
print("node  h    h*   admissible")
for n in sorted(H):
    print(f"{n}    {H[n]:4.1f} {dist[n]:4.1f}  {'yes' if H[n] <= dist[n] else 'NO'}")


def beam_search(start, goal, w=2):
    """Keep the w cheapest partial routes at each level (cost so far as the score)."""
    beam = [[start]]
    while beam:
        children = [p + [nb] for p in beam for nb in sorted(G[p[-1]]) if nb not in p]
        done = [p for p in children if p[-1] == goal]
        if done:
            return min(done, key=cost)
        beam = sorted(children, key=cost)[:w]
        print("   beam:", [("-".join(p), cost(p)) for p in beam])


r = beam_search("S", "G", w=2)
print("Beam search (w = 2):", "-".join(r), cost(r))

# Contraction hierarchies on the 12-node example (Figures 4.29 to 4.46, Listing 4.6)
import heapq
import math

EDGES = [(1, 2, 2), (1, 10, 3), (2, 4, 7), (2, 10, 4), (3, 5, 4), (3, 8, 3), (3, 9, 5), (4, 8, 5), (4, 10, 6),
         (4, 12, 3), (5, 9, 5), (6, 7, 5), (6, 10, 3), (6, 11, 4), (7, 9, 6), (7, 11, 2), (8, 9, 3), (8, 12, 1),
         (9, 11, 3), (10, 12, 1), (11, 12, 4)]
G = {n: {} for n in range(1, 13)}
for u, v, w in EDGES:
    G[u][v] = G[v][u] = w


def dijkstra(graph, s, banned=(), limit=math.inf):
    d, pq = {s: 0}, [(0, s)]
    while pq:
        g, u = heapq.heappop(pq)
        if g > d[u] or g > limit:
            continue
        for v, w in graph[u].items():
            if v not in banned and g + w < d.get(v, math.inf):
                d[v] = g + w; heapq.heappush(pq, (g + w, v))
    return d


def contract(order):
    """Contract nodes from least to most important; add a shortcut u-w whenever the only shortest u-w path runs through v."""
    graph = {n: dict(nb) for n, nb in G.items()}       # the overlay graph: original edges plus shortcuts
    rank, middle, done = {n: i for i, n in enumerate(order)}, {}, set()
    for v in order:
        nbrs = [u for u in graph[v] if u not in done]
        for i, u in enumerate(nbrs):
            for w in nbrs[i + 1:]:
                via = graph[u][v] + graph[v][w]
                witness = dijkstra(graph, u, banned=done | {v}, limit=via).get(w, math.inf)
                if witness > via:                          # no witness path: a shortcut is needed
                    if via < graph[u].get(w, math.inf):
                        graph[u][w] = graph[w][u] = via
                        middle[(u, w)] = middle[(w, u)] = v
                        print(f"contract {v}: shortcut {u}-{w} ({via})")
        done.add(v)
    return graph, rank, middle


def query(graph, rank, s, t):
    """Bidirectional Dijkstra that only relaxes edges toward higher-ranked nodes, in both directions."""
    d, par, pq, settled = [{s: 0}, {t: 0}], [{s: None}, {t: None}], [[(0, s)], [(0, t)]], [set(), set()]
    for side in (0, 1):                                   # full upward searches (the graphs are tiny)
        while pq[side]:
            g, u = heapq.heappop(pq[side])
            if u in settled[side]:
                continue
            settled[side].add(u)
            for v, w in graph[u].items():
                if rank[v] > rank[u] and g + w < d[side].get(v, math.inf):
                    d[side][v], par[side][v] = g + w, u
                    heapq.heappush(pq[side], (g + w, v))
    meet = min(set(d[0]) & set(d[1]), key=lambda n: d[0][n] + d[1][n])
    up = []
    n = meet
    while n is not None:
        up.append(n); n = par[0][n]
    down = []
    n = par[1][meet]
    while n is not None:
        down.append(n); n = par[1][n]
    return up[::-1] + down, d[0][meet] + d[1][meet], meet, len(settled[0]) + len(settled[1])


def unpack(u, w, middle):
    if (u, w) not in middle:
        return [u]
    v = middle[(u, w)]
    return unpack(u, v, middle) + unpack(v, w, middle)


graph, rank, middle = contract(list(range(1, 13)))       # the book's arbitrary order 1, 2, ..., 12
route, length, meet, settled = query(graph, rank, 1, 5)
full = [n for u, w in zip(route, route[1:]) for n in unpack(u, w, middle)] + [route[-1]]
print("route on the overlay graph:", route, "meeting node", meet)
print("unpacked route:", full, "cost", length, f"({settled} nodes settled)")      # 1-10-12-8-3-5, cost 12

# Informed search on the University of Toronto road network (Section 4.4)
# !pip install osmnx          # uncomment in Colab
import heapq
import math
import random
import osmnx

G = osmnx.graph_from_point((43.661667, -79.395), dist=300, simplify=True)
O = osmnx.distance.nearest_nodes(G, -79.392442, 43.664527)          # King Edward VII equestrian statue
T = osmnx.distance.nearest_nodes(G, -79.397669, 43.659659)          # Bahen Centre
adj = {n: {} for n in G.nodes}
for u, v, d in G.edges(data=True):
    if u != v and d["length"] < adj[u].get(v, math.inf):
        adj[u][v] = d["length"]                                     # shortest parallel edge, no self-loops


def haversine(a, b):
    la1, lo1, la2, lo2 = map(math.radians, (G.nodes[a]["y"], G.nodes[a]["x"], G.nodes[b]["y"], G.nodes[b]["x"]))
    h = math.sin((la2 - la1) / 2) ** 2 + math.cos(la1) * math.cos(la2) * math.sin((lo2 - lo1) / 2) ** 2
    return 2 * 6_371_000 * math.asin(math.sqrt(h))


length = lambda r: sum(adj[a][b] for a, b in zip(r, r[1:]))


def trace(parent, n):
    r = []
    while n is not None:
        r.append(n); n = parent[n]
    return r[::-1]


def a_star(weight=1.0):
    """A*: f = g + w·h, with h the haversine distance to the destination (admissible for w = 1)."""
    g, parent, closed, pq = {O: 0}, {O: None}, set(), [(0, O)]
    while pq:
        _, u = heapq.heappop(pq)
        if u in closed:
            continue
        closed.add(u)
        if u == T:
            return trace(parent, u), len(closed)
        for v, w in adj[u].items():
            if g[u] + w < g.get(v, math.inf):
                g[v], parent[v] = g[u] + w, u
                heapq.heappush(pq, (g[v] + weight * haversine(v, T), v))


def book_astar():
    """The book's Listing 4.9 scoring: straight-line distance from the origin plus to the destination, without g(n)."""
    score = {n: haversine(O, n) + haversine(n, T) for n in G.nodes}
    frontier, parent, explored = [O], {O: None}, set()
    while frontier:
        u = min(frontier, key=score.get)
        frontier.remove(u); explored.add(u)
        for v in adj[u]:
            if v not in explored and v not in frontier:
                parent[v] = u
                if v == T:
                    return trace(parent, v), len(explored)
                frontier.append(v)


def random_route(rng):
    """Randomized blind search (optalgotools.routing.randomized_search): expand a random frontier node."""
    frontier, parent, explored = [O], {O: None}, set()
    while frontier:
        u = frontier.pop(rng.randrange(len(frontier)))
        explored.add(u)
        for v in adj[u]:
            if v not in explored and v not in parent:
                parent[v] = u
                if v == T:
                    return trace(parent, v)
                frontier.append(v)


def detour(route, i, j):
    """Child route: drop route[i..j] and reconnect route[i-1] to route[j+1] by the shortest path avoiding the rest of the route."""
    a, b, banned = route[i - 1], route[j + 1], set(route[:i - 1]) | set(route[j + 2:]) | set(route[i:j + 1])
    d, parent, pq = {a: 0}, {a: None}, [(0, a)]
    while pq:
        g, u = heapq.heappop(pq)
        if u == b:
            return route[:i - 1] + trace(parent, b) + route[j + 2:]
        if g > d[u]:
            continue
        for v, w in adj[u].items():
            if v not in banned and g + w < d.get(v, math.inf):
                d[v], parent[v] = g + w, u
                heapq.heappush(pq, (g + w, v))


def children(route, n):
    out = []
    for i in range(1, len(route) - 1):
        for j in range(i, len(route) - 1):
            c = detour(route, i, j)
            if c:
                out.append(c)
                if len(out) == n:
                    return out
    return out


def hill_climbing(n=20, seed=0):
    current = random_route(random.Random(seed))
    while True:
        best = min(children(current, n), key=length, default=current)
        if length(best) >= length(current):
            return current
        current = best


def beam_search(k=10, n=20, seed=0):
    rng = random.Random(seed)
    beam, seen = [random_route(rng) for _ in range(k)], set()
    while True:
        pool = {tuple(c) for r in beam for c in children(r, n)} | {tuple(r) for r in beam}
        new = [list(r) for r in heapq.nsmallest(k, pool - seen | {tuple(r) for r in beam}, key=length)]
        seen |= pool
        if [length(r) for r in new] == [length(r) for r in beam]:
            return min(beam, key=length)
        beam = new


for name, run in [("Hill climbing (n = 20)", lambda: (hill_climbing(), None)), ("Beam search (k = 10, n = 20)", lambda: (beam_search(), None)),
                  ("Book's A* (origin + destination distances)", book_astar), ("A* (g + haversine)", a_star),
                  ("Weighted A* (w = 2)", lambda: a_star(2))]:
    r, explored = run()
    print(f"{name:44s} {length(r):7.1f} m" + (f", {explored} nodes explored" if explored else ""))