3rd part AStar implementation

main.py

@@ -1,4 +1,5 @@
 import sys
+import secrets
 
 from src.graphics import *
 from src.waiter import *
@@ -7,7 +8,7 @@ if __name__ == "__main__":
     # SETUP
     pygame.init()
     clock = pygame.time.Clock()
-    fps = 40
+    fps = 2
     graphics = Graphics()
     waiter = Waiter(graphics)
 
@@ -15,6 +16,9 @@ if __name__ == "__main__":
     graphics.drawBackground(waiter.matrix)
     graphics.update(waiter.X, waiter.Y)
 
+    goal = None
+    path = [(0, 0)]
+    # print(waiter.findPath())
     while True:
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
@@ -28,8 +32,26 @@ if __name__ == "__main__":
                     sys.exit()
                     break
 
-            graphics.clear(waiter.X, waiter.Y)
-            waiter.update(event, graphics)
-            graphics.update(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)
+
         pygame.display.flip()
         clock.tick(fps)

src/graphics.py

@@ -4,16 +4,16 @@ import pygame
 class Graphics:
     def __init__(self):
         self.image = {
-            'floor': pygame.image.load('../resources/images/floor.jpg'),
-            'wall': pygame.image.load('../resources/images/wall.png'),  #
-            'bar': pygame.image.load('../resources/images/table3.png'),  #
-            'bar_floor': pygame.image.load('../resources/images/waiter.png'),
-            'table': pygame.image.load('../resources/images/table3.png'),
-            'waiter': pygame.image.load('../resources/images/waiter.png'),
-            'chair_front': pygame.image.load('../resources/images/chair-front.png'),  #
-            'chair_back': pygame.image.load('../resources/images/chair-back.png'),  #
-            'chair_left': pygame.image.load('../resources/images/chair-left.png'),  #
-            'chair_right': pygame.image.load('../resources/images/chair-right.png')  #
+            'floor': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/floor.jpg'),
+            'wall': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/wall.png'),  #
+            'bar': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/table3.png'),  #
+            'bar_floor': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/waiter.png'),
+            'table': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/table3.png'),
+            'waiter': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/waiter.png'),
+            'chair_front': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/chair-front.png'),  #
+            'chair_back': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/chair-back.png'),  #
+            'chair_left': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/chair-left.png'),  #
+            'chair_right': pygame.image.load('/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/images/chair-right.png')  #
         }
         self.block_size = 50
         self.height = 15

src/matrix.py

@@ -1,4 +1,4 @@
-from src.tile import Tile
+from .tile import Tile
 
 
 class Matrix:
@@ -13,10 +13,11 @@ class Matrix:
 
         for x in range(graphics.width):
             for y in range(graphics.height):
-                self.matrix[x][y] = Tile(type_='floor')
+                self.matrix[x][y] = Tile('floor', x, y)
 
     def set_default_restaurant(self, graphics):
-        lines = [line.rstrip('\n') for line in open('../resources/simulations/simulation_1.txt')]
+        lines = [line.rstrip('\n') for line in open(
+            '/Users/marcindobrowolski/Desktop/Python/Sztuczna_Inteligencja_2020/resources/simulations/simulation_1.txt')]
         symbols = {
             '_': 'floor',
             'W': 'wall',
@@ -33,7 +34,7 @@ class Matrix:
         for x in range(graphics.width):
             for y in range(graphics.height):
                 sign = lines[y][x]
-                self.matrix[x][y] = Tile(symbols[sign])
+                self.matrix[x][y] = Tile(symbols[sign], x, y)
 
     def get_type(self, x, y):
         return self.matrix[x][y].type

src/tile.py

@@ -1,12 +1,12 @@
 # TILE
 class Tile:
-    def __init__(self, type_):
+    def __init__(self, type_, x, y):
         self.type = type_
 
         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:
@@ -15,3 +15,17 @@ class Tile:
 
         self.action_required = 0
 
+        # Atrybuty niezbedne dla A*
+        self.position = (x, y)
+        self.parent = None
+        self.totalCost = 0  # Koszt totalny czyli dystans do wierzcholka startowego + heurystyka F
+        self.startCost = 0  # Dystans do wierzcholka startowego G
+        # Dystant do wierzcholka koncowego oszacowany za pomoca funkcji heurystyki H
+        self.heuristic = 0
+
+    # Operator porownywania pol
+    def __eq__(self, other):
+        return True if (self.position == other.position) else False
+
+
+# dodanie atrybutu x i y do klasy Tile

src/waiter.py

@@ -1,6 +1,7 @@
 import pygame
 
-from src.matrix import Matrix
+from .matrix import Matrix
+from .tile import Tile
 
 
 # WAITER
@@ -11,6 +12,8 @@ class Waiter(pygame.sprite.Sprite):
         self.Y = 0
         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):
@@ -19,14 +22,232 @@ class Waiter(pygame.sprite.Sprite):
                 self.X += x
                 self.Y += y
 
-    def update(self, event, graphics):
-        if event.type == pygame.KEYDOWN:
-            if event.key == pygame.K_LEFT:
-                self.move(-1, 0, graphics)
-            if event.key == pygame.K_RIGHT:
-                self.move(1, 0, graphics)
-            if event.key == pygame.K_UP:
+    def update(self, instruction, graphics):
+        if instruction == 'L':
+            if self.direction == 'N':
+                self.direction = 'W'
+            elif self.direction == 'S':
+                self.direction = 'E'
+            elif self.direction == 'E':
+                self.direction = 'N'
+            elif self.direction == 'W':
+                self.direction = 'S'
+        if instruction == 'R':
+            if self.direction == 'N':
+                self.direction = 'E'
+            elif self.direction == 'S':
+                self.direction = 'W'
+            elif self.direction == 'E':
+                self.direction = 'S'
+            elif self.direction == 'W':
+                self.direction = 'N'
+        if instruction == 'F':
+            if self.direction == 'N':
                 self.move(0, -1, graphics)
-            if event.key == pygame.K_DOWN:
+            if self.direction == 'S':
                 self.move(0, 1, graphics)
-            print(self.X, self.Y)
+            if self.direction == 'E':
+                self.move(1, 0, graphics)
+            if self.direction == 'W':
+                self.move(-1, 0, graphics)
+        #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]]
+
+        # Inicjalizacja list
+        openList = []
+        closedList = []
+
+        openList.append(startNode)
+
+        while len(openList) > 0:
+            openList.sort(key=getTotalCost)
+
+            currentNode = openList.pop(0)
+            closedList.append(currentNode)
+
+            # Tutaj odbywac sie bedzie budowanie sciezki gdy algorytm osiagnie cel
+            if currentNode == goalNode:
+                path = []
+                current = currentNode
+                while current is not None:
+                    path.append(current.position)
+                    current = current.parent
+                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:
+                        children.append(
+                            self.matrix.matrix[currentNode.position[0]][currentNode.position[1] + 1])
+                if currentNode.position[1] - 1 >= 0:
+                    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.position[0] + 1 < len(self.matrix.matrix):
+                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] >= 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:
+
+                for closedChild in closedList:
+                    if child == closedChild:
+                        perm = 1
+                        break
+
+                if perm == 1:
+                    perm = 0
+                    continue
+
+                child.parent = currentNode
+                child.startCost = currentNode.startCost + 1
+                child.heuristic = abs(
+                    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:
+                        perm = 1
+                        continue
+
+                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