259 lines
7.0 KiB
Python
259 lines
7.0 KiB
Python
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()
|