Algorytm A*, dodanie kałuż, obrót kelnera, drobne poprawki

Restaurant/main.py

@@ -2,10 +2,111 @@ import pygame
 import random
 import time
 import queue
+import math
 
 pygame.init()
 
 
+def heuristic(current, goal):
+    dx = abs(current[0] - goal[0])
+    dy = abs(current[1] - goal[1])
+    return math.sqrt(dx * dx + dy * dy)
+
+
+class Node():
+    def __init__(self, parent = None, position = None):
+        self.parent = parent
+        self.position = position
+
+        self.g = 0
+        self.h = 0
+        self.f = 0
+
+    def __eq__(self, other):
+        return self.position == other.position
+
+
+def aStar(start, end):
+    if end in cantwalk:
+        print("Wybrano nieodpowiednie pole!")
+        return []
+
+    start_node = Node(None, start)
+    start_node.g = start_node.h = start_node.f = 0
+    end_node = Node(None, end)
+    end_node.g = end_node.h = end_node.f = 0
+
+    open_list = []
+    closed_list = []
+    open_list.append(start_node)
+
+    while len(open_list) > 0:
+        current_node = open_list[0]
+        current_index = 0
+        for index, item in enumerate(open_list):
+            if item.f < current_node.f:
+                current_node = item
+                current_index = index
+
+        open_list.pop(current_index)
+        closed_list.append(current_node)
+
+        if current_node == end_node:
+            path = []
+            current = current_node
+            while current is not None:
+                path.append(current.position)
+                current = current.parent
+            return list(map(lambda t: (t[0], t[1]), path[::-1])) #zwraca odwróconą ściezkę
+
+        children = []
+        # można dodać (-1, -1), (-1, 1), (1, -1), (1, 1) dla poruszania się po przekątnej
+        for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
+            node_position = (current_node.position[0] + new_position[0], current_node.position[1]
+                             + new_position[1])
+
+            if node_position[0] >= (rows - 1) or node_position[0] <= 0 or node_position[1] >= (rows - 1) \
+                    or node_position[1] <= 0:        #spr. czy w kracie
+                continue
+
+            if node_position in cantwalk: #spr. czy można przejść
+                continue
+
+            new_node = Node(current_node, node_position)
+            children.append(new_node)
+
+        didBreak = False
+        for child in children:
+            didBreak = False
+            for closed_child in closed_list:
+                if child == closed_child:
+                    didBreak = True
+                    break
+
+            if didBreak:
+                continue
+
+            if child.position in cantwalk:
+                child.g = current_node.g + 99999
+            elif child.position in puddles:
+                child.g = current_node.g + 3
+            else:
+                child.g = current_node.g + 1
+
+            child.h = heuristic(child.position, end_node.position)
+            child.f = child.g + child.h
+
+            for open_node in open_list:
+                if child == open_node and child.g > open_node.g:
+                    didBreak = True
+                    break
+
+            if didBreak:
+                continue
+
+            open_list.append(child)
+
+
 class Dish(object):
     def __init__(self, dishName, prepTime, eatTime, price):
         self.dishName = dishName
@@ -47,7 +148,6 @@ class Kitchen(object):
             return None
 
     def draw(self, surface):
-
         image1 = pygame.image.load(r'kitchen1.png')
         image1 = pygame.transform.scale(image1, (sizeBetween - 1, sizeBetween - 1))
         surface.blit(image1, (13*sizeBetween+1, 0*sizeBetween+1))
@@ -80,6 +180,7 @@ class Client(object):
     def takeASeat(self, table):
         self.myTable = table
 
+
 class Table(object):
     def __init__(self, pos, capacity):
         self.pos = pos
@@ -103,12 +204,13 @@ class Waiter(object):
     def __init__(self, color, pos, direction):
         self.color = color
         self.pos = pos      #pozycja agenta, zapisana w formie dwuelementowej listy
+        self.posx = pos[0]
+        self.posy = pos[1]
         self.dirnx = 0      #zmienne dirnx i dirny używane są do ruchu bota i ustalania, w którą stronę jest zwrócony
         self.dirny = 1
         self.plates = []    #lista niesionych przez agenta talerzy, planowo lista par: (talerz, klient)
         self.direction = direction #kierunek, w ktory jest skierowany bot
 
-
     def moveRandomly(self, noWalkable):
         rand = random.randrange(1, 5, 1)    #losuje w zakresie 1-4
         #print(rand)
@@ -215,23 +317,42 @@ class Waiter(object):
         client.takePlateAndEat(plate)
 
     def draw(self, surface):
-        if self.direction == 'right':
-            image = pygame.image.load(r'waiter_right.png')
-            image = pygame.transform.scale(image, (sizeBetween - 1, sizeBetween - 1))
+        image = pygame.image.load(r'waiter_right.png')
+        image = pygame.transform.scale(image, (sizeBetween - 1, sizeBetween - 1))
 
-            i = self.pos[0]
-            j = self.pos[1]
+        if self.direction == "right":
+            image = pygame.transform.rotate(image, 270)
+        elif self.direction == "left":
+            image = pygame.transform.rotate(image, 90)
+        elif self.direction == "up":
+            image = pygame.transform.rotate(image, 180)
 
