Dodanie funkcji h(start, goal)

agent.py

@@ -2,6 +2,7 @@ from warehouse import Coordinates, Tile, Pack
 from attributes import PackStatus, TURN_LEFT_DIRECTIONS, TURN_RIGHT_DIRECTIONS
 from queue import PriorityQueue
 from pathfinder import Node
+from math import sqrt
 
 class Agent:
     def __init__(self, start_x, start_y, assigned_warehouse, radius=5):
@@ -98,48 +99,53 @@ class Agent:
         self.transported_package = pack
 
     def find_route(self, goal):
-         explored = []
-         # tu trzeba dodać kolejkę priorytetową fringe
-         fringe = PriorityQueue()
-         current_tile = self.assigned_warehouse.tiles[self.x][self.y]
-         fringe.put((1, current_tile))
+        explored = []
+        # tu trzeba dodać kolejkę priorytetową fringe
+        fringe = PriorityQueue()
+        current_tile = self.assigned_warehouse.tiles[self.x][self.y]
+        fringe.put((1, current_tile))
 
-         while True:
-             if len(fringe) == 0:
-                 return False
-             # elem to węzeł z kolejki o najmniejszym priorytecie (najlepszy)
-             elem = fringe.get()
+        while True:
+            if len(fringe) == 0:
+                return False
+            # elem to węzeł z kolejki o najmniejszym priorytecie (najlepszy)
+            elem = fringe.get()
 
-             # jeśli zgadzają się współrzędne x i y wybranego węzłą oraz celu
-             if elem.x_position == goal.x_position and elem.y_position == goal.y_position:
-                 # tu powinna zostać zwrócona ścieżka
-                 print("doszedl")
-                 return True
+            # jeśli zgadzają się współrzędne x i y wybranego węzłą oraz celu
+            if elem.x_position == goal.x_position and elem.y_position == goal.y_position:
+                # tu powinna zostać zwrócona ścieżka
+                print("doszedl")
+                return True
 
              # przeniesienie wybranego węzłą z fringe do explored
-             explored.append(elem)
+            explored.append(elem)
 
              # dla każdego następnika węzła elem
              # trzeba napisać funkcję succ wyznaczającą następników elem
              # succesor[0] - akcja
              # succesor[1] - stan czyli współrzędne i zwrot
              # succesor[2] - rodzic
-             succ = self.get_succ(elem)
+            succ = self.get_succ(elem)
 
-             for action, state in succ:
-                 new_node = Node(elem, elem.x_position, elem.y_position)
-                 # x[1] = succesor[1]
-                 # x[2] = elem
-                 # x[0] = akcja
-                 # trzeba napisać funkcję f(x) liczącą priorytet danego węzła
-                 # p = f(x)
+            for action, state in succ:
+                new_node = Node(elem, elem.x_position, elem.y_position)
+                # x[1] = succesor[1]
+                # x[2] = elem
+                # x[0] = akcja
+                # trzeba napisać funkcję f(x) liczącą priorytet danego węzła
+                # p = f(x)
 
-                 # if x[1] not in fringe and x[1] not in explored:
-                 #     fringe.append(x[1])
-                 # nie wiem jak wyznaczyć priorytet który x już ma w kolejce
-                 # elif x[1] in fringe and priorytet który stan x już ma w kolejce > p:
-             # tu chodzi o to że zmieni się akcja i rodzić, ale stan nie
-             # zamień w fringe stary węzeł o stanie x[1] z węzłem x
+                # if x[1] not in fringe and x[1] not in explored:
+                #     fringe.append(x[1])
+                # nie wiem jak wyznaczyć priorytet który x już ma w kolejce
+                # elif x[1] in fringe and priorytet który stan x już ma w kolejce > p:
+            # tu chodzi o to że zmieni się akcja i rodzić, ale stan nie
+            # zamień w fringe stary węzeł o stanie x[1] z węzłem x
+
+    def h(self, start, goal):
+        diff_x = pow(goal.x_position - start.x_position,2)
+        diff_y = pow(goal.y_position - start.y_position, 2)
+        return round(sqrt(diff_x+diff_y), 3)
 
     def get_succ(self, elem):
         move = self.move