Traktor/astar.py
2024-06-08 12:41:46 +02:00

150 lines
5.5 KiB
Python

import pygame
from board import Board
from constant import width, height, rows, cols
from tractor import Tractor
import heapq
fps = 2
WIN = pygame.display.set_mode((width, height))
pygame.display.set_caption('Inteligenty Traktor')
class Node:
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
# 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_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
if not path:
run = False
continue
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 action == "right":
tractor.direction = "right"
elif action == "left":
tractor.direction = "left"
elif action == "down":
tractor.direction = "down"
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()