3rd part AStar implementation

main.py

@@ -1,4 +1,5 @@
 import sys
+import secrets
 
 from src.graphics import *
 from src.waiter import *
@@ -7,15 +8,17 @@ if __name__ == "__main__":
     # SETUP
     pygame.init()
     clock = pygame.time.Clock()
-    fps = 40
+    fps = 2
     graphics = Graphics()
     waiter = Waiter(graphics)
 
     # init functions
     graphics.drawBackground(waiter.matrix)
     graphics.update(waiter.X, waiter.Y)
-    print(waiter.findPath((1, 10)))
 
+    goal = None
+    path = [(0, 0)]
+    # print(waiter.findPath())
     while True:
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
@@ -29,9 +32,26 @@ if __name__ == "__main__":
                     sys.exit()
                     break
 
-            graphics.clear(waiter.X, waiter.Y)
+                if event.key == pygame.K_r:
+                    temp = False
+                    while not temp:
+                        x = secrets.randbelow(graphics.width)
+                        y = secrets.randbelow(graphics.height)
+
+                        if waiter.matrix.matrix[x][y].walk_through == 1:
+                            temp = True
+
+                    goal = (x, y)
+                    path = waiter.findPath(goal)
+
+        if path != []:
+            temp = path.pop(0)
+            print(temp)
+            waiter.travel(temp, graphics)
+
+            #graphics.clear(waiter.X, waiter.Y)
+            #waiter.update(event, graphics)
+            #graphics.update(waiter.X, waiter.Y)
 
-            waiter.update(event, graphics)
-            graphics.update(waiter.X, waiter.Y)
         pygame.display.flip()
         clock.tick(fps)

src/tile.py

@@ -6,7 +6,7 @@ class Tile:
         if self.type == 'wall':
             self.watch_through = 0
             self.walk_through = 0
-        elif self.type == 'table' or self.type == 'bar' or self.type == 'chair':
+        elif self.type == 'table' or self.type == 'bar' or self.type == 'chair_front' or self.type == 'chair_back' or self.type == 'chair_left' or self.type == 'chair_right':
             self.watch_through = 1
             self.walk_through = 0
         else:

src/waiter.py

@@ -22,39 +22,39 @@ class Waiter(pygame.sprite.Sprite):
                 self.X += x
                 self.Y += y
 
-    def update(self, event, graphics):
+    def update(self, instruction, graphics):
-        if event.type == pygame.KEYDOWN:
+        if instruction == 'L':
-            if event.key == pygame.K_RIGHT:
+            if self.direction == 'N':
-                if self.direction == 'N':
+                self.direction = 'W'
-                    self.direction = 'W'
+            elif self.direction == 'S':
-                elif self.direction == 'S':
+                self.direction = 'E'
-                    self.direction = 'E'
+            elif self.direction == 'E':
-                elif self.direction == 'E':
+                self.direction = 'N'
-                    self.direction = 'N'
+            elif self.direction == 'W':
-                elif self.direction == 'W':
+                self.direction = 'S'
-                    self.direction = 'S'
+        if instruction == 'R':
-            if event.key == pygame.K_LEFT:
+            if self.direction == 'N':
-                if self.direction == 'N':
+                self.direction = 'E'
-                    self.direction = 'E'
+            elif self.direction == 'S':
-                elif self.direction == 'S':
+                self.direction = 'W'
-                    self.direction = 'W'
+            elif self.direction == 'E':
-                elif self.direction == 'E':
+                self.direction = 'S'
-                    self.direction = 'S'
+            elif self.direction == 'W':
-                elif self.direction == 'W':
+                self.direction = 'N'
-                    self.direction = 'N'
+        if instruction == 'F':
-            if event.key == pygame.K_UP:
+            if self.direction == 'N':
-                if self.direction == 'N':
+                self.move(0, -1, graphics)
-                    self.move(0, 1, graphics)
+            if self.direction == 'S':
-                if self.direction == 'S':
+                self.move(0, 1, graphics)
-                    self.move(0, -1, graphics)
+            if self.direction == 'E':
-                if self.direction == 'E':
+                self.move(1, 0, graphics)
-                    self.move(1, 0, graphics)
+            if self.direction == 'W':
-                if self.direction == 'W':
+                self.move(-1, 0, graphics)
-                    self.move(-1, 0, graphics)
+        #print(self.X, self.Y)
-            print(self.X, self.Y)
 
     # AStar
     def findPath(self, goal):
+
         # Stworzenie startowego i koncowego wierzcholka
         startNode = self.matrix.matrix[self.X][self.Y]
         goalNode = self.matrix.matrix[goal[0]][goal[1]]
@@ -67,17 +67,10 @@ class Waiter(pygame.sprite.Sprite):
 
         while len(openList) > 0:
             openList.sort(key=getTotalCost)
+
             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:
                 path = []
@@ -88,6 +81,7 @@ class Waiter(pygame.sprite.Sprite):
                 return path[::-1]
 
             children = []
+
             if currentNode.position[0] >= 0:
                 if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
                     if self.matrix.matrix[currentNode.position[0]][currentNode.position[1] + 1].walk_through == 1:
@@ -99,54 +93,161 @@ class Waiter(pygame.sprite.Sprite):
                             self.matrix.matrix[currentNode.position[0]][currentNode.position[1] - 1])
 
             if currentNode.position[0] + 1 < len(self.matrix.matrix):
