From d7c3f503227c503a5fd9af92b911351b8d9b68cc Mon Sep 17 00:00:00 2001 From: Uni Date: Wed, 8 May 2024 19:50:30 +0200 Subject: [PATCH] bfs b A* g, poczatki A*, bardzo do poprawy --- app.py | 47 ++++++++++++++++++++--------------------------- astar.py | 49 +++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 69 insertions(+), 27 deletions(-) create mode 100644 astar.py diff --git a/app.py b/app.py index d87b03c..b156228 100644 --- a/app.py +++ b/app.py @@ -3,12 +3,14 @@ import prefs from classes.beerKeg import BeerKeg from classes.coffeMachine import CoffeMachine from classes.table import Table -from pygame.locals import K_w, K_s, K_a, K_d, K_q, K_e, K_r, K_b +from pygame.locals import K_w, K_s, K_a, K_d, K_q, K_e, K_r, K_b,K_g from classes.cell import Cell from classes.agent import Agent from collections import deque import threading import time +from astar import AStar + pygame.init() window = pygame.display.set_mode((prefs.WIDTH, prefs.HEIGHT)) pygame.display.set_caption("Game Window") @@ -23,8 +25,8 @@ def initBoard(): row.append(cell) cells.append(row) - #test - # Na potrzeby prezentacji tworzę sobie prostokatne sciany na które nie da się wejść + # Test + # Na potrzeby prezentacji tworzę sobie prostokątne ściany na które nie da się wejść x1 = 3 y1 = 6 for i in range(x1, x1+4): @@ -39,14 +41,12 @@ def initBoard(): cells[6][2].interactableItem = Table(cells[6][2], "Table") cells[4][2].interactableItem = Table(cells[4][2], "Table") - cells[9][9].waga = 2 cells[9][8].waga = 2 cells[9][7].waga = 2 cells[10][6].waga = 2 cells[7][7].waga = 2 - def draw_grid(window, cells, agent): for i in range(prefs.GRID_SIZE): for j in range(prefs.GRID_SIZE): @@ -64,33 +64,30 @@ def draw_grid(window, cells, agent): initBoard() agent = Agent(prefs.SPAWN_POINT[0], prefs.SPAWN_POINT[1], cells) -target_x, target_y = 9,11 +target_x, target_y = 9, 11 + def watekDlaSciezkiAgenta(): time.sleep(3) while True: if len(path) > 0: element = path.pop(0) print(element) - if element =="left": + if element == "left": agent.rotate_left() - if element =="right": + if element == "right": agent.rotate_right() - if element =="forward": + if element == "forward": agent.move_direction() time.sleep(1) - - - running = True while running: for event in pygame.event.get(): if event.type == pygame.QUIT: running = False -# takie głupie kontrolki do usunięcia potem, tylko do preznetacji - keys = pygame.key.get_pressed() + if keys[K_w] and not agent.moved: agent.move_up() if keys[K_s] and not agent.moved: @@ -111,19 +108,21 @@ while running: agent.move_direction() if keys[K_b]: - path = agent.bfs2(target_x, target_y) - print("Najkrótsza ścieżka:", path) - watek = threading.Thread(target=watekDlaSciezkiAgenta) - watek.daemon = True - watek.start() + path = agent.bfs2(target_x, target_y) + print("Najkrótsza ścieżka:", path) + watek = threading.Thread(target=watekDlaSciezkiAgenta) + watek.daemon = True + watek.start() + + if keys[K_g]: + path = AStar(cells).astar((agent.current_cell.X, agent.current_cell.Y), (target_x, target_y)) + print("Shortest path:", path) if pygame.key.get_pressed()[pygame.K_e]: if agent.current_cell.interactableItem and pygame.time.get_ticks() - agent.last_interact_time > 500: agent.last_interact_time = pygame.time.get_ticks() agent.current_cell.interactableItem.interact(agent) - - window.fill((255, 0, 0)) draw_grid(window, cells, agent) agent.update(window) @@ -131,9 +130,3 @@ while running: time.sleep(0.1) pygame.quit() - - - - - - diff --git a/astar.py b/astar.py new file mode 100644 index 0000000..8a264dd --- /dev/null +++ b/astar.py @@ -0,0 +1,49 @@ +from collections import deque +import heapq + +class AStar: + def __init__(self, cells): + self.cells = cells + + def heuristic(self, current, target): + # Euclidean distance heuristic + dx = abs(current[0] - target[0]) + dy = abs(current[1] - target[1]) + return dx + dy + + def get_neighbors(self, cell): + neighbors = [] + x, y = cell[0], cell[1] + if x > 0 and not self.cells[x - 1][y].blocking_movement: + neighbors.append((x - 1, y)) + if x < len(self.cells) - 1 and not self.cells[x + 1][y].blocking_movement: + neighbors.append((x + 1, y)) + if y > 0 and not self.cells[x][y - 1].blocking_movement: + neighbors.append((x, y - 1)) + if y < len(self.cells[x]) - 1 and not self.cells[x][y + 1].blocking_movement: + neighbors.append((x, y + 1)) + return neighbors + + def astar(self, start, target): + open_list = [(0, start)] + came_from = {} + g_score = {start: 0} + + while open_list: + _, current = heapq.heappop(open_list) + if current == target: + path = [] + while current in came_from: + path.append(current) + current = came_from[current] + return path[::-1] + + for neighbor in self.get_neighbors(current): + tentative_g_score = g_score[current] + 1 + if tentative_g_score < g_score.get(neighbor, float('inf')): + came_from[neighbor] = current + g_score[neighbor] = tentative_g_score + f_score = tentative_g_score + self.heuristic(neighbor, target) + heapq.heappush(open_list, (f_score, neighbor)) + + return []