Implement A* tile cost

main.py

@@ -19,11 +19,12 @@ def main():
     pygame.display.set_caption('TRAKTOHOLIK')  # nazwa okna
 
     start_cords = (8, 1)
-    goals = [(3, 3), (7, 7), (0, 0)]
+    goals = [(8, 7), (7, 7), (0, 0)]
     end_cords = goals[0]
     start_dir = tractor.curr_direction  # przypisanie początkowego ustawienia traktora do zmiennej
     # path = BFSSearcher().search(start_cords, end_cords, start_dir)  # wygenerowanie listy ruchów na bazie BFS
-    path = a_star_search(start_cords, end_cords, start_dir)  # generowanie ścieżki za pomocą A*
+    path = a_star_search(start_cords, end_cords, start_dir, world)  # generowanie ścieżki za pomocą A*
+
 
     run = True
     while run:
@@ -46,7 +47,7 @@ def main():
                 end_cords = goals[0]  # ustaw kolejny cel
                 start_dir = tractor.curr_direction  # aktualizacja kierunku traktora
                 # path = BFSSearcher().search(start_cord, end_cords, start_dir)  # generacja nowej ścieżki
-                path = a_star_search(new_start, end_cords, start_dir)
+                path = a_star_search(new_start, end_cords, start_dir, world)
 
         pygame.time.wait(settings.freeze_time)
         pygame.display.update()

src/utils/astar.py

@@ -1,12 +1,11 @@
 from operator import itemgetter
 import copy
 import tractor
+from world import World
 
-import world
 from constants import Constants as C
 
 
-# noinspection DuplicatedCode
 class Node:
     def __init__(self, x, y, agent_direction, action=None, parent=None):  # inicjalizacja węzła
         self.x = x
@@ -65,16 +64,16 @@ def h_cost(goal: tuple, node: Node):  # oblicznie heurystyki
     return abs(node.x - goal[0]) + abs(node.y - goal[1])
 
 
-def g_cost(node: Node):  # funkcja kosztu : ile kosztuje przejechanie przez dane pole
+def g_cost(node: Node, world_map: World):  # funkcja kosztu : ile kosztuje przejechanie przez dane pole
     cost = 0
     while node.get_parent() is not None:
-        cost += 10
+        cost += world_map.get_tile_cost(node.x, node.y) + 1
         node = node.get_parent()
     return cost
 
 
-def f_cost(goal: tuple, node: Node):  # funkcja zwracająca sumę funkcji kosztu oraz heurestyki
+def f_cost(goal: tuple, node: Node, world_map: World):  # funkcja zwracająca sumę funkcji kosztu oraz heurestyki
-    return g_cost(node) + h_cost(goal, node)
+    return g_cost(node, world_map) + h_cost(goal, node)
 
 
 def print_moves(node: Node):  # zwraca listę ruchów jakie należy wykonać by dotrzeć do punktu docelowego
@@ -89,7 +88,7 @@ def is_goal_reached(element: Node, goal: tuple):
     return (element.x, element.y) == goal
 
 
-def a_star_search(state: tuple, goal: tuple, agent_direction: list):
+def a_star_search(state: tuple, goal: tuple, agent_direction: list, world_map: World):
     fringe = []
     explored = []
 
@@ -121,12 +120,12 @@ def a_star_search(state: tuple, goal: tuple, agent_direction: list):
                 explored_tuple.append(((node.get_x(), node.get_y()), node.get_direction()))
 
             new_node = Node(elem_state[0][0], elem_state[0][1], elem_state[1], action, element[0])
-            p = f_cost(goal, new_node)
+            p = f_cost(goal, new_node, world_map)
 
             if elem_state not in fringe_tuple and elem_state not in explored_tuple:
                 fringe.append((new_node, p))
                 fringe = sorted(fringe, key=itemgetter(1))
-                # print("fringe_loop_if: ", fringe)
+
             elif elem_state in fringe_tuple:
                 i = 0
                 for (state_prio, r) in fringe_tuple_prio:
@@ -135,6 +134,5 @@ def a_star_search(state: tuple, goal: tuple, agent_direction: list):
                             fringe.insert(i, (new_node, p))
                             fringe.pop(i + 1)
                             fringe = sorted(fringe, key=itemgetter(1))
-                            # print("fringe_loop_elif: ", fringe)
                             break
                     i += 1

src/world.py

@@ -9,8 +9,8 @@ class World:
         self.world_data = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
-                           [1, 1, 1, 1, 1, 0, 0, 0, 0, 1],
+                           [1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
-                           [1, 1, 1, 1, 1, 0, 0, 0, 0, 1],
+                           [1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                            [1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
@@ -30,15 +30,15 @@ class World:
                 if tile == 1:
                     img = pygame.transform.scale(self.dirt_image, (self.settings.tile_size, self.settings.tile_size))
                     type = 'dirt'
-                    cost = 0
+                    cost = 1
                 elif tile == 0:
                     img = pygame.transform.scale(self.rock_image, (self.settings.tile_size, self.settings.tile_size))
                     type = 'rock'
-                    cost = 100
+                    cost = 1000
                 img_rect = img.get_rect()
                 img_rect.x = col_count * self.settings.tile_size
                 img_rect.y = row_count * self.settings.tile_size
-                tile = Tile(type, len(self.world_data) - row_count - 1, col_count, img, img_rect, cost)
+                tile = Tile(type, col_count, len(self.world_data) - row_count - 1, img, img_rect, cost)
                 self.tiles.add(tile)
                 col_count += 1
             row_count += 1
@@ -57,3 +57,5 @@ class World:
         for tile in self.tiles:
             if tile.position == (x, y):
                 return tile.cost
+
+