astar fix

astar.py

@@ -3,120 +3,104 @@ from board import Board
 from constant import width, height, rows, cols
 from tractor import Tractor
 import heapq
-import math
 
 fps = 2
 WIN = pygame.display.set_mode((width, height))
 pygame.display.set_caption('Inteligenty Traktor')
 
+
 class Node:
-    def __init__(self, x, y):
+    def __init__(self, state, parent=None, action=None, cost=0):
-        self.x = x
+        self.state = state  # Stan reprezentowany przez węzeł
-        self.y = y
+        self.parent = parent  # Węzeł rodzica
-        self.f = 0
+        self.action = action  # Akcja prowadząca do tego stanu
-        self.g = 0
+        self.cost = cost  # Koszt przejścia do tego stanu
-        self.h = 0
+        self.f = 0  # Wartość funkcji priorytetowej
-        self.cost = 1
+        self.tie_breaker = 0  # Wartość używana do rozwiązywania konfliktów priorytetów
-        self.visited = False
-        self.closed = False
-        self.parent = None
 
     def __lt__(self, other):
+        # Porównanie węzłów w celu ustalenia kolejności w kolejce priorytetowej
+        if self.f == other.f:
+            return self.tie_breaker > other.tie_breaker  # Większy tie_breaker ma wyższy priorytet
         return self.f < other.f
-
+        # Jesli pare wezlow ma taie same f, to tilebreaker ustawia
-    def neighbors(self, grid):
+        # prorytety akcje right i down maja wyzszy priorytet
-        ret = []
-        x, y = self.x, self.y
-        if x > 0 and grid[x - 1][y]:
-            ret.append(grid[x - 1][y])
-        if x < len(grid) - 1 and grid[x + 1][y]:
-            ret.append(grid[x + 1][y])
-        if y > 0 and grid[x][y - 1]:
-            ret.append(grid[x][y - 1])
-        if y < len(grid[0]) - 1 and grid[x][y + 1]:
-            ret.append(grid[x][y + 1])
-        return ret
-
-def init(grid):
-    for x in range(len(grid)):
-        for y in range(len(grid[x])):
-            node = grid[x][y]
-            node.f = 0
-            node.g = 0
-            node.h = 0
-            node.cost = 1
-            node.visited = False
-            node.closed = False
-            node.parent = None
-
-def heap():
-    return []
-
-def search(grid, start, end, board, heuristic=None):
-    init(grid)
-    if heuristic is None:
-        heuristic = manhattan
-    open_heap = heap()
-    heapq.heappush(open_heap, start)
-
-    while open_heap:
-        current_node = heapq.heappop(open_heap)
-        if (current_node.x, current_node.y) == (end.x, end.y):
-
-            ret = []
-            while current_node.parent:
-                ret.append(current_node)
-                current_node = current_node.parent
-            ret.append(start)    
-            ret_path = ret[::-1]
-            for node in ret_path:
-                print(f"({node.x}, {node.y}): {node.g}")
-            print("Znaleziono ścieżkę [(x,y)jako(kolumna,wiersz)] o koszcie:", ret_path[-1].g)
-            return ret_path, start 
-        current_node.closed = True
-        for neighbor in current_node.neighbors(grid):
-            if neighbor.closed:  
-                continue
-            g_score = current_node.g + board.get_cost(neighbor.x, neighbor.y)  
-            been_visited = neighbor.visited
-            if not been_visited or g_score < neighbor.g:
-                neighbor.visited = True
-                neighbor.parent = current_node
-                neighbor.h = neighbor.h or heuristic((neighbor.x, neighbor.y), (end.x, end.y))
-                neighbor.g = g_score
-                neighbor.f = neighbor.g + neighbor.h
-                if not been_visited:
-                    heapq.heappush(open_heap, neighbor)
-    
-    print("Nie znaleziono ścieżki.")
-    return None
 
 
 def manhattan(pos0, pos1):
+    # Heurystyka odległości Manhattan
     d1 = abs(pos1[0] - pos0[0])
     d2 = abs(pos1[1] - pos0[1])
     return d1 + d2
 
 
