2nd part AStar implementation

main.py

@@ -14,6 +14,7 @@ if __name__ == "__main__":
     # init functions
     graphics.drawBackground(waiter.matrix)
     graphics.update(waiter.X, waiter.Y)
+    print(waiter.findPath((1, 10)))
 
     while True:
         for event in pygame.event.get():
@@ -29,6 +30,7 @@ if __name__ == "__main__":
                     break
 
             graphics.clear(waiter.X, waiter.Y)
+
             waiter.update(event, graphics)
             graphics.update(waiter.X, waiter.Y)
         pygame.display.flip()

src/tile.py

@@ -18,9 +18,9 @@ class Tile:
         # Atrybuty niezbedne dla A*
         self.position = (x, y)
         self.parent = None
-        self.totalCost = 0  # Koszt totalny czyli dystans do wierzcholka startowego + heurystyka
+        self.totalCost = 0  # Koszt totalny czyli dystans do wierzcholka startowego + heurystyka F
-        self.startCost = 0  # Dystans do wierzcholka startowego
+        self.startCost = 0  # Dystans do wierzcholka startowego G
-        # Dystant do wierzcholka koncowego oszacowany za pomoca funkcji heurystyki
+        # Dystant do wierzcholka koncowego oszacowany za pomoca funkcji heurystyki H
         self.heuristic = 0
 
     # Operator porownywania pol

src/waiter.py

@@ -1,6 +1,7 @@
 import pygame
 
 from .matrix import Matrix
+from .tile import Tile
 
 
 # WAITER
@@ -12,6 +13,7 @@ class Waiter(pygame.sprite.Sprite):
         self.frame = 0
         self.matrix = Matrix(graphics=graphics)
         self.direction = 'E'
+        self.tile = Tile(self, self.X, self.Y)
 
     # Borders
     def move(self, x, y, graphics):
@@ -53,61 +55,98 @@ class Waiter(pygame.sprite.Sprite):
 
     # AStar
     def findPath(self, goal):
-        #Stworzenie startowego i koncowego wierzcholka
+        # Stworzenie startowego i koncowego wierzcholka
         startNode = self.matrix.matrix[self.X][self.Y]
         goalNode = self.matrix.matrix[goal[0]][goal[1]]
 
-        #Inicjalizacja list
+        # Inicjalizacja list
         openList = []
         closedList = []
 
         openList.append(startNode)
 
         while len(openList) > 0:
-
+            openList.sort(key=getTotalCost)
-            openList.sort(key=tile.totalCost)
             currentNode = openList.pop(0)
             closedList.append(currentNode)
 
+            print("\nOpenList")
+            for tile in openList:
+                print(tile.position, end=" ")
+
+            print("\nClosed List")
+            for tile in closedList:
+                print(tile.position, end=' ')
+
+            # Tutaj odbywac sie bedzie budowanie sciezki gdy algorytm osiagnie cel
             if currentNode == goalNode:
-                pass
+                path = []
-                #Tutaj odbywac sie bedzie budowanie sciezki gdy algorytm osiagnie cel
+                current = currentNode
+                while current is not None:
+                    path.append(current.position)
+                    current = current.parent
+                return path[::-1]
 
             children = []
-            if currentNode.X >= 0:
+            if currentNode.position[0] >= 0:
-                if currentNode.Y + 1 < len(self.matrix.matrix[0]):
+                if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
-                    if self.matrix.matrix[currentNode.X][currentNode.Y + 1].walk_through == 1:
+                    if self.matrix.matrix[currentNode.position[0]][currentNode.position[1] + 1].walk_through == 1:
-                        children.append(self.matrix.matrix[currentNode.X][currentNode.Y + 1])
+                        children.append(
-                if currentNode.Y - 1 >= 0:
+                            self.matrix.matrix[currentNode.position[0]][currentNode.position[1] + 1])
-                    if self.matrix.matrix[currentNode.X][currentNode.Y - 1].walk_through == 1:
+                if currentNode.position[1] - 1 >= 0:
-                        children.append(self.matrix.matrix[currentNode.X][currentNode.Y - 1])
+                    if self.matrix.matrix[currentNode.position[0]][currentNode.position[1] - 1].walk_through == 1:
+                        children.append(
+                            self.matrix.matrix[currentNode.position[0]][currentNode.position[1] - 1])
 
-            if currentNode.X + 1 < len(self.matrix.matrix):
+            if currentNode.position[0] + 1 < len(self.matrix.matrix):
-                if currentNode.Y + 1 < len(self.matrix.matrix[0]):
+                if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
-                    if self.matrix.matrix[currentNode.X + 1][currentNode.Y + 1].walk_through == 1:
+                    if self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] + 1].walk_through == 1:
-                        children.append(self.matrix.matrix[currentNode.X + 1][currentNode.Y + 1])
+                        children.append(
-                if currentNode.Y - 1 >= 0:
+                            self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] + 1])
-                    if self.matrix.matrix[currentNode.X + 1][currentNode.Y - 1].walk_through == 1:
+                if currentNode.position[1] - 1 >= 0:
-                        children.append(self.matrix.matrix[currentNode.X + 1][currentNode.Y - 1])
+                    if self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] - 1].walk_through == 1:
-                if currentNode.Y >= 0:
+                        children.append(
-                    if self.matrix.matrix[currentNode.X + 1][currentNode.Y].walk_through == 1:
+                            self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] - 1])
-                        children.append(self.matrix.matrix[currentNode.X + 1][currentNode.Y])
+                if currentNode.position[1] >= 0:
+                    if self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1]].walk_through == 1:
+                        children.append(
+                            self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1]])
 
-            if currentNode.X - 1 >= 0:
+            if currentNode.position[0] - 1 >= 0:
-                if currentNode.Y + 1 < len(self.matrix.matrix[0]):
+                if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
-                    if self.matrix.matrix[currentNode.X - 1][currentNode.Y + 1].walk_through == 1:
+                    if self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] + 1].walk_through == 1:
-                        children.append(self.matrix.matrix[currentNode.X - 1][currentNode.Y + 1])
+                        children.append(
-                if currentNode.Y - 1 >= 0:
+                            self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] + 1])
-                    if self.matrix.matrix[currentNode.X - 1][currentNode.Y - 1].walk_through == 1:
+                if currentNode.position[1] - 1 >= 0:
-                        children.append(self.matrix.matrix[currentNode.X - 1][currentNode.Y - 1])
+                    if self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] - 1].walk_through == 1:
-                if currentNode.Y >= 0:
+                        children.append(
-                    if self.matrix.matrix[currentNode.X - 1][currentNode.Y].walk_through == 1:
+                            self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] - 1])
-                        children.append(self.matrix.matrix[currentNode.X - 1][currentNode.Y])
+                if currentNode.position[1] >= 0:
+                    if self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1]].walk_through == 1:
+                        children.append(
+                            self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1]])
 
             for child in children:
 
-                if child in closedList:
+                #child.parent = currentNode
-                    continue
+
-
+                for closedChild in closedList:
+                    if child == closedChild:
+                        continue
+
+                child.startCost = currentNode.startCost + 1
+                child.heuristic = ((child.position[0] - goalNode.position[0]) ** 2) + (
+                    (child.position[1] - goalNode.position[1]) ** 2)
+                child.totalCost = child.startCost + child.heuristic
+
+                for openNode in openList:
+
+                    if child == openNode and child.startCost >= openNode.startCost:
+                        continue
+
+                #child.parent = currentNode
+                openList.append(child)
 
 
+def getTotalCost(tile):
+    return tile.totalCost