161 lines
6.7 KiB
Python
161 lines
6.7 KiB
Python
import math
|
|
from operator import ne
|
|
|
|
import pygame
|
|
from Global_variables import Global_variables as G_var
|
|
from Min_heap import Min_heap
|
|
from Node import Node, State
|
|
from Package import Package
|
|
from Shelf import Shelf
|
|
|
|
|
|
class Pathfinding:
|
|
def __init__(self, enviroment_2d):
|
|
self.enviroment_2d = enviroment_2d
|
|
self.reset_grid()
|
|
self.path = []
|
|
|
|
def reset_grid(self):
|
|
self.grid = [[ # tworze pustej tablicy o wymiarach naszej kraty
|
|
None
|
|
for y in range(G_var().DIMENSION_Y)]
|
|
for x in range(G_var().DIMENSION_X)
|
|
]
|
|
for x in range(G_var().DIMENSION_X): # zapełnianie tablicy obiektami Node
|
|
for y in range(G_var().DIMENSION_Y):
|
|
is_walkable = True
|
|
to_check_type = self.enviroment_2d[x][y]
|
|
if isinstance(to_check_type, Shelf):
|
|
is_walkable = False
|
|
elif isinstance(to_check_type, Package) and to_check_type.is_placed:
|
|
is_walkable = False
|
|
self.grid[x][y] = Node(State(1, x, y), is_walkable)
|
|
|
|
def succ(self,node): #funckja zwraca sąsiadów noda w argumencie
|
|
node_x = node.state.x
|
|
node_y = node.state.y
|
|
neighbours = []
|
|
neighbours_cords = [[1,0],[-1,0],[0,-1],[0,1]]
|
|
for cord in neighbours_cords:
|
|
neighbour_x = node_x + cord[0]
|
|
neighbour_y = node_y + cord[1]
|
|
if(neighbour_x >= 0 and neighbour_x < G_var().DIMENSION_X and neighbour_y >= 0 and neighbour_y < G_var().DIMENSION_Y):
|
|
neighbours.append(self.grid[neighbour_x][neighbour_y])
|
|
return neighbours
|
|
|
|
################## TO REMOVE
|
|
def set_text(self, string, coordx, coordy, fontSize): #Function to set text
|
|
|
|
font = pygame.font.Font('freesansbold.ttf', fontSize)
|
|
#(0, 0, 0) is black, to make black text
|
|
string = str(string)
|
|
text = font.render(string, True, (0, 0, 0))
|
|
textRect = text.get_rect()
|
|
textRect.center = (coordx, coordy)
|
|
return (text, textRect)
|
|
|
|
def draw_node(self,node, window, color):
|
|
node_x = node.state.x
|
|
node_y = node.state.y
|
|
#######################SET TEXT
|
|
f_cost_text = self.set_text(node.f_cost(), node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/2), node_y *
|
|
G_var().RECT_SIZE + (G_var().RECT_SIZE/2), 10)
|
|
g_cost_text = self.set_text(node.g_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4), node_y *
|
|
G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)
|
|
h_cost_text = self.set_text(node.h_cost, node_x * G_var().RECT_SIZE + (G_var().RECT_SIZE/4*3), node_y *
|
|
G_var().RECT_SIZE + (G_var().RECT_SIZE/4), 10)
|
|
|
|
###############################
|
|
node_x = node.state.x
|
|
node_y = node.state.y
|
|
block = pygame.Rect(
|
|
node_x * G_var().RECT_SIZE, node_y *
|
|
G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE
|
|
)
|
|
pygame.draw.rect(window,
|
|
color,
|
|
block)
|
|
window.blit(f_cost_text[0], f_cost_text[1])
|
|
window.blit(g_cost_text[0], g_cost_text[1])
|
|
window.blit(h_cost_text[0], h_cost_text[1])
|
|
|
|
###############################
|
|
|
|
def find_path(self, starting_state, target_state, window): # algorytm wyszukiwania trasy
|
|
start_node = self.grid[starting_state.x][starting_state.y]
|
|
target_node = self.grid[target_state.x][target_state.y]
|
|
|
|
fringe = Min_heap()
|
|
explored = []
|
|
|
|
is_target_node_walkable = True
|
|
if not target_node.walkable:
|
|
target_node.walkable = True
|
|
is_target_node_walkable = False
|
|
fringe.insert(start_node)
|
|
|
|
while fringe.count() > 0:
|
|
# current_node = fringe[0]
|
|
current_node = fringe.extract()
|
|
#################################################### TEST
|
|
# current_node_color = (213, 55, 221)
|
|
# to_check_color = (55, 213, 55)
|
|
# current_color = (233,55,55)
|
|
# # self.draw_node(current_node,window,current_node_color)
|
|
# for node_to_check in explored:
|
|
# self.draw_node(node_to_check,window, current_node_color)
|
|
# for node_to_check in fringe.items:
|
|
# self.draw_node(node_to_check,window, to_check_color)
|
|
# self.draw_node(current_node,window,current_color)
|
|
# pygame.display.flip()
|
|
###############################################################
|
|
explored.append(current_node)
|
|
|
|
if current_node.state == target_node.state:
|
|
path = self.retrace_path(start_node,target_node)
|
|
self.path = path
|
|
target_node.walkable = is_target_node_walkable
|
|
return
|
|
|
|
for neighbour in self.succ(current_node):
|
|
neighbour_in_explored = [e for e in explored if e.state == neighbour.state]
|
|
if not neighbour.walkable or len(neighbour_in_explored) > 0:
|
|
continue
|
|
new_movement_cost_to_neighbour = current_node.g_cost + self.get_distance(current_node,neighbour)
|
|
if new_movement_cost_to_neighbour < neighbour.g_cost or not fringe.contains(neighbour):
|
|
neighbour.g_cost = new_movement_cost_to_neighbour
|
|
neighbour.h_cost = self.get_distance(neighbour,target_node)
|
|
neighbour.parent = current_node
|
|
if not fringe.contains(neighbour):
|
|
fringe.insert(neighbour)
|
|
target_node.walkable = is_target_node_walkable
|
|
|
|
def get_distance(self, node_a, node_b): # funckja liczy dystans dla odległości między dwoma nodami
|
|
dist_x = abs(node_a.state.x - node_b.state.x)
|
|
dist_y = abs(node_a.state.y - node_b.state.y)
|
|
|
|
return (dist_x + dist_y) * 10
|
|
|
|
def retrace_path(self, start_node, end_node): # funkcja zwraca tablice która ma w sobie wartosci pola parent
|
|
# od end_node do start_node
|
|
path = []
|
|
current_node = end_node
|
|
|
|
while current_node != start_node:
|
|
path.append(current_node)
|
|
current_node = current_node.parent
|
|
path.reverse()
|
|
return path
|
|
|
|
def draw_path(self, window): # rysuję ścieżkę na ekranie
|
|
color = (213, 55, 221)
|
|
for node in self.path:
|
|
node_x = node.state.x
|
|
node_y = node.state.y
|
|
block = pygame.Rect(
|
|
node_x * G_var().RECT_SIZE, node_y *
|
|
G_var().RECT_SIZE, G_var().RECT_SIZE, G_var().RECT_SIZE
|
|
)
|
|
pygame.draw.rect(window,
|
|
color,
|
|
block) |