Fixing astar and BIG performance improvments

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)):
+            #################################################### TEST
-            #     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_color = (213, 55, 221)
-            #         current_node = fringe[i]
+            # to_check_color = (55, 213, 55)
-            
+            # current_color = (233,55,55)
-            # fringe.remove(current_node)
+            # # 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 (dist_x + dist_y) * 10
-            return 10 * (dist_x - dist_y)
-        return 10 * (dist_y - dist_x)
     
     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

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

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)

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

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]

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.update_all_elements()
-                self.environment.draw_all_elements()
+            self.environment.draw_all_elements()