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)