150 lines
5.5 KiB
Python
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() |