|
| 1 | +import pygame |
| 2 | +import heapq |
| 3 | +import math |
| 4 | +from collections import deque |
| 5 | + |
| 6 | +# Constants |
| 7 | +WIDTH = 800 |
| 8 | +HEIGHT = 600 |
| 9 | +GRID_SIZE = 20 |
| 10 | +BACKGROUND_COLOR = (0, 0, 0) |
| 11 | +WALL_COLOR = (50, 50, 50) |
| 12 | +PATH_COLOR = (255, 0, 0) |
| 13 | +START_COLOR = (0, 255, 0) |
| 14 | +END_COLOR = (0, 0, 255) |
| 15 | +OPEN_COLOR = (255, 255, 255) |
| 16 | +CLOSED_COLOR = (128, 128, 128) |
| 17 | +TEXT_COLOR = (255, 255, 255) |
| 18 | + |
| 19 | +class Node: |
| 20 | + def __init__(self, row, col, width): |
| 21 | + self.row = row |
| 22 | + self.col = col |
| 23 | + self.x = col * width |
| 24 | + self.y = row * width |
| 25 | + self.width = width |
| 26 | + self.color = BACKGROUND_COLOR |
| 27 | + self.neighbors = [] |
| 28 | + self.g_score = float('inf') # To keep track of g_score in A* |
| 29 | + self.f_score = float('inf') # To keep track of f_score in A* |
| 30 | + |
| 31 | + def get_pos(self): |
| 32 | + return self.row, self.col |
| 33 | + |
| 34 | + def is_closed(self): |
| 35 | + return self.color == CLOSED_COLOR |
| 36 | + |
| 37 | + def is_open(self): |
| 38 | + return self.color == OPEN_COLOR |
| 39 | + |
| 40 | + def is_wall(self): |
| 41 | + return self.color == WALL_COLOR |
| 42 | + |
| 43 | + def is_start(self): |
| 44 | + return self.color == START_COLOR |
| 45 | + |
| 46 | + def is_end(self): |
| 47 | + return self.color == END_COLOR |
| 48 | + |
| 49 | + def reset(self): |
| 50 | + self.color = BACKGROUND_COLOR |
| 51 | + |
| 52 | + def make_start(self): |
| 53 | + self.color = START_COLOR |
| 54 | + |
| 55 | + def make_end(self): |
| 56 | + self.color = END_COLOR |
| 57 | + |
| 58 | + def make_wall(self): |
| 59 | + self.color = WALL_COLOR |
| 60 | + |
| 61 | + def make_open(self): |
| 62 | + self.color = OPEN_COLOR |
| 63 | + |
| 64 | + def make_closed(self): |
| 65 | + self.color = CLOSED_COLOR |
| 66 | + |
| 67 | + def make_path(self): |
| 68 | + self.color = PATH_COLOR |
| 69 | + |
| 70 | + def draw(self, win): |
| 71 | + pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.width)) |
| 72 | + |
| 73 | + def update_neighbors(self, grid): |
| 74 | + self.neighbors = [] |
| 75 | + if self.row < len(grid) - 1 and not grid[self.row + 1][self.col].is_wall(): # Down |
| 76 | + self.neighbors.append(grid[self.row + 1][self.col]) |
| 77 | + if self.row > 0 and not grid[self.row - 1][self.col].is_wall(): # Up |
| 78 | + self.neighbors.append(grid[self.row - 1][self.col]) |
| 79 | + if self.col < len(grid[0]) - 1 and not grid[self.row][self.col + 1].is_wall(): # Right |
| 80 | + self.neighbors.append(grid[self.row][self.col + 1]) |
| 81 | + if self.col > 0 and not grid[self.row][self.col - 1].is_wall(): # Left |
| 82 | + self.neighbors.append(grid[self.row][self.col - 1]) |
| 83 | + |
| 84 | + def __lt__(self, other): |
| 85 | + return self.f_score < other.f_score |
| 86 | + |
| 87 | + |
| 88 | +# A* algorithm |
| 89 | +def astar_algorithm(draw, grid, start, end): |
| 90 | + open_set = [] |
| 91 | + heapq.heappush(open_set, (0, start)) |
| 92 | + came_from = {} |
| 93 | + g_score = {node: float("inf") for row in grid for node in row} |
| 94 | + g_score[start] = 0 |
| 95 | + f_score = {node: float("inf") for row in grid for node in row} |
| 96 | + f_score[start] = heuristic(start.get_pos(), end.get_pos()) |
| 97 | + |
| 98 | + while open_set: |
| 99 | + for event in pygame.event.get(): |
| 100 | + if event.type == pygame.QUIT: |
| 101 | + pygame.quit() |
| 102 | + current = heapq.heappop(open_set)[1] |
| 103 | + |
| 104 | + if current == end: |
| 105 | + reconstruct_path(came_from, end, draw) |
| 106 | + end.make_end() |
| 107 | + start.make_start() |
| 108 | + return True |
| 109 | + |
| 110 | + for neighbor in current.neighbors: |
| 111 | + tentative_g_score = g_score[current] + 1 |
| 112 | + if tentative_g_score < g_score[neighbor]: |
| 113 | + came_from[neighbor] = current |
| 114 | + g_score[neighbor] = tentative_g_score |
| 115 | + f_score[neighbor] = g_score[neighbor] + heuristic(neighbor.get_pos(), end.get_pos()) |
| 116 | + neighbor.g_score = g_score[neighbor] |
| 117 | + neighbor.f_score = f_score[neighbor] |
| 118 | + if neighbor not in [i[1] for i in open_set]: |
| 119 | + heapq.heappush(open_set, (f_score[neighbor], neighbor)) |
| 120 | + neighbor.make_open() |
| 121 | + |
| 122 | + draw() |
| 123 | + if current != start: |
| 124 | + current.make_closed() |
| 125 | + |
| 126 | + return False |
| 127 | + |
| 128 | +# BFS algorithm |
| 129 | +def bfs_algorithm(draw, grid, start, end): |
| 130 | + queue = deque([start]) |
| 131 | + came_from = {} |
| 132 | + g_score = {node: float("inf") for row in grid for node in row} |
| 133 | + g_score[start] = 0 |
| 134 | + |
| 135 | + while queue: |
| 136 | + for event in pygame.event.get(): |
| 137 | + if event.type == pygame.QUIT: |
| 138 | + pygame.quit() |
| 139 | + current = queue.popleft() |
| 140 | + |
| 141 | + if current == end: |
| 142 | + reconstruct_path(came_from, end, draw) |
| 143 | + end.make_end() |
| 144 | + start.make_start() |
| 145 | + return True |
| 146 | + |
| 147 | + for neighbor in current.neighbors: |
| 148 | + if g_score[neighbor] == float("inf"): |
| 149 | + came_from[neighbor] = current |
| 150 | + g_score[neighbor] = g_score[current] + 1 |
| 151 | + queue.append(neighbor) |
| 152 | + neighbor.make_open() |
| 153 | + |
| 154 | + draw() |
| 155 | + if current != start: |
| 156 | + current.make_closed() |
| 157 | + |
| 158 | + return False |
| 159 | + |
| 160 | +# Heuristic function for A* algorithm |
| 161 | +def heuristic(pos1, pos2): |
| 162 | + x1, y1 = pos1 |
| 163 | + x2, y2 = pos2 |
| 164 | + return abs(x1 - x2) + abs(y1 - y2) |
| 165 | + |
| 166 | +# Reconstruct path from end node to start node |
| 167 | +def reconstruct_path(came_from, current, draw): |
| 168 | + while current in came_from: |
| 169 | + current = came_from[current] |
| 170 | + current.make_path() |
| 171 | + draw() |
| 172 | + |
| 173 | +# Draw grid and nodes |
| 174 | +def draw_grid(win, grid): |
| 175 | + for row in grid: |
| 176 | + for node in row: |
| 177 | + node.draw(win) |
| 178 | + |
| 179 | +def draw(win, grid, algorithm): |
| 180 | + win.fill(BACKGROUND_COLOR) |
| 181 | + draw_grid(win, grid) |
| 182 | + pygame.display.update() |
| 183 | + |
| 184 | +def main(): |
| 185 | + pygame.init() |
| 186 | + win = pygame.display.set_mode((WIDTH, HEIGHT)) |
| 187 | + pygame.display.set_caption("Pathfinding Visualization") |
| 188 | + |
| 189 | + grid = [[Node(row, col, GRID_SIZE) for col in range(WIDTH // GRID_SIZE)] for row in range(HEIGHT // GRID_SIZE)] |
| 190 | + start = None |
| 191 | + end = None |
| 192 | + |
| 193 | + run = True |
| 194 | + while run: |
| 195 | + draw(win, grid, None) |
| 196 | + for event in pygame.event.get(): |
| 197 | + if event.type == pygame.QUIT: |
| 198 | + run = False |
| 199 | + if pygame.mouse.get_pressed()[0]: # Left mouse button |
| 200 | + pos = pygame.mouse.get_pos() |
| 201 | + row, col = pos[1] // GRID_SIZE, pos[0] // GRID_SIZE |
| 202 | + node = grid[row][col] |
| 203 | + if not start and node != end: |
| 204 | + start = node |
| 205 | + start.make_start() |
| 206 | + elif not end and node != start: |
| 207 | + end = node |
| 208 | + end.make_end() |
| 209 | + elif node != end and node != start: |
| 210 | + node.make_wall() |
| 211 | + elif pygame.mouse.get_pressed()[2]: # Right mouse button |
| 212 | + pos = pygame.mouse.get_pos() |
| 213 | + row, col = pos[1] // GRID_SIZE, pos[0] // GRID_SIZE |
| 214 | + node = grid[row][col] |
| 215 | + node.reset() |
| 216 | + if node == start: |
| 217 | + start = None |
| 218 | + elif node == end: |
| 219 | + end = None |
| 220 | + |
| 221 | + if event.type == pygame.KEYDOWN: |
| 222 | + if event.key == pygame.K_SPACE and start and end: |
| 223 | + for row in grid: |
| 224 | + for node in row: |
| 225 | + node.update_neighbors(grid) |
| 226 | + astar_algorithm(lambda: draw(win, grid, "A*"), grid, start, end) |
| 227 | + if event.key == pygame.K_b and start and end: |
| 228 | + for row in grid: |
| 229 | + for node in row: |
| 230 | + node.update_neighbors(grid) |
| 231 | + bfs_algorithm(lambda: draw(win, grid, "BFS"), grid, start, end) |
| 232 | + |
| 233 | + pygame.quit() |
| 234 | + |
| 235 | +if __name__ == "__main__": |
| 236 | + main() |
0 commit comments