A* search
This commit is contained in:
parent
1315b33894
commit
8cdf6d8118
155
astar.py
Normal file
155
astar.py
Normal file
@ -0,0 +1,155 @@
|
||||
import pygame
|
||||
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 __lt__(self, other):
|
||||
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
|
||||
|
||||
|
||||
def manhattan(pos0, pos1):
|
||||
d1 = abs(pos1[0] - pos0[0])
|
||||
d2 = abs(pos1[1] - pos0[1])
|
||||
return d1 + d2
|
||||
|
||||
|
||||
def main():
|
||||
rotation = ["left", "up", "right", "down"]
|
||||
run = True
|
||||
clock = pygame.time.Clock()
|
||||
board = Board()
|
||||
board.load_images()
|
||||
|
||||
start_row, start_col = 1, 1
|
||||
end_row, end_col = 5, 0
|
||||
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)
|
||||
|
||||
|
||||
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_node = path.pop(0) if path else start_node
|
||||
tractor.row, tractor.col = next_node.y, next_node.x
|
||||
|
||||
|
||||
#rotation
|
||||
|
||||
|
||||
|
||||
|
||||
board.draw_cubes(WIN)
|
||||
tractor.draw(WIN)
|
||||
pygame.display.update()
|
||||
|
||||
while True:
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
return
|
||||
|
||||
main()
|
34
board.py
34
board.py
@ -8,6 +8,7 @@ class Board:
|
||||
self.board = []
|
||||
self.load_images()
|
||||
self.generate_board()
|
||||
self.load_costs()
|
||||
|
||||
|
||||
def load_images(self):
|
||||
@ -16,6 +17,7 @@ class Board:
|
||||
self.rock= pygame.image.load("board/rock.png")
|
||||
self.weeds = pygame.image.load("board/weeds.png")
|
||||
self.soil = pygame.image.load("board/zyzna.png")
|
||||
self.carrot = pygame.image.load("board/carrot.png")
|
||||
|
||||
def generate_board(self):
|
||||
self.board = [[random.choice([0,1,2,3,4,5,6,7,8,9]) for _ in range(rows)] for _ in range(cols)]
|
||||
@ -41,9 +43,35 @@ class Board:
|
||||
win.blit(self.grass, cube_rect)
|
||||
elif cube == 10:
|
||||
win.blit(self.soil, cube_rect)
|
||||
elif cube == 11:
|
||||
carrot_scale = pygame.transform.scale(self.carrot, (size,size))
|
||||
win.blit(self.carrot, cube_rect)
|
||||
win.blit(carrot_scale, cube_rect)
|
||||
|
||||
else:
|
||||
win.blit(self.dirt, cube_rect)
|
||||
|
||||
|
||||
def load_costs(self):
|
||||
self.costs = {
|
||||
0: 100, #kamien
|
||||
1: 2, #chwasty
|
||||
2: 1, #po trawie
|
||||
3: 1, #po trawie
|
||||
4: 1, #po trawie
|
||||
5: 1, #po trawie
|
||||
6: 3, #ziemia
|
||||
7: 3, #ziemia
|
||||
8: 3, #ziemia
|
||||
9: 3, #ziemia
|
||||
10: 4, #zyzna
|
||||
11: 10 #marchewka
|
||||
}
|
||||
|
||||
def get_cost(self, row, col):
|
||||
tile_type = self.board[row][col]
|
||||
return self.costs.get(tile_type, 1)
|
||||
|
||||
def is_rock(self, row, col):
|
||||
return self.board[row][col] == 0
|
||||
|
||||
@ -56,5 +84,11 @@ class Board:
|
||||
def is_dirt(self,row,col):
|
||||
return self.board[row][col] in (6,7,8,9)
|
||||
|
||||
def is_soil(self, row, col):
|
||||
return self.board[row][col] == 10
|
||||
|
||||
def set_soil(self, row, col):
|
||||
self.board[row][col] = 10
|
||||
|
||||
def set_carrot(self, row, col):
|
||||
self.board[row][col] = 11
|
Loading…
Reference in New Issue
Block a user