+def nastepnik(state, board):
+    # Funkcja generująca możliwe następne stany (akcje)
+    x, y = state
+    successors = []
+    actions = [('right', (x+1, y), 1), ('down', (x, y+1), 1), ('up', (x, y-1), 0), ('left', (x-1, y), 0)]
+    for action, next_state, tie_breaker in actions:
+        if 0 <= next_state[0] < cols and 0 <= next_state[1] < rows:
+            cost = board.get_cost(next_state[0], next_state[1])
+            successors.append((action, next_state, cost, tie_breaker))
+    return successors
+
+
+def goal_test(state, goal):
+    #  Czy dany stan jest stanem docelowym
+    return state == goal
+
+
+def graphsearch(istate, goal, board, heuristic=manhattan):
+    # Algorytm przeszukiwania grafu
+    fringe = []  # Kolejka priorytetowa przechowująca węzły do odwiedzenia
+    explored = set()  # Zbiór odwiedzonych stanów
+    start_node = Node(istate)
+    start_node.f = heuristic(istate, goal)  # Obliczenie wartości heurystycznej dla stanu początkowego
+    start_node.tie_breaker = 0  # Ustawienie tie_breaker dla węzła startowego,
+    heapq.heappush(fringe, start_node)
+
+    while fringe:
+        elem = heapq.heappop(fringe)
+        if goal_test(elem.state, goal):
+            path = []
+            total_cost = elem.cost  # Zapisanie całkowitego kosztu
+            while elem:
+                path.append((elem.state, elem.action))
+                elem = elem.parent
+            return path[::-1], total_cost  # Zwrócenie ścieżki i kosztu
+
+        explored.add(elem.state)
+        for action_index, (action, state, cost, tie_breaker) in enumerate(nastepnik(elem.state, board)):
+            x = Node(state, parent=elem, action=action, cost=elem.cost + cost)
+            x.f = x.cost + heuristic(state, goal)  # Obliczenie wartości funkcji priorytetowej
+            x.tie_breaker = elem.tie_breaker * 4 + action_index  # Obliczanie tie_breaker na podstawie akcji
+
+            if state not in explored and not any(node.state == state for node in fringe):
+                heapq.heappush(fringe, x)
+            else:
+                for i, node in enumerate(fringe):
+                    if node.state == state and (node.f > x.f or (node.f == x.f and node.tie_breaker < x.tie_breaker)):
+                        fringe[i] = x
+                        heapq.heapify(fringe)
+                        break
+
+    print("Nie znaleziono ścieżki.")
+    return None, 0  # Zwrócenie ścieżki jako None i kosztu jako 0 w przypadku braku ścieżki
+
 def main():
     run = True
     clock = pygame.time.Clock()
     board = Board()
     board.load_images()
 
-    start_row, start_col = 0,0
+    start_state = (9, 9)  # Stan początkowy
-    end_row, end_col = 9,9
+    goal_state = (0, 0)  # Stan docelowy
-    tractor = Tractor(start_row, start_col)  
+    tractor = Tractor(start_state[1], start_state[0])
-    board.set_grass(start_row, start_col)
+    board.set_grass(start_state[0], start_state[1])  # Ustawienie startowego pola jako trawę
-    board.set_grass(end_row, end_col)
+    board.set_grass(goal_state[0], goal_state[1])  # Ustawienie docelowego pola jako trawę
-   
-    grid = [[Node(x, y) for y in range(rows)] for x in range(cols)]
-    
-    start = grid[start_row][start_col]
-    end = grid[end_row][end_col]
-    
-    path, start_node = search(grid, start, end, board)
 
+    path, total_cost = graphsearch(start_state, goal_state, board)
 
     while run:
         clock.tick(fps)
@@ -129,39 +113,38 @@ def main():
             run = False
             continue
 
-        next_node = path.pop(0) if path else start_node
+        next_state, action = path.pop(0) if path else (start_state, None)
-        dx = next_node.x - tractor.col
+        print(next_state)  # Wypisanie następnego stanu
-        dy = next_node.y - tractor.row
+        tractor.row, tractor.col = next_state[1], next_state[0]
-        tractor.row, tractor.col = next_node.y, next_node.x
 
-       
+        if action == "right":
-        if   dx > 0:
             tractor.direction = "right"
-        elif dx < 0:
+        elif action == "left":
             tractor.direction = "left"
-        elif dy > 0:
+        elif action == "down":
             tractor.direction = "down"
-        elif dy < 0:
+        elif action == "up":
             tractor.direction = "up"
 
+        # Aktualizacja planszy na podstawie położenia traktora
         if board.is_weed(tractor.col, tractor.row):
             board.set_grass(tractor.col, tractor.row)
-                  
         elif board.is_dirt(tractor.col, tractor.row):
             board.set_soil(tractor.col, tractor.row)
-                    
         elif board.is_soil(tractor.col, tractor.row):
             board.set_carrot(tractor.col, tractor.row)
 
-
         board.draw_cubes(WIN)
         tractor.draw(WIN)
         pygame.display.update()
 
+    print(f"Całkowity koszt trasy: {total_cost}")
+
     while True:
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 pygame.quit()
                 return
 
+
 main()