bfs b A* g, poczatki A*, bardzo do poprawy

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()
-
-
-
-
-
-

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 []