-                if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
-                    if self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] + 1].walk_through == 1:
-                        children.append(
-                            self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] + 1])
-                if currentNode.position[1] - 1 >= 0:
-                    if self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] - 1].walk_through == 1:
-                        children.append(
-                            self.matrix.matrix[currentNode.position[0] + 1][currentNode.position[1] - 1])
                 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.position[0] - 1 >= 0:
-                if currentNode.position[1] + 1 < len(self.matrix.matrix[0]):
-                    if self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] + 1].walk_through == 1:
-                        children.append(
-                            self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] + 1])
-                if currentNode.position[1] - 1 >= 0:
-                    if self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] - 1].walk_through == 1:
-                        children.append(
-                            self.matrix.matrix[currentNode.position[0] - 1][currentNode.position[1] - 1])
                 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]])
 
+            perm = 0
             for child in children:
 
-                #child.parent = currentNode
-
                 for closedChild in closedList:
                     if child == closedChild:
-                        continue
+                        perm = 1
+                        break
 
+                if perm == 1:
+                    perm = 0
+                    continue
+
+                child.parent = currentNode
                 child.startCost = currentNode.startCost + 1
-                child.heuristic = ((child.position[0] - goalNode.position[0]) ** 2) + (
+                child.heuristic = abs(
-                    (child.position[1] - goalNode.position[1]) ** 2)
+                    goal[0] - child.position[0]) + abs(goal[1] - child.position[1])
                 child.totalCost = child.startCost + child.heuristic
 
                 for openNode in openList:
-
+                    if child == openNode and child.startCost > openNode.startCost:
-                    if child == openNode and child.startCost >= openNode.startCost:
+                        perm = 1
                         continue
 
-                #child.parent = currentNode
+                if perm == 1:
+                    perm = 0
+                    continue
+
                 openList.append(child)
 
+    def translatePath(self, path):
+
+        currentPosition = (self.X, self.Y)
+        currentDirection = self.direction
+        output = ''
+
+        for node in path:
+
+            if currentDirection == 'N':
+
+                if currentPosition[0] == node[0]:
+
+                    if currentPosition[1] + 1 == node[1]:
+                        output += 'LLF'
+                        currentDirection = 'S'
+
+                    if currentPosition[1] - 1 == node[1]:
+                        output += 'F'
+
+                elif currentPosition[1] == node[1]:
+
+                    if currentPosition[0] + 1 == node[0]:
+                        output += 'RF'
+                        currentDirection = 'E'
+
+                    if currentPosition[0] - 1 == node[0]:
+                        output += 'LF'
+                        currentDirection = 'W'
+
+            elif currentDirection == 'E':
+
+                if currentPosition[0] == node[0]:
+
+                    if currentPosition[1] + 1 == node[1]:
+                        output += 'RF'
+                        currentDirection = 'S'
+
+                    if currentPosition[1] - 1 == node[1]:
+                        output += 'LF'
+                        currentDirection = 'N'
+
+                elif currentPosition[1] == node[1]:
+
+                    if currentPosition[0] + 1 == node[0]:
+                        output += 'F'
+
+                    if currentPosition[0] - 1 == node[0]:
+                        output += 'LLF'
+                        currentDirection = 'W'
+
+            elif currentDirection == 'S':
+
+                if currentPosition[0] == node[0]:
+
+                    if currentPosition[1] + 1 == node[1]:
+                        output += 'F'
+
+                    if currentPosition[1] - 1 == node[1]:
+                        output += 'LLF'
+                        currentDirection = 'N'
+
+                elif currentPosition[1] == node[1]:
+
+                    if currentPosition[0] + 1 == node[0]:
+                        output += 'LF'
+                        currentDirection = 'E'
+
+                    if currentPosition[0] - 1 == node[0]:
+                        output += 'RF'
+                        currentDirection = 'W'
+
+            elif currentDirection == 'W':
+
+                if currentPosition[0] == node[0]:
+
+                    if currentPosition[1] + 1 == node[1]:
+                        output += 'LF'
+                        currentDirection = 'S'
+
+                    if currentPosition[1] - 1 == node[1]:
+                        output += 'RF'
+                        currentDirection = 'N'
+
+                elif currentPosition[1] == node[1]:
+
+                    if currentPosition[0] + 1 == node[0]:
+                        output += 'LLF'
+                        currentDirection = 'E'
+
+                    if currentPosition[0] - 1 == node[0]:
+                        output += 'F'
+
+            currentPosition = node
+
+        return output
+
+    def travel(self, goal, graphics):
+        translatedPath = self.translatePath([goal])
+        print('{} - {} - {}'.format(self.direction, goal, translatedPath))
+        for character in translatedPath:
+            if character == 'F':
+                graphics.clear(self.X, self.Y)
+                self.update('F', graphics)
+                graphics.update(self.X, self.Y)
+
+            if character == 'R':
+                #graphics.clear(self.X, self.Y)
+                self.update('R', graphics)
+                #graphics.update(self.X, self.Y)
+
+            if character == 'L':
+                #graphics.clear(self.X, self.Y)
+                self.update('L', graphics)
+                #graphics.update(self.X, self.Y)
+
 
 def getTotalCost(tile):
     return tile.totalCost