astar fix

This commit is contained in:
s481904 2024-06-08 12:41:46 +02:00
parent 11d091de6a
commit 0c5532ac0d

187
astar.py
View File

@ -3,120 +3,104 @@ from board import Board
from constant import width, height, rows, cols from constant import width, height, rows, cols
from tractor import Tractor from tractor import Tractor
import heapq import heapq
import math
fps = 2 fps = 2
WIN = pygame.display.set_mode((width, height)) WIN = pygame.display.set_mode((width, height))
pygame.display.set_caption('Inteligenty Traktor') pygame.display.set_caption('Inteligenty Traktor')
class Node: 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): 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 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): def manhattan(pos0, pos1):
# Heurystyka odległości Manhattan
d1 = abs(pos1[0] - pos0[0]) d1 = abs(pos1[0] - pos0[0])
d2 = abs(pos1[1] - pos0[1]) d2 = abs(pos1[1] - pos0[1])
return d1 + d2 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(): def main():
run = True run = True
clock = pygame.time.Clock() clock = pygame.time.Clock()
board = Board() board = Board()
board.load_images() 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: while run:
clock.tick(fps) clock.tick(fps)
@ -129,39 +113,38 @@ def main():
run = False run = False
continue 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" tractor.direction = "right"
elif dx < 0: elif action == "left":
tractor.direction = "left" tractor.direction = "left"
elif dy > 0: elif action == "down":
tractor.direction = "down" tractor.direction = "down"
elif dy < 0: elif action == "up":
tractor.direction = "up" tractor.direction = "up"
# Aktualizacja planszy na podstawie położenia traktora
if board.is_weed(tractor.col, tractor.row): if board.is_weed(tractor.col, tractor.row):
board.set_grass(tractor.col, tractor.row) board.set_grass(tractor.col, tractor.row)
elif board.is_dirt(tractor.col, tractor.row): elif board.is_dirt(tractor.col, tractor.row):
board.set_soil(tractor.col, tractor.row) board.set_soil(tractor.col, tractor.row)
elif board.is_soil(tractor.col, tractor.row): elif board.is_soil(tractor.col, tractor.row):
board.set_carrot(tractor.col, tractor.row) board.set_carrot(tractor.col, tractor.row)
board.draw_cubes(WIN) board.draw_cubes(WIN)
tractor.draw(WIN) tractor.draw(WIN)
pygame.display.update() pygame.display.update()
print(f"Całkowity koszt trasy: {total_cost}")
while True: while True:
for event in pygame.event.get(): for event in pygame.event.get():
if event.type == pygame.QUIT: if event.type == pygame.QUIT:
pygame.quit() pygame.quit()
return return
main() main()