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 == "visited": color = WHITE 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.f:.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 == "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_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_visited(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("visited", 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.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, 19) 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* Comps ''' open = Queue('PRIO', 'f') open.push(grid.grid[0][0]) closed = Queue('PRIO', 'f') neighbors = [] path = [] pygame.init() # window 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() def a_star(): neighbor = None while not open.empty(): current_field = open.pop() 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) break closed.push(current_field) grid.set_visited(current_field.x, current_field.y) # Nachbarn finden for dx, dy in [(0, -1), (-1, 0), (0, 1), (1, 0)]: nx = current_field.x + dx ny = current_field.y + dy # Prüfen, ob der Nachbar gültig ist if 0 <= nx < grid.cols and 0 <= ny < grid.rows: neighbor = grid.grid[nx][ny] # Hindernis oder bereits in geschlossener Liste -> überspringen if neighbor.state == "obstacale" or neighbor in [item for item in closed.items]: continue # Neuen g-Wert berechnen tentative_g = current_field.g + 1 # Kosten für einen Schritt = 1 # Wenn Nachbar nicht in offener Liste oder neuer Pfad besser if neighbor not in [item for item in open.items] or tentative_g < neighbor.g: neighbor.parent = current_field neighbor.g = tentative_g neighbor.h = grid.heuristic(neighbor) neighbor.f = neighbor.g + neighbor.h # Knoten zur offenen Liste hinzufügen oder aktualisieren if neighbor not in [item for item in open.items]: open.push(neighbor) else: # Queue aktualisieren open.items.sort(key=lambda item: item.f) def a_star_main(): global done while not done: for event in pygame.event.get(): if event.type == pygame.QUIT: done = True a_star() screen.fill(BLACK) grid.draw(screen) # refresh display pygame.display.flip() # refreshrate clock.tick(120) a_star_main() pygame.quit()