Dodanie wyświetlania trasy od wózka do paczki za pomocą astar

Environment.py

@@ -10,6 +10,8 @@ from Truck import Truck
 from Global_variables import Global_variables as G_var
 from pygame.constants import *
 
+from astar import Pathfinding, State
+
 
 class Environment:
     def __init__(self, window):
@@ -25,13 +27,30 @@ class Environment:
         self.truck = new_truck
         self.moving_truck = Moving_truck(
             self.window, self.enviroment_2d, self.truck)
+        self.astar = Pathfinding(self.enviroment_2d)
 
     def draw_all_elements(self):
         for row in self.enviroment_2d:
             for field in row:
                 field.draw()
         self.grid.draw_grid()
+        self.use_astar() # w przyszlosci trzeba przeniesc funkcje w jakies logiczniejsze miejsce np funkcje update()
+        self.astar.draw_path(self.window)
         pygame.display.flip()
+    
+    def use_astar(self):
+        start_state = State(1,self.truck.x,self.truck.y)
+        package = self.find_packate()
+        end_state = State(1,package.x, package.y)
+        self.astar.find_path(start_state,end_state)
+    
+    def find_packate(self):   #ta funkcja została zrobiona na szybko, może nie być potrzebna w przyszłości kiedy
+        #ktoś wpadnie na lepsze rozwiązanie
+        for row in self.enviroment_2d:
+            for field in row:
+                if isinstance(field,Package):
+                    return field
+        return None
 
     def update_truck(self, event):
         if event.type == KEYDOWN:

astar.py

@@ -1,8 +1,15 @@
+from ast import walk
+import math
+
+import pygame
+from Global_variables import Global_variables as G_var
+from Shelf import Shelf
+
 
 class State:
 
     def __init__(self, direction, x, y):
-        self.direction = direction # kierunek w ktorym "patrzy wozek"
+        self.direction = direction  # kierunek w ktorym "patrzy wozek"
         self.x = x
         self.y = y
 
@@ -23,11 +30,13 @@ class State:
 
 
 class Node:
-    def __init__(self, action, state, parent):
+    def __init__(self, state, walkable):
         self.state = state
-        self.action = action  # akcja jaką ma wykonać (jedz prawo, lewo, przod)
         self.direction = state.direction
-        self.parent = parent  # ojciec wierzchołka
+        self.walkable = walkable
+        self.g_cost = 0
+        self.h_cost = 0
+        self.parent = None
 
     def get_action(self):
         return self.action
@@ -38,6 +47,110 @@ class Node:
     def get_parent(self):
         return self.parent
 
+    def f_cost(self):
+        if self.walkable:
+            return self.g_cost + self.h_cost
+        else:
+            return math.inf
+
+
+class Pathfinding:
+    def __init__(self, enviroment_2d):
+        # self.grid = []
+        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
+                if isinstance(enviroment_2d[x][y], Shelf):
+                    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
+        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
+                
+
+    def find_path(self, starting_state, target_state):   # algorytm wyszukiwania trasy
+        start_node = self.grid[starting_state.x][starting_state.y]
+        target_node = self.grid[target_state.x][target_state.y]
+
+        fringe = []
+        explored = []
+
+        fringe.append(start_node)
+
+        while len(fringe) > 0:
+            current_node = fringe[0]
+            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)
+            explored.append(current_node)
+
+            if current_node.state == target_node.state:
+                path = self.retrace_path(start_node,target_node)
+                self.path = path
+            
+            for neighbour in self.succ(current_node):
+                if not neighbour.walkable or 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:
+                    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:
+                        fringe.append(neighbour)
+    
+    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)
+
+        if dist_x > dist_y:
+            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
+        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)
+
+        
+
 
 def cost(node):  # funkcja kosztu : ile kosztuje przejechanie przez dane pole
     cost = 0
@@ -45,6 +158,7 @@ def cost(node):  # funkcja kosztu : ile kosztuje przejechanie przez dane pole
         cost = cost + 1 + 1
         node = node.parent
     return cost
+#
 
 
 def f(goal, node):  # funkcja zwracająca sumę funkcji kosztu oraz heurestyki
@@ -67,6 +181,6 @@ def print_moves(elem):  # zwraca listę ruchów jakie należy wykonać by dotrze
 def succ(elem):  # funkcja następnika, przypisuje jakie akcje są możliwe do wykonania na danym polu oraz jaki będzie stan (kierunek, położenie) po wykonaniu tej akcji
     pass
 
+
 def graphsearch(explored, fringe, goaltest, istate):  # przeszukiwanie grafu wszerz
     pass
-