import pygame import math from graph import Queue # Define some colors BLACK = (0, 0, 0) WHITE = (255, 255, 255) BLUE = (0, 0, 255) GREEN = (0, 255, 0) RED = (255, 0, 0) ORANGE = (255, 165, 0) GREY = (128, 128, 128) WIDTH = 25 HEIGHT = 25 MARGIN = 3 grid_size = 20 class Field: def __init__(self, x, y): self.x = x self.y = y self.state = "free" # states: free, obstacale, start, target self.g = float('inf') self.h = 0 self.f = float('inf') self.parent = None def draw(self, screen): # state based coloring color = WHITE if self.state == "obstacale": color = BLACK elif self.state == "start": color = BLUE elif self.state == "target": color = GREEN elif self.state == "path": color = ORANGE elif self.state == "current": color = RED elif self.state == "visited": color = GREY x_calc = (MARGIN + WIDTH) * self.x + MARGIN y_calc = (MARGIN + HEIGHT) * (grid_size - 1 - self.y) + MARGIN # flipping pygame.draw.rect( screen, color, [x_calc, y_calc, WIDTH, HEIGHT] ) ''' # Render the heuristic value as text if self.state != "obstacale": # Don't display on obstacles # Create a font object font = pygame.font.Font(None, 16) # None means default font, 14 is the size # Round the heuristic value to 1 decimal place for better display f_text = f"{self.g:.1f}" # Render the text text = font.render(f_text, True, BLACK) # True for anti-aliasing, BLACK for text color # Calculate text position (centered in the rectangle) text_rect = text.get_rect(center=(x_calc + WIDTH / 2, y_calc + HEIGHT / 2)) # Draw the text on the screen screen.blit(text, text_rect) ''' class Grid: def __init__(self, cols, rows): self.cols = cols # x self.rows = rows # y self.grid = [] i = 0 while i < cols: # col = x col = [] j = 0 while j < rows: # row = y col.append(Field(i, j)) # (x,y) j += 1 self.grid.append(col) i += 1 self.start = None self.target = None def draw(self, screen): for col in self.grid: for field in col: field.draw(screen) def heuristic(self, field): return math.sqrt((field.x - self.target[0]) ** 2 + (field.y - self.target[1]) ** 2) def get_state(self, x, y): return self.grid[x][y].state def set_state(self, state, x, y): if state == "free" or state == "obstacale" or state == "start" or state == "target" or state == "path" or state == "current" or state == "visited": self.grid[x][y].state = state def set_free(self, x, y): self.set_state("free", x, y) def set_obstacle(self, x, y): self.set_state("obstacale", x, y) def set_current(self, x, y): self.set_state("current", x, y) def set_visited(self, x,y): self.set_state("visited", x, y) def set_path(self, x, y): if not (x == self.start[0] and y == self.start[1]) and not (x == self.target[0] and y == self.target[1]): self.set_state("path", x, y) def set_start(self, x, y): # reset old start if it exits if self.start: self.set_free(self.start[0], self.start[1]) self.set_state("start", x, y) self.grid[x][y].parent = self.grid[x][y] self.grid[x][y].g = 0 self.grid[x][y].h = self.heuristic(self.grid[x][y]) self.grid[x][y].f = self.grid[x][y].h + self.grid[x][y].g self.start = (x, y) def set_target(self, x, y): # reset old target if it exits if self.target: self.set_free(self.target[0], self.target[1]) self.set_state("target", x, y) self.target = (x, y) ''' Initializing the Grid ''' start = (0, 0) target = (19, 0) grid = Grid(grid_size, grid_size) # check if start an target are valid if 0 <= start[0] < grid.cols and 0 <= target[0] < grid.cols and 0 <= start[1] < grid.cols and 0 <= target[ 1] < grid.cols: grid.set_target(target[0], target[1]) grid.set_start(start[0], start[1]) for i in range(0, 10): grid.set_obstacle(9, i) for j in range(4, 10): grid.set_obstacle(j, 9) for i in range(9, 20): grid.set_obstacle(16, i) ''' Initializing A* Components ''' open = Queue('PRIO', 'f') open.push(grid.grid[0][0]) closed = Queue('PRIO', 'f') neighbors = [] path = [] ''' Initializing Visuals ''' pygame.init() window_width = grid_size * (WIDTH + MARGIN) + MARGIN window_height = grid_size * (HEIGHT + MARGIN) + MARGIN size = (window_width, window_height) # made size variable screen = pygame.display.set_mode(size) pygame.display.set_caption("A* Algorithm") done = False clock = pygame.time.Clock() while not done: for event in pygame.event.get(): if event.type == pygame.QUIT: done = True while not open.is_empty(): current_field = open.pop() grid.set_current(current_field.x, current_field.y) screen.fill(BLACK) grid.draw(screen) pygame.display.flip() clock.tick(15) if current_field.x == grid.target[0] and current_field.y == grid.target[1]: path.append(current_field) grid.set_path(current_field.x, current_field.y) while not (current_field.x == grid.start[0] and current_field.y == grid.start[1]): current_field = current_field.parent path.insert(0, current_field) grid.set_path(current_field.x, current_field.y) grid.draw(screen) pygame.display.flip() clock.tick(15) open.clear() closed.push(current_field) grid.set_visited(current_field.x, current_field.y) for dx, dy in [(0, -1), (-1, 0), (0, 1), (1, 0)]: nx = current_field.x + dx ny = current_field.y + dy if 0 <= nx < grid.cols and 0 <= ny < grid.rows: neighbor = grid.grid[nx][ny] if neighbor.state == "obstacale" or closed.items.__contains__(neighbor): continue tentative_g = current_field.g + 1 if tentative_g < neighbor.g: neighbor.parent = current_field neighbor.g = tentative_g neighbor.h = grid.heuristic(neighbor) neighbor.f = neighbor.g + neighbor.h if open.items.__contains__(neighbor): open.items.sort(key=lambda item: item.f) else: open.push(neighbor) screen.fill(BLACK) grid.draw(screen) pygame.display.flip() # refreshrate clock.tick(30) pygame.quit()