From 9d48ba35352f6fefb9e03bc7047960aeee9cd74b Mon Sep 17 00:00:00 2001 From: Uzia Peer Date: Mon, 12 Oct 2026 19:26:04 +0300 Subject: [PATCH 1/4] Add Basic Theta Star headers and failing tests --- graphs/tests/test_theta_star.py | 138 ++++++++++++++++++++++++++++++++ graphs/theta_star.py | 79 ++++++++++++++++++ 2 files changed, 217 insertions(+) create mode 100644 graphs/tests/test_theta_star.py create mode 100644 graphs/theta_star.py diff --git a/graphs/tests/test_theta_star.py b/graphs/tests/test_theta_star.py new file mode 100644 index 000000000000..ec85e8445c64 --- /dev/null +++ b/graphs/tests/test_theta_star.py @@ -0,0 +1,138 @@ +"""Tests for the Basic Theta* examples from assignment 1.""" + +from fractions import Fraction +from itertools import pairwise +from math import dist +from random import Random + +import pytest + +from graphs.theta_star import theta_star + +Point = tuple[int, int] + + +def segment_is_clear(grid: list[list[int]], start: Point, end: Point) -> bool: + """Check a segment independently by splitting it at grid lines.""" + height, width = len(grid), len(grid[0]) + + def is_free(x: int, y: int) -> bool: + return 0 <= x < width and 0 <= y < height and grid[y][x] == 0 + + for x, y in (start, end): + if not (0 <= x <= width and 0 <= y <= height): + return False + if not any(is_free(cx, cy) for cx in (x - 1, x) for cy in (y - 1, y)): + return False + + dx, dy = end[0] - start[0], end[1] - start[1] + cuts = {Fraction(0), Fraction(1)} + for axis, delta, size in ((0, dx, width), (1, dy, height)): + if delta: + cuts.update( + Fraction(k - start[axis], delta) + for k in range(size + 1) + if 0 < Fraction(k - start[axis], delta) < 1 + ) + + for lower, upper in pairwise(sorted(cuts)): + midpoint = (lower + upper) / 2 + x = start[0] + midpoint * dx + y = start[1] + midpoint * dy + xs = (int(x) - 1, int(x)) if x.denominator == 1 else (int(x // 1),) + ys = (int(y) - 1, int(y)) if y.denominator == 1 else (int(y // 1),) + if not any(is_free(cx, cy) for cx in xs for cy in ys): + return False + return True + + +@pytest.mark.parametrize("size", [1, 2, 3]) +def test_small_free_grids(size: int) -> None: + grid = [[0] * size for _ in range(size)] + assert theta_star(grid, (0, 0), (size, size)) == [(0, 0), (size, size)] + + +@pytest.mark.parametrize( + ("goal", "expected"), + [ + ((0, 0), [(3, 2), (2, 1), (0, 0)]), + ((1, 0), [(3, 2), (1, 0)]), + ], +) +def test_paper_figures_1_and_3(goal: Point, expected: list[Point]) -> None: + grid = [[0, 0, 0, 1], [0, 1, 0, 0]] + assert theta_star(grid, (3, 2), goal) == expected + + +def test_paper_figure_2() -> None: + grid = [[0] * 6 for _ in range(3)] + grid[0][2] = 1 + assert theta_star(grid, (6, 3), (0, 0)) == [(6, 3), (0, 0)] + + +@pytest.mark.parametrize( + ("goal", "expected"), + [ + ((8, 3), [(0, 0), (6, 2), (8, 3)]), + ((9, 2), [(0, 0), (6, 2), (9, 2)]), + ], +) +def test_paper_figure_4(goal: Point, expected: list[Point]) -> None: + grid = [[0] * 9 for _ in range(4)] + grid[2][4] = grid[1][6] = 1 + assert theta_star(grid, (0, 0), goal) == expected + + +def test_obstacle_requires_a_turn() -> None: + grid = [[0, 0, 0], [0, 1, 0], [0, 0, 0]] + assert theta_star(grid, (0, 0), (3, 3)) == [(0, 0), (1, 2), (3, 3)] + + +def test_same_start_and_goal() -> None: + assert theta_star([[0]], (0, 0), (0, 0)) == [(0, 0)] + + +def test_no_path() -> None: + assert theta_star([[0, 1, 0], [0, 1, 0]], (0, 0), (3, 0)) == [] + + +def test_diagonal_touching_is_allowed() -> None: + assert theta_star([[0, 1], [1, 0]], (0, 0), (2, 2)) == [(0, 0), (2, 2)] + + +@pytest.mark.parametrize("grid", [[], [[]], [[0], [0, 1]], [[2]], [[True]]]) +def test_invalid_grid(grid: list[list[int]]) -> None: + with pytest.raises(ValueError): + theta_star(grid, (0, 0), (1, 1)) + + +@pytest.mark.parametrize("start", [(-1, 0), (2, 0), (0.5, 0), (True, 0)]) +def test_invalid_start(start: tuple[float, int]) -> None: + with pytest.raises(ValueError): + theta_star([[0]], start, (1, 1)) + + +def test_large_free_grid() -> None: + grid = [[0] * 100 for _ in range(100)] + assert theta_star(grid, (0, 0), (100, 100)) == [(0, 0), (100, 100)] + + +@pytest.mark.parametrize("seed", [7, 17, 91]) +def test_large_random_grid(seed: int) -> None: + rng = Random(seed) + grid = [[int(rng.random() < 0.15) for _ in range(70)] for _ in range(70)] + grid[0] = [0] * 70 + for row in grid: + row[-1] = 0 + + path = theta_star(grid, (0, 0), (70, 70)) + assert path and path[0] == (0, 0) and path[-1] == (70, 70) + assert all(segment_is_clear(grid, a, b) for a, b in pairwise(path)) + assert sum(dist(a, b) for a, b in pairwise(path)) >= dist(path[0], path[-1]) + + +def test_random_example_from_assignment_1() -> None: + rng = Random(17) + grid = [[int(rng.random() < 0.15) for _ in range(10)] for _ in range(10)] + grid[0][0] = grid[-1][-1] = 0 + assert theta_star(grid, (0, 0), (10, 10)) == [(0, 0), (6, 7), (9, 10), (10, 10)] diff --git a/graphs/theta_star.py b/graphs/theta_star.py new file mode 100644 index 000000000000..46c82ddcf7da --- /dev/null +++ b/graphs/theta_star.py @@ -0,0 +1,79 @@ +"""Basic Theta* from "Theta*: Any-Angle Path Planning on Grids". + +Alex Nash, Kenny Daniel, Sven Koenig, Ariel Felner (2007). +https://cdn.aaai.org/AAAI/2007/AAAI07-187.pdf + +Programmer: Uzia Peer +Date: 2026-10-12 +""" + + +def theta_star( + grid: list[list[int]], # noqa: ARG001 + start: tuple[int, int], # noqa: ARG001 + goal: tuple[int, int], # noqa: ARG001 +) -> list[tuple[int, int]]: + """Find an any-angle path through a grid using Basic Theta* (Algorithms 1-2). + + ``grid[y][x]`` is 0 for a free cell and 1 for an obstacle. The first row + is the bottom row. ``start`` and ``goal`` are cell corners, not cell centers. + A grid of width w and height h has corners from (0, 0) to (w, h). + The path includes both endpoints. Return [] if no path exists. Invalid + grids or endpoints raise ValueError. The path need not be globally shortest. + + Small free grids from assignment 1: + + >>> theta_star([[0]], (0, 0), (1, 1)) + [(0, 0), (1, 1)] + >>> theta_star([[0, 0], [0, 0]], (0, 0), (2, 2)) + [(0, 0), (2, 2)] + >>> theta_star([[0] * 3 for _ in range(3)], (0, 0), (3, 3)) + [(0, 0), (3, 3)] + + Paper figures 1 and 3: a turn near an obstacle and two update cases. + + >>> paper_grid = [[0, 0, 0, 1], [0, 1, 0, 0]] + >>> theta_star(paper_grid, (3, 2), (0, 0)) + [(3, 2), (2, 1), (0, 0)] + >>> theta_star(paper_grid, (3, 2), (1, 0)) + [(3, 2), (1, 0)] + + Paper figure 2: a path that post-smoothing cannot obtain from A*'s route. + + >>> cells = [[0] * 6 for _ in range(3)] + >>> cells[0][2] = 1 + >>> theta_star(cells, (6, 3), (0, 0)) + [(6, 3), (0, 0)] + + Paper figure 4: Basic Theta* can miss a shorter path. + + >>> cells = [[0] * 9 for _ in range(4)] + >>> cells[2][4] = cells[1][6] = 1 + >>> theta_star(cells, (0, 0), (8, 3)) + [(0, 0), (6, 2), (8, 3)] + >>> theta_star(cells, (0, 0), (9, 2)) + [(0, 0), (6, 2), (9, 2)] + + Additional edge cases from assignment 1: + + >>> theta_star([[0, 0, 0], [0, 1, 0], [0, 0, 0]], (0, 0), (3, 3)) + [(0, 0), (1, 2), (3, 3)] + >>> theta_star([[0]], (0, 0), (0, 0)) + [(0, 0)] + >>> theta_star([[0, 1, 0], [0, 1, 0]], (0, 0), (3, 0)) + [] + >>> theta_star([[0, 1], [1, 0]], (0, 0), (2, 2)) + [(0, 0), (2, 2)] + + A reproducible larger random grid: + + >>> from random import Random + >>> rng = Random(17) + >>> cells = [[int(rng.random() < 0.15) for _ in range(10)] for _ in range(10)] + >>> cells[0][0] = cells[-1][-1] = 0 + >>> path = theta_star(cells, (0, 0), (10, 10)) + >>> path + [(0, 0), (6, 7), (9, 10), (10, 10)] + """ + # Assignment 4: search intentionally left empty. Tests should fail. + return [] From bcd3e1f9df1bf9547c8352f84a377095bf25cd5e Mon Sep 17 00:00:00 2001 From: Uzia Peer Date: Mon, 12 Oct 2026 19:30:24 +0300 Subject: [PATCH 2/4] Implement Basic Theta star path search --- graphs/theta_star.py | 219 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 213 insertions(+), 6 deletions(-) diff --git a/graphs/theta_star.py b/graphs/theta_star.py index 46c82ddcf7da..b5aba3d0405c 100644 --- a/graphs/theta_star.py +++ b/graphs/theta_star.py @@ -7,12 +7,154 @@ Date: 2026-10-12 """ +import heapq +import logging +from itertools import count, pairwise +from math import hypot, inf -def theta_star( - grid: list[list[int]], # noqa: ARG001 - start: tuple[int, int], # noqa: ARG001 - goal: tuple[int, int], # noqa: ARG001 -) -> list[tuple[int, int]]: +Point = tuple[int, int] +logger = logging.getLogger(__name__) + + +class CornerGrid: + """A rectangular grid whose path vertices are cell corners. + + The bottom row is ``cells[0]``. Obstacles are marked by 1. + + >>> grid = CornerGrid([[0, 1], [0, 0]]) + >>> grid.width, grid.height + (2, 2) + >>> grid.line_of_sight((0, 0), (2, 2)) + True + """ + + def __init__(self, cells: list[list[int]]) -> None: + rows = tuple(tuple(row) for row in cells) + if not rows or not rows[0] or any(len(row) != len(rows[0]) for row in rows): + raise ValueError("Grid must be a non-empty rectangular matrix.") + if any( + type(cell) is not int or cell not in (0, 1) for row in rows for cell in row + ): + raise ValueError("Cells must be integers 0 (free) or 1 (blocked).") + self.cells = rows + self.height, self.width = len(rows), len(rows[0]) + + def blocked(self, x: int, y: int) -> bool: + """Treat cells outside the grid as blocked. + + >>> CornerGrid([[0]]).blocked(-1, 0) + True + """ + return not (0 <= x < self.width and 0 <= y < self.height) or bool( + self.cells[y][x] + ) + + def valid_corner(self, point: Point) -> bool: + """A usable corner touches at least one free cell. + + >>> CornerGrid([[1]]).valid_corner((0, 0)) + False + """ + if not isinstance(point, tuple) or len(point) != 2: + return False + x, y = point + if type(x) is not int or type(y) is not int: + return False + return ( + 0 <= x <= self.width + and 0 <= y <= self.height + and any(not self.blocked(cx, cy) for cx in (x - 1, x) for cy in (y - 1, y)) + ) + + def line_of_sight(self, a: Point, b: Point) -> bool: + """Test visibility with integer grid traversal, as in the paper. + + Diagonal contact at one corner is allowed, but moving along an edge + shared by two blocked cells is not. + + >>> CornerGrid([[0, 1], [1, 0]]).line_of_sight((0, 0), (2, 2)) + True + >>> CornerGrid([[1], [1]]).line_of_sight((0, 0), (0, 2)) + False + """ + if not self.valid_corner(a) or not self.valid_corner(b): + return False + x, y = a + dx, dy = abs(b[0] - x), abs(b[1] - y) + sx = 1 if b[0] >= x else -1 + sy = 1 if b[1] >= y else -1 + ox, oy = (sx - 1) // 2, (sy - 1) // 2 + error = 0 + if dx >= dy: + while x != b[0]: + error += dy + if error >= dx: + if self.blocked(x + ox, y + oy): + return False + y += sy + error -= dx + if error and self.blocked(x + ox, y + oy): + return False + if dy == 0 and self.blocked(x + ox, y) and self.blocked(x + ox, y - 1): + return False + x += sx + else: + while y != b[1]: + error += dx + if error >= dy: + if self.blocked(x + ox, y + oy): + return False + x += sx + error -= dy + if error and self.blocked(x + ox, y + oy): + return False + if dx == 0 and self.blocked(x, y + oy) and self.blocked(x - 1, y + oy): + return False + y += sy + return True + + def neighbors(self, point: Point) -> list[Point]: + """Return visible neighboring corners in a fixed order. + + >>> CornerGrid([[0]]).neighbors((0, 0)) + [(0, 1), (1, 0), (1, 1)] + """ + x, y = point + candidates = ((x + dx, y + dy) for dx in (-1, 0, 1) for dy in (-1, 0, 1)) + return [q for q in candidates if q != point and self.line_of_sight(point, q)] + + +def distance(a: Point, b: Point) -> float: + """Euclidean distance ``c(a,b)`` from the paper. + + >>> distance((0, 0), (3, 4)) + 5.0 + """ + return hypot(a[0] - b[0], a[1] - b[1]) + + +def path_length(path: list[Point]) -> float: + """Add the lengths of the path segments. + + >>> path_length([(0, 0), (3, 4), (6, 4)]) + 8.0 + """ + return sum(distance(a, b) for a, b in pairwise(path)) + + +def reconstruct_path(parent: dict[Point, Point], goal: Point) -> list[Point]: + """Follow parent pointers from the goal back to the start. + + >>> reconstruct_path({(0, 0): (0, 0), (1, 1): (0, 0)}, (1, 1)) + [(0, 0), (1, 1)] + """ + path = [goal] + while parent[path[-1]] != path[-1]: + path.append(parent[path[-1]]) + return path[::-1] + + +def theta_star(grid: list[list[int]], start: Point, goal: Point) -> list[Point]: """Find an any-angle path through a grid using Basic Theta* (Algorithms 1-2). ``grid[y][x]`` is 0 for a free cell and 1 for an obstacle. The first row @@ -75,5 +217,70 @@ def theta_star( >>> path [(0, 0), (6, 7), (9, 10), (10, 10)] """ - # Assignment 4: search intentionally left empty. Tests should fail. + corner_grid = CornerGrid(grid) + if not corner_grid.valid_corner(start) or not corner_grid.valid_corner(goal): + raise ValueError("Start and goal must be usable integer grid corners.") + return _search(corner_grid, start, goal) + + +def _search(corner_grid: CornerGrid, start: Point, goal: Point) -> list[Point]: + """Run Basic Theta* on a validated grid (shared with the cache experiment). + + >>> _search(CornerGrid([[0]]), (0, 0), (1, 1)) + [(0, 0), (1, 1)] + """ + + # g, parent, open, and closed use the same meanings as in Algorithms 1-2. + # h is the Euclidean distance to goal. Equal f prefers smaller g. + g = {start: 0.0} + parent = {start: start} + closed: set[Point] = set() + serial = count() + opened = [(distance(start, goal), 0.0, next(serial), start)] + + while opened: + _, old_g, _, current = heapq.heappop(opened) + if current in closed or old_g != g[current]: + continue + logger.debug("Expand %s g=%.6f parent=%s", current, g[current], parent[current]) + if current == goal: + path = reconstruct_path(parent, goal) + logger.info("Found path of length %.6f", path_length(path)) + return path + + closed.add(current) + for neighbor in corner_grid.neighbors(current): + if neighbor in closed: + logger.debug("Skip closed neighbor %s", neighbor) + continue + + # Path 2 goes directly from parent(current) to neighbor if visible. + # Otherwise Path 1 goes through current. + previous = parent[current] + shortcut = corner_grid.line_of_sight(previous, neighbor) + candidate_parent = previous if shortcut else current + branch = "Path 2" if shortcut else "Path 1" + candidate = g[candidate_parent] + distance(candidate_parent, neighbor) + if candidate < g.get(neighbor, inf): + g[neighbor], parent[neighbor] = candidate, candidate_parent + heapq.heappush( + opened, + ( + candidate + distance(neighbor, goal), + candidate, + next(serial), + neighbor, + ), + ) + logger.debug( + "%s: %s -> %s; g=%.6f", + branch, + candidate_parent, + neighbor, + candidate, + ) + else: + logger.debug("%s: no improvement for %s", branch, neighbor) + + logger.info("No path from %s to %s", start, goal) return [] From 5524fe46240a60ebe03cde86b059cf7d3d48e8d6 Mon Sep 17 00:00:00 2001 From: Uzia Peer Date: Tue, 13 Oct 2026 17:11:00 +0300 Subject: [PATCH 3/4] Document paper and author in Theta star tests --- graphs/tests/test_theta_star.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/graphs/tests/test_theta_star.py b/graphs/tests/test_theta_star.py index ec85e8445c64..77a94e1bdb68 100644 --- a/graphs/tests/test_theta_star.py +++ b/graphs/tests/test_theta_star.py @@ -1,4 +1,9 @@ -"""Tests for the Basic Theta* examples from assignment 1.""" +"""Tests for "Theta*: Any-Angle Path Planning on Grids". + +Alex Nash, Kenny Daniel, Sven Koenig, Ariel Felner (2007). +https://cdn.aaai.org/AAAI/2007/AAAI07-187.pdf +Programmer: Uzia Peer. Date: 2026-10-12. +""" from fractions import Fraction from itertools import pairwise From 5bff59f7f8f09f62337bff753d79156f062e6931 Mon Sep 17 00:00:00 2001 From: Uzia Peer Date: Tue, 13 Oct 2026 17:39:58 +0300 Subject: [PATCH 4/4] Use descriptive names in Basic Theta Star --- graphs/theta_star.py | 48 +++++++++++++++++++++++++------------------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/graphs/theta_star.py b/graphs/theta_star.py index b5aba3d0405c..1808b82b85a7 100644 --- a/graphs/theta_star.py +++ b/graphs/theta_star.py @@ -12,7 +12,7 @@ from itertools import count, pairwise from math import hypot, inf -Point = tuple[int, int] +grid_point = tuple[int, int] logger = logging.getLogger(__name__) @@ -39,17 +39,17 @@ def __init__(self, cells: list[list[int]]) -> None: self.cells = rows self.height, self.width = len(rows), len(rows[0]) - def blocked(self, x: int, y: int) -> bool: + def blocked(self, column: int, row: int) -> bool: """Treat cells outside the grid as blocked. >>> CornerGrid([[0]]).blocked(-1, 0) True """ - return not (0 <= x < self.width and 0 <= y < self.height) or bool( - self.cells[y][x] + return not (0 <= column < self.width and 0 <= row < self.height) or bool( + self.cells[row][column] ) - def valid_corner(self, point: Point) -> bool: + def valid_corner(self, point: grid_point) -> bool: """A usable corner touches at least one free cell. >>> CornerGrid([[1]]).valid_corner((0, 0)) @@ -66,7 +66,7 @@ def valid_corner(self, point: Point) -> bool: and any(not self.blocked(cx, cy) for cx in (x - 1, x) for cy in (y - 1, y)) ) - def line_of_sight(self, a: Point, b: Point) -> bool: + def line_of_sight(self, start: grid_point, end: grid_point) -> bool: """Test visibility with integer grid traversal, as in the paper. Diagonal contact at one corner is allowed, but moving along an edge @@ -77,16 +77,16 @@ def line_of_sight(self, a: Point, b: Point) -> bool: >>> CornerGrid([[1], [1]]).line_of_sight((0, 0), (0, 2)) False """ - if not self.valid_corner(a) or not self.valid_corner(b): + if not self.valid_corner(start) or not self.valid_corner(end): return False - x, y = a - dx, dy = abs(b[0] - x), abs(b[1] - y) - sx = 1 if b[0] >= x else -1 - sy = 1 if b[1] >= y else -1 + x, y = start + dx, dy = abs(end[0] - x), abs(end[1] - y) + sx = 1 if end[0] >= x else -1 + sy = 1 if end[1] >= y else -1 ox, oy = (sx - 1) // 2, (sy - 1) // 2 error = 0 if dx >= dy: - while x != b[0]: + while x != end[0]: error += dy if error >= dx: if self.blocked(x + ox, y + oy): @@ -99,7 +99,7 @@ def line_of_sight(self, a: Point, b: Point) -> bool: return False x += sx else: - while y != b[1]: + while y != end[1]: error += dx if error >= dy: if self.blocked(x + ox, y + oy): @@ -113,7 +113,7 @@ def line_of_sight(self, a: Point, b: Point) -> bool: y += sy return True - def neighbors(self, point: Point) -> list[Point]: + def neighbors(self, point: grid_point) -> list[grid_point]: """Return visible neighboring corners in a fixed order. >>> CornerGrid([[0]]).neighbors((0, 0)) @@ -124,16 +124,16 @@ def neighbors(self, point: Point) -> list[Point]: return [q for q in candidates if q != point and self.line_of_sight(point, q)] -def distance(a: Point, b: Point) -> float: +def distance(first: grid_point, second: grid_point) -> float: """Euclidean distance ``c(a,b)`` from the paper. >>> distance((0, 0), (3, 4)) 5.0 """ - return hypot(a[0] - b[0], a[1] - b[1]) + return hypot(first[0] - second[0], first[1] - second[1]) -def path_length(path: list[Point]) -> float: +def path_length(path: list[grid_point]) -> float: """Add the lengths of the path segments. >>> path_length([(0, 0), (3, 4), (6, 4)]) @@ -142,7 +142,9 @@ def path_length(path: list[Point]) -> float: return sum(distance(a, b) for a, b in pairwise(path)) -def reconstruct_path(parent: dict[Point, Point], goal: Point) -> list[Point]: +def reconstruct_path( + parent: dict[grid_point, grid_point], goal: grid_point +) -> list[grid_point]: """Follow parent pointers from the goal back to the start. >>> reconstruct_path({(0, 0): (0, 0), (1, 1): (0, 0)}, (1, 1)) @@ -154,7 +156,9 @@ def reconstruct_path(parent: dict[Point, Point], goal: Point) -> list[Point]: return path[::-1] -def theta_star(grid: list[list[int]], start: Point, goal: Point) -> list[Point]: +def theta_star( + grid: list[list[int]], start: grid_point, goal: grid_point +) -> list[grid_point]: """Find an any-angle path through a grid using Basic Theta* (Algorithms 1-2). ``grid[y][x]`` is 0 for a free cell and 1 for an obstacle. The first row @@ -223,7 +227,9 @@ def theta_star(grid: list[list[int]], start: Point, goal: Point) -> list[Point]: return _search(corner_grid, start, goal) -def _search(corner_grid: CornerGrid, start: Point, goal: Point) -> list[Point]: +def _search( + corner_grid: CornerGrid, start: grid_point, goal: grid_point +) -> list[grid_point]: """Run Basic Theta* on a validated grid (shared with the cache experiment). >>> _search(CornerGrid([[0]]), (0, 0), (1, 1)) @@ -234,7 +240,7 @@ def _search(corner_grid: CornerGrid, start: Point, goal: Point) -> list[Point]: # h is the Euclidean distance to goal. Equal f prefers smaller g. g = {start: 0.0} parent = {start: start} - closed: set[Point] = set() + closed: set[grid_point] = set() serial = count() opened = [(distance(start, goal), 0.0, next(serial), start)]