[2] add pathfinding algorithms

BoriskovaDV/docs/data/2-nd-exercise/main.py

@@ -1,5 +1,7 @@
 import sys
 import os
+from collections import deque
+import heapq
 
 class GridPoint:
     def __init__(self, x, y):
@@ -46,6 +48,16 @@ class Labyrinth:
         elif typ == 'path':
             p.blocked = False
 
+    def neighbors(self, p):
+        dirs = [(0, -1), (0, 1), (-1, 0), (1, 0)]
+        res = []
+        for dx, dy in dirs:
+            nx, ny = p.x + dx, p.y + dy
+            nb = self.get_point(nx, ny)
+            if nb and nb.can_step():
+                res.append(nb)
+        return res
+
 class MazeLoader:
     def load(self, filename):
         raise NotImplementedError
@@ -76,6 +88,92 @@ class TextMazeLoader(MazeLoader):
             raise ValueError(f"Need exactly one S and one E. Found S={start_cnt}, E={exit_cnt}")
         return lab
 
+class SearchAlgorithm:
+    def find_way(self, lab, start, goal):
+        raise NotImplementedError
+
+    def _build_path(self, prev, start, goal):
+        path = []
+        cur = goal
+        while cur:
+            path.append(cur)
+            cur = prev.get(cur)
+        path.reverse()
+        return path
+
+    def get_visited(self):
+        return getattr(self, '_visited', 0)
+
+class BreadthFirst(SearchAlgorithm):
+    def find_way(self, lab, start, goal):
+        q = deque([start])
+        prev = {start: None}
+        seen = {start}
+        while q:
+            cur = q.popleft()
+            if cur == goal:
+                self._visited = len(seen)
+                return self._build_path(prev, start, goal)
+            for nb in lab.neighbors(cur):
+                if nb not in seen:
+                    seen.add(nb)
+                    prev[nb] = cur
+                    q.append(nb)
+        self._visited = len(seen)
+        return []
+
+class DepthFirst(SearchAlgorithm):
+    def find_way(self, lab, start, goal):
+        stack = [start]
+        prev = {start: None}
+        seen = {start}
+        while stack:
+            cur = stack.pop()
+            if cur == goal:
+                self._visited = len(seen)
+                return self._build_path(prev, start, goal)
+            for nb in lab.neighbors(cur):
+                if nb not in seen:
+                    seen.add(nb)
+                    prev[nb] = cur
+                    stack.append(nb)
+        self._visited = len(seen)
+        return []
+
+class AStar(SearchAlgorithm):
+    def _dist(self, a, b):
+        return abs(a.x - b.x) + abs(a.y - b.y)
+
+    def find_way(self, lab, start, goal):
+        heap = []
+        cnt = 0
+        start_f = self._dist(start, goal)
+        heapq.heappush(heap, (start_f, cnt, start))
+        cnt += 1
+        prev = {}
+        g = {start: 0}
+        f = {start: start_f}
+        seen = set()
+        while heap:
+            cur_f, _, cur = heapq.heappop(heap)
+            seen.add(cur)
+            if cur == goal:
+                self._visited = len(seen)
+                return self._build_path(prev, start, goal)
+            if cur_f > f.get(cur, float('inf')):
+                continue
+            for nb in lab.neighbors(cur):
+                new_g = g[cur] + 1
+                if new_g < g.get(nb, float('inf')):
+                    prev[nb] = cur
+                    g[nb] = new_g
+                    new_f = new_g + self._dist(nb, goal)
+                    f[nb] = new_f
+                    heapq.heappush(heap, (new_f, cnt, nb))
+                    cnt += 1
+        self._visited = len(seen)
+        return []
+
 class TextView:
     def display(self, lab):
         os.system('cls' if os.name == 'nt' else 'clear')
@@ -103,3 +201,7 @@ if __name__ == "__main__":
     lab = loader.load("maze/maze1.txt")
     view = TextView()
     view.display(lab)
+    print("\nTesting BFS...")
+    bfs = BreadthFirst()
+    path = bfs.find_way(lab, lab.start_point, lab.exit_point)
+    print(f"BFS path length: {len(path)} visited: {bfs.get_visited()}")