From 0c0f013f94d25405d594c72b542c78c14d716b85 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Mon, 12 Oct 2026 13:59:54 +1000 Subject: [PATCH] perf: path_BFS, path_UCS and path_Astar no longer scan lists The frontier and explored sets were lists searched with 'in', and UCS and A* took np.argmin over the whole frontier at every expansion, so the cost grew roughly as V*E (1.8 power of V in the benchmark). BFS now uses a deque and a set of vertices seen; UCS and A* use a heap with a dict as ordered frontier, reparented vertices being pushed again and stale entries skipped. The path is built by appending and reversing rather than inserting at the front. The ties are broken as before, by the order vertices first joined the frontier, so results and verbose transcripts are unchanged: identical on 10 800 random tie-heavy cases against the previous code, and pinned by tests/test_planner_choices.py. On 10 000 vertices (directed, ~6 edges/vertex), per query: BFS 145 -> 3.1 ms, UCS 160 -> 6.7 ms, A* 17 -> 3.3 ms. Co-Authored-By: Claude Sonnet 5.5 --- src/pgraph/PGraph.py | 63 +++++++--- tests/test_planner_choices.py | 231 ++++++++++++++++++++++++++++++++++ 2 files changed, 276 insertions(+), 18 deletions(-) create mode 100644 tests/test_planner_choices.py diff --git a/src/pgraph/PGraph.py b/src/pgraph/PGraph.py index ae0e02b7..96b03d62 100644 --- a/src/pgraph/PGraph.py +++ b/src/pgraph/PGraph.py @@ -10,6 +10,8 @@ from matplotlib.transforms import Affine2D, ScaledTranslation from mpl_toolkits.mplot3d import proj3d import copy +import heapq +from collections import deque from collections.abc import Iterable, Iterator import tempfile import webbrowser @@ -3072,7 +3074,10 @@ def path_BFS( # we use lists not sets since the order is instructive in verbose # mode, really need ordered sets... - frontier: list[Vertex] = [S] + # the frontier is a queue; ``seen`` is everything that has been on it, + # so that "frontier or explored" is a set lookup, not a list scan + frontier: deque[Vertex] = deque([S]) + seen: set[Vertex] = {S} explored: list[Vertex] = [] parent: dict[Vertex, Vertex] = {} done = False @@ -3083,7 +3088,7 @@ def path_BFS( print("FRONTIER:", ", ".join([str(v.name) for v in frontier])) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - x = frontier.pop(0) + x = frontier.popleft() if verbose: print(" expand", x.name) @@ -3097,8 +3102,9 @@ def path_BFS( parent[n] = x done = True break - if n not in frontier and n not in explored: + if n not in seen: # add it to the frontier + seen.add(n) frontier.append(n) if verbose: print(" add", n.name, "to the frontier") @@ -3120,8 +3126,9 @@ def path_BFS( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() if summary or verbose: print( @@ -3182,8 +3189,14 @@ def path_UCS( S = self._resolve_vertex(S, "start") G = self._resolve_vertex(G, "goal") - frontier: list[Vertex] = [S] - explored: list[Vertex] = [] + # The frontier is a dict, used as an ordered set: its order is the order + # vertices first joined it, which breaks ties between equal ``f``, and + # its value is that position. ``heap`` finds the minimum ``f``; a + # vertex that is reparented is pushed again, and the older entry is + # recognised as stale when it is popped. + frontier: dict[Vertex, int] = {S: 0} + heap: list[tuple[float, int, Vertex]] = [(0, 0, S)] + explored: dict[Vertex, None] = {} parent: dict[Vertex, Vertex] = {} f: dict[Vertex, float] = {S: 0} # evaluation function @@ -3195,8 +3208,11 @@ def path_UCS( ) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - i = np.argmin([f[n] for n in frontier]) # minimum f in frontier - x = frontier.pop(i) + while True: # minimum f in frontier, skipping stale heap entries + fx, _, x = heapq.heappop(heap) + if x in frontier and f[x] == fx: + break + del frontier[x] if verbose: print(" expand", x.name) if x is G: @@ -3210,7 +3226,8 @@ def path_UCS( # add it to the frontier parent[n] = x f[n] = fnew - frontier.append(n) + frontier[n] = len(parent) # one entry per vertex added so far + heapq.heappush(heap, (fnew, frontier[n], n)) if verbose: print(" add", n.name, "to the frontier") @@ -3224,8 +3241,9 @@ def path_UCS( ) f[n] = fnew parent[n] = x + heapq.heappush(heap, (fnew, frontier[n], n)) - explored.append(x) + explored[x] = None if verbose: print(" move", x.name, "to the explored list") else: @@ -3240,8 +3258,9 @@ def path_UCS( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() parent_names: dict[str, str] = {} for v, p in parent.items(): @@ -3308,8 +3327,10 @@ def path_Astar( S = self._resolve_vertex(S, "start") G = self._resolve_vertex(G, "goal") - frontier: list[Vertex] = [S] - explored: list[Vertex] = [] + # see path_UCS for the frontier and heap + frontier: dict[Vertex, int] = {S: 0} + heap: list[tuple[float, int, Vertex]] = [(0, 0, S)] + explored: dict[Vertex, None] = {} parent: dict[Vertex, Vertex] = {} g: dict[Vertex, float] = {S: 0} # cost to come f: dict[Vertex, float] = {S: 0} # evaluation function @@ -3322,8 +3343,11 @@ def path_Astar( ) print("EXPLORED:", ", ".join([str(v.name) for v in explored])) - i = np.argmin([f[n] for n in frontier]) # minimum f in frontier - x = frontier.pop(i) + while True: # minimum f in frontier, skipping stale heap entries + fx, _, x = heapq.heappop(heap) + if x in frontier and f[x] == fx: + break + del frontier[x] if verbose: print(" expand", x.name) if x is G: @@ -3334,10 +3358,11 @@ def path_Astar( continue # self-loop, never part of a shortest path if n not in frontier and n not in explored: # add it to the frontier - frontier.append(n) parent[n] = x + frontier[n] = len(parent) # one entry per vertex added so far g[n] = g[x] + self._require_cost(e) # update cost to come f[n] = g[n] + n.heuristic_distance(G) # heuristic + heapq.heappush(heap, (f[n], frontier[n], n)) if verbose: print(" add", n.name, "to the frontier") elif n in frontier: @@ -3353,8 +3378,9 @@ def path_Astar( f[n] = g[n] + n.heuristic_distance(G) # heuristic parent[n] = x # reparent + heapq.heappush(heap, (f[n], frontier[n], n)) - explored.append(x) + explored[x] = None if verbose: print(" move", x.name, "to the explored list") @@ -3370,8 +3396,9 @@ def path_Astar( while x is not S: p = parent[x] length += self._require_cost(p.edgeto(x)) - path.insert(0, p) + path.append(p) x = p + path.reverse() parent_names: dict[str, str] = {} for v, p in parent.items(): diff --git a/tests/test_planner_choices.py b/tests/test_planner_choices.py new file mode 100644 index 00000000..79f86ff0 --- /dev/null +++ b/tests/test_planner_choices.py @@ -0,0 +1,231 @@ +""" +The path planners are searches with an explicit frontier whose order matters: +ties between equal costs are broken by the order vertices joined the frontier, +and ``verbose=True`` prints that order. Implemented naively (lists searched +with ``in``, ``np.argmin`` over the frontier) they were O(V^2) or worse, and +the frontier is now a queue or heap with set lookups. These tests pin the +results *and the verbose transcripts* that the original implementation gave, +so the faster one must make exactly the same choices. + +The expectations were generated by the list-based planners that preceded it. +""" + +import contextlib +import io +import unittest + +from pgraph import DGraph, UGraph + + +def grid(): + # a 3 x 2 grid of unit squares plus the diagonal a-e: many equal costs + g = UGraph(dim=2) + coords = dict(a=(0, 0), b=(1, 0), c=(2, 0), d=(0, 1), e=(1, 1), f=(2, 1)) + v = {n: g.add_vertex(list(xy), name=n) for n, xy in coords.items()} + for p, q in ["ab", "bc", "de", "ef", "ad", "be", "cf", "ae"]: + g.add_edge(v[p], v[q], cost=1) + return g, "a", "f" + + +def reparent(): + # x is first reached directly (cost 5), then more cheaply via y (1 + 1) + g = DGraph(dim=2) + v = {n: g.add_vertex([x, 0], name=n) for n, x in dict(s=0, y=1, x=2, t=3).items()} + for p, q, c in [("s", "x", 5), ("s", "y", 1), ("y", "x", 1), ("x", "t", 1)]: + g.add_edge(v[p], v[q], cost=c) + return g, "s", "t" + + +# (graph, planner) -> (path, length, verbose transcript) +EXPECTED = {('grid', 'Astar'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a(0)\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b(2), d(3), e(2)\n' + 'EXPLORED: a\n' + ' expand e\n' + ' add f to the frontier\n' + ' move e to the explored list\n' + '\n' + 'FRONTIER: b(2), d(3), f(2)\n' + 'EXPLORED: a, e\n' + ' expand f\n' + '2 vertices explored, 2 remaining on the frontier\n'), + ('grid', 'BFS'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b, d, e\n' + 'EXPLORED: a\n' + ' expand b\n' + ' add c to the frontier\n' + ' move b to the explored list\n' + '\n' + 'FRONTIER: d, e, c\n' + 'EXPLORED: a, b\n' + ' expand d\n' + ' move d to the explored list\n' + '\n' + 'FRONTIER: e, c\n' + 'EXPLORED: a, b, d\n' + ' expand e\n' + ' goal f reached\n' + '3 vertices explored, 1 remaining on the frontier\n'), + ('grid', 'UCS'): (['a', 'e', 'f'], + 2.0, + '\n' + 'FRONTIER: a(0)\n' + 'EXPLORED: \n' + ' expand a\n' + ' add b to the frontier\n' + ' add d to the frontier\n' + ' add e to the frontier\n' + ' move a to the explored list\n' + '\n' + 'FRONTIER: b(1), d(1), e(1)\n' + 'EXPLORED: a\n' + ' expand b\n' + ' add c to the frontier\n' + ' move b to the explored list\n' + '\n' + 'FRONTIER: d(1), e(1), c(2)\n' + 'EXPLORED: a, b\n' + ' expand d\n' + ' move d to the explored list\n' + '\n' + 'FRONTIER: e(1), c(2)\n' + 'EXPLORED: a, b, d\n' + ' expand e\n' + ' add f to the frontier\n' + ' move e to the explored list\n' + '\n' + 'FRONTIER: c(2), f(2)\n' + 'EXPLORED: a, b, d, e\n' + ' expand c\n' + ' move c to the explored list\n' + '\n' + 'FRONTIER: f(2)\n' + 'EXPLORED: a, b, d, e, c\n' + ' expand f\n' + '5 vertices explored, 0 remaining on the frontier\n'), + ('reparent', 'Astar'): (['s', 'y', 'x', 't'], + 3.0, + '\n' + 'FRONTIER: s(0)\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x(6), y(3)\n' + 'EXPLORED: s\n' + ' expand y\n' + ' reparent x: cost 2 via y is less than cost 5 via s, change parent from ' + 's to y \n' + ' move y to the explored list\n' + '\n' + 'FRONTIER: x(3)\n' + 'EXPLORED: s, y\n' + ' expand x\n' + ' add t to the frontier\n' + ' move x to the explored list\n' + '\n' + 'FRONTIER: t(3)\n' + 'EXPLORED: s, y, x\n' + ' expand t\n' + '3 vertices explored, 0 remaining on the frontier\n'), + ('reparent', 'BFS'): (['s', 'x', 't'], + 6.0, + '\n' + 'FRONTIER: s\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x, y\n' + 'EXPLORED: s\n' + ' expand x\n' + ' goal t reached\n' + '1 vertices explored, 1 remaining on the frontier\n'), + ('reparent', 'UCS'): (['s', 'y', 'x', 't'], + 3.0, + '\n' + 'FRONTIER: s(0)\n' + 'EXPLORED: \n' + ' expand s\n' + ' add x to the frontier\n' + ' add y to the frontier\n' + ' move s to the explored list\n' + '\n' + 'FRONTIER: x(5), y(1)\n' + 'EXPLORED: s\n' + ' expand y\n' + ' reparent x: cost 2 via y is less than cost 5 via s, change parent from s ' + 'to y \n' + ' move y to the explored list\n' + '\n' + 'FRONTIER: x(2)\n' + 'EXPLORED: s, y\n' + ' expand x\n' + ' add t to the frontier\n' + ' move x to the explored list\n' + '\n' + 'FRONTIER: t(3)\n' + 'EXPLORED: s, y, x\n' + ' expand t\n' + '3 vertices explored, 0 remaining on the frontier\n')} + + +class TestPlannerChoices(unittest.TestCase): + def check(self, make, planner): + g, s, t = make() + buf = io.StringIO() + with contextlib.redirect_stdout(buf): + result = getattr(g, f"path_{planner}")(s, t, verbose=True, summary=True) + path, length, transcript = EXPECTED[(make.__name__, planner)] + self.assertEqual([v.name for v in result[0]], path) + self.assertEqual(result[1], length) + self.assertEqual(buf.getvalue(), transcript) + + def test_grid_BFS(self): + self.check(grid, "BFS") + + def test_grid_UCS(self): + self.check(grid, "UCS") + + def test_grid_Astar(self): + self.check(grid, "Astar") + + def test_reparent_BFS(self): + self.check(reparent, "BFS") + + def test_reparent_UCS(self): + self.check(reparent, "UCS") + + def test_reparent_Astar(self): + self.check(reparent, "Astar") + + def test_reparenting_happens(self): + # guard the guard: the transcript really does contain a reparent + self.assertIn("reparent x", EXPECTED[("reparent", "UCS")][2]) + + +if __name__ == "__main__": + unittest.main()