astar fix
This commit is contained in:
parent
11d091de6a
commit
0c5532ac0d
199
astar.py
199
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):
|
||||
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)
|
||||
@ -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()
|
||||
Loading…
Reference in New Issue
Block a user