-            surface.blit(image, (i*sizeBetween+1, j*sizeBetween+1))
+        i = self.pos[0]
+        j = self.pos[1]
+        surface.blit(image, (i * sizeBetween + 1, j * sizeBetween + 1))
 
-        else:
-            image = pygame.image.load(r'waiter_left.png')
-            image = pygame.transform.scale(image, (sizeBetween - 1, sizeBetween - 1))
+    def goByAStar(self, end):
+        positionList = aStar(self.pos, end)
+        print(positionList)
+        lenght = len(positionList) - 1
+        for i in range(lenght):
+            movex = positionList[i + 1][0] - positionList[i][0]
+            movey = positionList[i + 1][1] - positionList[i][1]
+            self.posx += movex
+            self.posy += movey
+            self.pos = (self.posx, self.posy)
 
-            i = self.pos[0]
-            j = self.pos[1]
+            if movex == -1 and movey == 0:
+                self.direction = "left"
+            elif movex == 1 and movey == 0:
+                self.direction = "right"
+            elif movey == -1 and movex == 0:
+                self.direction = "down"
+            else:
+                self.direction = "up"
 
-            surface.blit(image, (i*sizeBetween+1, j*sizeBetween+1))
+            redrawWindow(window)
+            time.sleep(0.5)
 
 
 def drawGrid(width, rows, surface):
@@ -252,6 +373,10 @@ def redrawWindow(surface):
     kitchen.draw(surface)
     for i in range(len(tables)):
         tables[i].draw(surface)
+    for i in range(len(puddles)):
+        image = pygame.image.load(r'puddle.png')
+        image = pygame.transform.scale(image, (sizeBetween - 1, sizeBetween - 1))
+        surface.blit(image, (puddles[i][0]* sizeBetween + 1, puddles[i][1]* sizeBetween + 1))
     drawGrid(width, rows, surface)
     pygame.display.update()
 
@@ -263,7 +388,7 @@ def noWalkable(tables, kitchen):
     return list
 
 def main():
-    global width, rows, bot, beige, white, black, sizeBetween, tables, kitchen
+    global width, rows, bot, beige, white, black, sizeBetween, tables, kitchen, cantwalk, puddles, window
     # skróty do kolorów
     beige = (255, 205, 178)
     white = (255, 255, 255)
@@ -273,35 +398,44 @@ def main():
     rows = 15
     sizeBetween = width // rows     #wielkość pojedynczej kratki
     window = pygame.display.set_mode((width, width))
-    bot = Waiter((255, 0, 0), [12, 8], 'right')
+    bot = Waiter((255, 0, 0), (12, 8), "right")
 
     tables = []
-    tables.append(Table([0, 3], 1))
-    tables.append(Table([0, 6], 1))
-    tables.append(Table([0, 9], 2))
-    tables.append(Table([0, 12], 2))
-    tables.append(Table([4, 4], 2))
-    tables.append(Table([4, 7], 2))
-    tables.append(Table([4, 10], 2))
-    tables.append(Table([4, 13], 2))
-    tables.append(Table([8, 3], 1))
-    tables.append(Table([8, 6], 1))
-    tables.append(Table([8, 9], 2))
-    tables.append(Table([8, 12], 2))
+    tables.append(Table((0, 3), 1))
+    tables.append(Table((0, 6), 1))
+    tables.append(Table((0, 9), 2))
+    tables.append(Table((0, 12), 2))
+    tables.append(Table((4, 4), 2))
+    tables.append(Table((4, 7), 2))
+    tables.append(Table((4, 10), 2))
+    tables.append(Table((4, 13), 2))
+    tables.append(Table((8, 3), 1))
+    tables.append(Table((8, 6), 1))
+    tables.append(Table((8, 9), 2))
+    tables.append(Table((8, 12), 2))
 
     flag = True
     clock = pygame.time.Clock()
-    kitchen = Kitchen([13, 1])
+    kitchen = Kitchen((13, 1))
 
-    list = noWalkable(tables, kitchen)  # lista pozycji, na ktore bot nie moze wejsc
+    cantwalk = noWalkable(tables, kitchen)  # lista pozycji, na ktore bot nie moze wejsc
+    puddles = []
+    puddles.append((7, 10))
+    puddles.append((2, 5))
+    puddles.append((3, 12))
+    puddles.append((10, 3))
+    puddles.append((6, 2))
+    puddles.append((12, 10))
+    puddles.append((1, 4))
 
     while flag:
         pygame.time.delay(100)
         clock.tick(60)
-        for i in range(40):     #bot testowo ma wykonać 40 kroków
-            bot.moveRandomly(list)
-            redrawWindow(window)
-            time.sleep(0.5)     #opóźnienie każdego kolejnego kroku o pół sekundy
+        redrawWindow(window)
+        #bot.moveRandomly(list)
+        goal = (1,5)
+        bot.goByAStar(goal)
+        time.sleep(120)
         flag = False