diff --git a/astar.py b/astar.py new file mode 100644 index 0000000..cc3ca77 --- /dev/null +++ b/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 diff --git a/consts.py b/consts.py new file mode 100644 index 0000000..c85c099 --- /dev/null +++ b/consts.py @@ -0,0 +1,11 @@ + +WIN_WIDTH = 500 +WIN_HEIGHT = 500 + +COLS = 10 +ROWS = 10 + +PADDING = 5 + +CELL_SIZE = WIN_WIDTH / COLS +CELL_SIZE_PADDED = CELL_SIZE - PADDING * 2 diff --git a/drawing.py b/drawing.py new file mode 100644 index 0000000..12f0d7d --- /dev/null +++ b/drawing.py @@ -0,0 +1,28 @@ +import pygame +from pygame.locals import * + +from consts import * + + +def draw_cell(display, position, color): + position = (position[0] * CELL_SIZE + PADDING, position[1] * CELL_SIZE + PADDING) + rect = pygame.Rect(position, (40, 40)) + pygame.draw.rect(display, color, rect) + + +def gridtoscreen(pos): + return (pos[0] * CELL_SIZE, pos[1] * CELL_SIZE) + + +def center_line(pos): + return (pos[0] + CELL_SIZE / 2, pos[1] + CELL_SIZE / 2) + + +def draw_path(display, color, path): + if path is None: + return + + for i in range(0, len(path)-1): + n1 = center_line(gridtoscreen(path[i].g_pos())) + n2 = center_line(gridtoscreen(path[i + 1].g_pos())) + pygame.draw.line(display, color, n1, n2, 4)