diff --git a/astar.py b/astar.py index 075fdd16f..710f6e9ca 100644 --- a/astar.py +++ b/astar.py @@ -3,124 +3,108 @@ 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): - self.x = x - self.y = y - self.f = 0 - self.g = 0 - self.h = 0 - self.cost = 1 - self.visited = False - self.closed = False - self.parent = None + def __init__(self, state, parent=None, action=None, cost=0): + self.state = state # Stan reprezentowany przez węzeł + self.parent = parent # Węzeł rodzica + self.action = action # Akcja prowadząca do tego stanu + self.cost = cost # Koszt przejścia do tego stanu + self.f = 0 # Wartość funkcji priorytetowej + self.tie_breaker = 0 # Wartość używana do rozwiązywania konfliktów priorytetów 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 - - def neighbors(self, grid): - 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 + # Jesli pare wezlow ma taie same f, to tilebreaker ustawia + # prorytety akcje right i down maja wyzszy priorytet 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 - end_row, end_col = 9,9 - tractor = Tractor(start_row, start_col) - board.set_grass(start_row, start_col) - board.set_grass(end_row, end_col) - - 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) - + + start_state = (9, 9) # Stan początkowy + goal_state = (0, 0) # Stan docelowy + tractor = Tractor(start_state[1], start_state[0]) + board.set_grass(start_state[0], start_state[1]) # Ustawienie startowego pola jako trawę + board.set_grass(goal_state[0], goal_state[1]) # Ustawienie docelowego pola jako trawę + + path, total_cost = graphsearch(start_state, goal_state, board) while run: clock.tick(fps) - + for event in pygame.event.get(): if event.type == pygame.QUIT: run = False @@ -129,39 +113,38 @@ def main(): run = False continue - next_node = path.pop(0) if path else start_node - dx = next_node.x - tractor.col - dy = next_node.y - tractor.row - tractor.row, tractor.col = next_node.y, next_node.x + next_state, action = path.pop(0) if path else (start_state, None) + print(next_state) # Wypisanie następnego stanu + tractor.row, tractor.col = next_state[1], next_state[0] - - if dx > 0: + if action == "right": 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" - - 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 ) - + + # 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() \ No newline at end of file