Add A* algorithm without implemeting it to agent

.idea/ai-project.iml

@@ -5,7 +5,7 @@
       <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
       <excludeFolder url="file://$MODULE_DIR$/venv" />
     </content>
-    <orderEntry type="inheritedJdk" />
+    <orderEntry type="jdk" jdkName="Python 3.10" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
 </module>

.idea/misc.xml

@@ -1,4 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9 (ai-project)" project-jdk-type="Python SDK" />
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10" project-jdk-type="Python SDK" />
 </project>

main.py

@@ -5,14 +5,15 @@ from src.tractor import Tractor
 from src.settings import Settings
 from src.utils.bfs import BFSSearcher
 from src.constants import Constants
-
+from utils.astar import a_star
+import numpy as np
 
 def main():
     pygame.init()
 
     settings = Settings()
     world = World(settings)
-    tractor = Tractor("Spalinowy", "Nawóz 1", settings, 8*settings.tile_size, 8*settings.tile_size)
+    tractor = Tractor("Spalinowy", "Nawóz 1", settings, 8 * settings.tile_size, 8 * settings.tile_size)
     obstacles = [tile for tile in world.tiles if tile.type == 'rock']
     clock = pygame.time.Clock()  # FPS purpose
 
@@ -54,4 +55,29 @@ def main():
 
 
 if __name__ == '__main__':
-    main()
+
+    # inicjalizacja array z zerami
+    rows = 10
+    cols = 10
+    field = np.zeros((rows, cols), dtype=int)
+
+    # tworzenie ścian w array
+    for i in range(0, 9):
+        field[1, i] = 1
+
+    field[2, 8] = 1
+    field[3, 8] = 1
+    field[3, 7] = 1
+    field[3, 6] = 1
+
+    print(field)
+
+    start = (0, 0)
+    goals = [(2, 7)]
+    while goals:
+        goal = goals.pop(0)
+        path = a_star(field, start, goal)
+        print(path)
+
+
+    # main()

src/utils/astar.py

@@ -0,0 +1,93 @@
+class Node:
+    def __init__(self, x, y):  # inicjalizacja węzła
+        self.x = x
+        self.y = y
+        self.neighbours = []
+        self.parent = None
+        self.g_cost = 0
+        self.f_cost = 0
+        self.h_cost = 0
+
+    def create_neighbours(self,
+                          world_map):  # tworzenie sąsiadów i sprawdzanie czy nie wykraczają poza macierz i czy nie są przeszkodą
+        if self.x - 1 >= 0 and world_map[self.x - 1, self.y] != 1:
+            self.neighbours.append([self.x - 1, self.y])
+
+        if self.x + 1 < world_map.shape[0] and world_map[self.x + 1, self.y] != 1:
+            self.neighbours.append([self.x + 1, self.y])
+
+        if self.y + 1 < world_map.shape[1] and world_map[self.x, self.y + 1] != 1:
+            self.neighbours.append([self.x, self.y + 1])
+
+        if self.y - 1 >= 0 and world_map[self.x, self.y - 1] != 1:
+            self.neighbours.append([self.x, self.y - 1])
+
+    def calculate_h_cost(self, goal):
+        self.h_cost = abs(self.x - goal.x) + abs(self.y - goal.y)
+
+
+def a_star(world_map, start, goal):  # algorytm zwraca ścieżkę którą ma przebyć agent
+
+    # tworzenie dictionary składającego się z nodeów
+    node_dict = {}
+
+    for i in range(len(world_map)):
+        for j in range(len(world_map[0])):
+            node_dict[i, j] = Node(i, j)
+
+    # przypisanie startu i celu do zmiennej
+    start = node_dict[start[0], start[1]]
+    goal = node_dict[goal[0], goal[1]]
+
+    # inicjalizacja listy open i closed
+    open_list = []
+    closed_list = []
+
+    # dodanie do listy punktu startowego
+    open_list.append(start)
+
+    while open_list:
+        # sortowanie listy open rosnąco
+        open_list.sort(key=lambda o: o.f_cost)
+
+        # pobranie z listy open elementu o najniższym koszcie
+        current = open_list.pop(0)
+
+        # dodanie do listy closed pobranego elementu
+        closed_list.append(current)
+
+        # stworzenie listy z możliwymi opcjami
+        current.create_neighbours(world_map)
+
+        # sprawdzenie celu
+        if current == goal:
+            break
+
+        # iteracja przez sąsiadów
+        for next in current.neighbours:
+            if node_dict[next[0], next[1]] in closed_list:
+                continue
+
+            if node_dict[next[0], next[1]] in open_list:
+                new_g_cost = current.g_cost + 1
+                if node_dict[next[0], next[1]].g_cost > new_g_cost:
+                    node_dict[next[0], next[1]].g_cost = new_g_cost
+                    node_dict[next[0], next[1]].parent = node_dict[current.x, current.y]
+
+            else:
+                node_dict[next[0], next[1]].g_cost = node_dict[current.x, current.y].g_cost + 1
+                node_dict[next[0], next[1]].calculate_h_cost(goal)
+                node_dict[next[0], next[1]].parent = node_dict[current.x, current.y]
+                open_list.append(node_dict[next[0], next[1]])
+
+    # inicjalizacja listy ze ścieżką
+    path = []
+    while current.parent:
+        path.append([current.x, current.y])
+        current = current.parent
+
+    # dodanie punktu początkowego oraz odwrócenie i zwrócenie listy
+    path.append([start.x, start.y])
+    path.reverse()
+
+    return path