Werks

astar.py

@@ -0,0 +1,108 @@
+from numpy import Infinity
+
+
+CELL = 0
+WALL = 1
+START = 2
+END = 3
+
+
+ROWS = 10
+COLS = 10
+
+
+class Node:
+    def __init__(self, x, y) -> None:
+        self.x = x
+        self.y = y
+        self.gscore = Infinity
+        self.fscore = Infinity
+        self.celltype = CELL
+
+    def __str__(self) -> str:
+        return f"({self.x}, {self.y})"
+
+    def __repr__(self) -> str:
+        return f"({self.x}, {self.y})"
+
+    def g_pos(self):
+        return (self.x, self.y)
+
+    def s_gscore(self, g):
+        self.gscore = g
+
+    def s_fscore(self, f):
+        self.fscore = f
+
+    def g_gscore(self):
+        return self.gscore
+
+    def g_fscore(self):
+        return self.fscore
+
+    def g_neighbours(self, grid):
+        neighours = []
+        if self.x > 0:
+            neighours.append(grid[self.x - 1][self.y])
+
+        if self.x < ROWS - 1:
+            neighours.append(grid[self.x + 1][self.y])
+
+        if self.y > 0:
+            neighours.append(grid[self.x][self.y - 1])
+
+        if self.y < COLS - 1:
+            neighours.append(grid[self.x][self.y + 1])
+
+        return filter(lambda n: n.celltype != WALL, neighours)
+
+
+def get_best(openset):
+    if len(openset) == 1:
+        return openset[0]
+
+    sorted = openset
+    sorted.sort(key=lambda c: c.fscore)
+
+    return sorted[0]
+
+
+def manhatan_distance(node, end):
+    return abs(node.x - end.x) + abs(node.y - end.y)
+
+
+def heuristic(node, end):
+    return manhatan_distance(node, end)
+
+
+def reconstruct_path(cameFrom, current):
+    path = [current]
+    while current in cameFrom:
+        current = cameFrom[current]
+        path.append(current)
+    return path
+
+
+def a_star(grid, start, end):
+    openset = [start]
+    cameFrom = dict()
+
+    start.s_gscore(0)
+    start.s_fscore(heuristic(start, end))
+
+    while len(openset) != 0:
+        current = get_best(openset)
+        if current.celltype == END:
+            return reconstruct_path(cameFrom, current)
+
+        openset.remove(current)
+        for n in current.g_neighbours(grid):
+            temp_gscore = current.g_gscore() + 1
+            if temp_gscore < n.g_gscore():
+                cameFrom[n] = current
+                n.s_gscore(temp_gscore)
+                n.s_fscore(temp_gscore + heuristic(n, end))
+                if n not in openset:
+                    openset.append(n)
+
+    return None