167 lines
4.7 KiB
Python
167 lines
4.7 KiB
Python
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():
|
|
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)
|
|
|
|
|
|
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
|
|
dx = next_node.x - tractor.col
|
|
dy = next_node.y - tractor.row
|
|
tractor.row, tractor.col = next_node.y, next_node.x
|
|
|
|
|
|
if dx > 0:
|
|
tractor.direction = "right"
|
|
elif dx < 0:
|
|
tractor.direction = "left"
|
|
elif dy > 0:
|
|
tractor.direction = "down"
|
|
elif dy < 0:
|
|
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 )
|
|
|
|
|
|
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() |