diff --git a/Astar.py b/Astar.py index 4b494cd..9ed6b09 100644 --- a/Astar.py +++ b/Astar.py @@ -11,7 +11,11 @@ from Shelf import Shelf class Pathfinding: def __init__(self, enviroment_2d): - # self.grid = [] + 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)] @@ -20,12 +24,13 @@ class Pathfinding: for x in range(G_var().DIMENSION_X): # zapełnianie tablicy obiektami Node for y in range(G_var().DIMENSION_Y): is_walkable = True - if isinstance(enviroment_2d[x][y], Shelf): + 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) - self.path = [] - def succ(self,node): #funckja zwraca sąsiadów noda w argumencie node_x = node.state.x node_y = node.state.y @@ -39,33 +44,73 @@ class Pathfinding: return neighbours - def find_path(self, starting_state, target_state): # algorytm wyszukiwania trasy +################## 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 = [] fringe = Min_heap() explored = [] - # explored = set() is_target_node_walkable = True if not target_node.walkable: target_node.walkable = True is_target_node_walkable = False - # fringe.append(start_node) fringe.insert(start_node) - # while len(fringe) > 0: while fringe.count() > 0: # current_node = fringe[0] current_node = fringe.extract() - # for i in range(1, len(fringe)): - # if fringe[i].f_cost() < current_node.f_cost() or (fringe[i].f_cost() == current_node.f_cost() and fringe[i].h_cost < current_node.h_cost): - # current_node = fringe[i] - - # fringe.remove(current_node) + #################################################### 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) - # explored.add(current_node) if current_node.state == target_node.state: path = self.retrace_path(start_node,target_node) @@ -77,15 +122,11 @@ class Pathfinding: neighbour_in_explored = [e for e in explored if e.state == neighbour.state] if not neighbour.walkable or len(neighbour_in_explored) > 0: continue - # if neighbour in explored: - # 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 neighbour in fringe: 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 neighbour in fringe: if not fringe.contains(neighbour): fringe.insert(neighbour) target_node.walkable = is_target_node_walkable @@ -94,9 +135,7 @@ class Pathfinding: dist_x = abs(node_a.state.x - node_b.state.x) dist_y = abs(node_a.state.y - node_b.state.y) - if dist_x > dist_y: - return 10 * (dist_x - dist_y) - return 10 * (dist_y - dist_x) + 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 diff --git a/Environment.py b/Environment.py index 1cd1c4d..9fbb584 100644 --- a/Environment.py +++ b/Environment.py @@ -28,9 +28,9 @@ class Environment: new_truck = Truck(window, 14, 7) self.enviroment_2d[14][7] = new_truck self.truck = new_truck - self.moving_truck = Moving_truck( - self.window, self.enviroment_2d, self.truck, self.package_spawner) self.astar = Pathfinding(self.enviroment_2d) + self.moving_truck = Moving_truck( + self.window, self.enviroment_2d, self.truck, self.package_spawner, self.astar) self.finding_fields = Finding_fields(self.enviroment_2d) def draw_all_elements(self): @@ -38,13 +38,13 @@ class Environment: for field in row: field.draw() self.grid.draw_grid() - self.astar.draw_path(self.window) + # self.astar.draw_path(self.window) pygame.display.flip() def update_all_elements(self): self.use_astar() # wywyoływanie za każdym razem astar jest bardzo zasobożerne. Lepiej raz na przejście self.update_truck() - time.sleep(0.5) + # time.sleep(0.5) def use_astar(self): @@ -54,7 +54,7 @@ class Environment: else: end_position = self.finding_fields.find_package() end_state = State(1,end_position.x, end_position.y) - self.astar.find_path(start_state,end_state) + self.astar.find_path(start_state,end_state,self.window) def update_truck(self): next_field_to_move = self.astar.path[0].state diff --git a/Global_variables.py b/Global_variables.py index 9129f8e..a27df53 100644 --- a/Global_variables.py +++ b/Global_variables.py @@ -10,11 +10,6 @@ class Global_variables(object): RECT_COLOR = (70, 77, 87) SHELF_COLOR = (143, 68, 33) - def __init__(self) -> None: - dim_x = 28 - dim_y = 15 - self.GRID = [["empty" for i in range(dim_x)] for j in range(dim_y)] - def __new__(cls): if cls._instance is None: cls._instance = super(Global_variables, cls).__new__(cls) diff --git a/Min_heap.py b/Min_heap.py index cb22778..b69bbbf 100644 --- a/Min_heap.py +++ b/Min_heap.py @@ -39,8 +39,6 @@ class Min_heap: print("STOS PUSTY!") return min = self.items[0] - # self.items[0] = self.items[last_item_index] - # self.items[0] = self.items.pop() self.items[0] = self.items[len(self.items) - 1] self.items.pop() self.heapify(0) @@ -64,7 +62,6 @@ class Min_heap: temp_node.h_cost = math.inf temp_node.heap_index = len(self.items) - 1 self.items.append(temp_node) - # item.heap_index = len(self.items) - 1 self.decrese_key(len(self.items) - 1, item) def count(self): @@ -74,5 +71,5 @@ class Min_heap: in_range = len(self.items) > item.heap_index contains = False if in_range: - contains = self.items[item.heap_index] == item + contains = self.items[item.heap_index] is item return in_range and contains \ No newline at end of file diff --git a/Moving_truck.py b/Moving_truck.py index 87c6172..d87d508 100644 --- a/Moving_truck.py +++ b/Moving_truck.py @@ -2,13 +2,14 @@ from Empty import Empty from Package import Package from Shelf import Shelf - +# TODO: DODAC OBSERWER ZAMIAST PRZEKAZYWANIE WSZYSTKICH BZDET class Moving_truck: - def __init__(self, window, enviroment_2d, truck, package_spawner): + def __init__(self, window, enviroment_2d, truck, package_spawner, a_star): self.enviroment_2d = enviroment_2d self.truck = truck self.window = window self.package_spawner = package_spawner + self.astar = a_star def move(self, x, y): truck_x = self.truck.x @@ -29,6 +30,7 @@ class Moving_truck: package = self.enviroment_2d[package_x][package_y] self.truck.has_package = True self.truck.package_type = package.type + self.astar.reset_grid() self.move_without_swapping(truck_x, truck_y, package_x, package_y) @@ -41,6 +43,7 @@ class Moving_truck: y].is_placed = True self.truck.has_package = False self.package_spawner.spawn_package() + self.astar.reset_grid() def swap_fields(self, x1, y1, x2, y2): self.enviroment_2d[x1][y1], self.enviroment_2d[x2][y2] = self.enviroment_2d[x2][y2], self.enviroment_2d[x1][y1] diff --git a/Program.py b/Program.py index 9ee68bc..2ef8674 100644 --- a/Program.py +++ b/Program.py @@ -17,5 +17,5 @@ class Program: for event in pygame.event.get(): # integrating with keyboard if event.type == QUIT: running = False - self.environment.update_all_elements() - self.environment.draw_all_elements() + self.environment.update_all_elements() + self.environment.draw_all_elements()