bfs graph implementation

src/main.py

@@ -1,5 +1,6 @@
 import pygame
 import random
+import time
 
 pygame.init()
 
@@ -8,11 +9,33 @@ WIDTH = 675
 HEIGHT = 675
 size = 75
 QTY_OF_PACKAGES = 8
+RECT_SIZE = 9
 
 IMAGE = pygame.image.load('img/wozek2.png')
 BACKGROUND = pygame.transform.scale(pygame.image.load('img/background.png'), (WIDTH, HEIGHT))
 DOCK = pygame.transform.scale(pygame.image.load('img/dock_left.png'), (75, 75))
 
+get_pix_from_position = {}
+
+
+def get_position_from_pix(pix_pos):
+    for key, value in get_pix_from_position.items():
+        if pix_pos == value:
+            return key
+    print("ERROR: THERE IS NO POSITION LIKE THIS (get_position_from_pix)")
+
+
+def create_postiions():
+    size_agent = 40
+    x_position_pix = size_agent
+    y_position_pix = size_agent
+    for position_x in range(RECT_SIZE):
+        for position_y in range(RECT_SIZE):
+            get_pix_from_position[(position_x, position_y)] = (x_position_pix, y_position_pix)
+            y_position_pix += size
+        y_position_pix = size_agent
+        x_position_pix += size
+
 
 def generate_package(a, b):
     y = random.randint(1, 4)
@@ -27,6 +50,76 @@ def generate_package(a, b):
         p1 = Package((a, b), x, "Builders", 'img/package_builders.png')
     return p1
 
+# FRINGE - struktura danych przechowująca wierzchołki do odwiedzenia
+# EXPLORED - lista odwiedzonych stanów
+# istate - stan początkowy
+# Succ - funkcja następnika
+# Goaltest -test spełnienia celu
+
+
+def breadth_first_search(istate): # COORDINATES OF A START PLACE
+    fringe = []
+    explored = []
+    start = Node(istate)
+    fringe.append(start)
+    path = []
+
+    while True:
+        if not fringe:
+            return False
+
+        elem = fringe.pop(0)
+
+        if agent.goal_test(elem.state):
+            # return ciag akcji zbudowany z wykorzystaniem pol parent i action
+            while elem.parent is not None:
+                path.append(elem.action)
+                elem = elem.parent
+            path.reverse()
+            return path
+
+        explored.append(elem)
+
+        for action, state in elem.successor():
+            fringe_states = []
+            explored_states = []
+            for node in fringe:
+                fringe_states.append(node.state)
+            for node2 in explored:
+                explored_states.append(node2.state)
+
+            if state not in fringe_states and state not in explored_states:
+                x = Node(state)
+                x.parent = elem
+                x.action = action
+                fringe.append(x)
+
+
+class Node:
+    def __init__(self, state, action=None, parent=None):
+        self.state = state
+        self.action = action
+        self.parent = parent
+
+    def successor(self):
+        neighbours = []
+        x = self.state[0]
+        y = self.state[1]
+
+        if x < 8:
+            neighbours.append(("right", (x+1, y)))
+
+        if x > 0:
+            neighbours.append(("left", (x-1, y)))
+
+        if y > 0:
+            neighbours.append(("up", (x, y-1)))
+
+        if y < 8:
+            neighbours.append(("down", (x, y+1)))
+
+        return neighbours
+
 
 class Package:
     def __init__(self, pos, content, content_size, pack_image):
@@ -40,34 +133,90 @@ class Package:
 
 class Shelf:
     def __init__(self, pos, content, content_size):
+        self.pos_pix = get_pix_from_position[pos]
         self.content = content
         self.content_size = content_size
         self.image = pygame.transform.scale(pygame.image.load('img/shelf.png'), (60, 60))
-        self.rect = self.image.get_rect(center=pos)
+        self.rect = self.image.get_rect(center=self.pos_pix)
 
 
 class Agent:
     def __init__(self, pos):
         self.pos = pos
+        self.pos_coord = get_position_from_pix(pos)
         self.image = IMAGE
         self.rect = self.image.get_rect(center=pos)
+        self.goal = (0, 0)
+        self.path = None
+        self.goal_achieved = False
+
+        self.image_left = pygame.transform.flip(IMAGE, True, False)
+        self.image_right = IMAGE
+        self.image_down = pygame.transform.rotate(self.image_right, -90)
+        self.image_up = pygame.transform.rotate(self.image_left, -90)
 
     def move(self, key):
         if key == pygame.K_UP and self.rect.y - size > 0:
             self.rect.move_ip(0, -size)
             self.pos = (self.pos[0], self.pos[1] - size)
+            #
+            self.pos_coord = get_position_from_pix(self.pos)
 
         if key == pygame.K_DOWN and self.rect.y + size < HEIGHT:
             self.rect.move_ip(0, size)
             self.pos = (self.pos[0], self.pos[1] + size)
+            #
+            self.pos_coord = get_position_from_pix(self.pos)
 
         if key == pygame.K_RIGHT and self.rect.x + size < WIDTH:
             self.rect.move_ip(size, 0)
             self.pos = (self.pos[0] + size, self.pos[1])
+            #
+            self.pos_coord = get_position_from_pix(self.pos)
 
         if key == pygame.K_LEFT and self.rect.x - size > 0:
             self.rect.move_ip(-size, 0)
             self.pos = (self.pos[0] - size, self.pos[1])
+            #
+            self.pos_coord = get_position_from_pix(self.pos)
+
+    def move_bfs(self):
+        if self.path is None or not self.path:
+            self.goal_achieved = True
+            # LIFT PACKAGE??
+
+        else:
+            move = self.path.pop(0)
+
+            if move == "right":
+                self.image = self.image_right # ROTATION
+
+                self.rect.move_ip(size, 0)
+                self.pos = (self.pos[0] + size, self.pos[1])
+                self.pos_coord = get_position_from_pix(self.pos)
+            elif move == "left":
+                self.image = self.image_left # ROTATION
+
+                self.rect.move_ip(-size, 0)
+                self.pos = (self.pos[0] - size, self.pos[1])
+                self.pos_coord = get_position_from_pix(self.pos)
+
+            elif move == "down":
+                self.image = self.image_down # ROTATION
+
+                self.rect.move_ip(0, size)
+                self.pos = (self.pos[0], self.pos[1] + size)
+                self.pos_coord = get_position_from_pix(self.pos)
+
+            elif move == "up":
+                self.image = self.image_up # ROTATION
+
+                self.rect.move_ip(0, -size)
+                self.pos = (self.pos[0], self.pos[1] - size)
+                self.pos_coord = get_position_from_pix(self.pos)
+
+            else:
+                print("error: bad path :move_bfs")
 
     def lift_package(self, key):
         if key == pygame.K_e:
@@ -77,9 +226,56 @@ class Agent:
                 elif package.pos == agent.pos:
                     package.is_package_up = True
 
+    def lift_package_bfs(self):
+        for package in Package_list:
+            if package.is_package_up:
+                 package.is_package_up = False
+            elif package.pos == agent.pos:
+                package.is_package_up = True
+
+    def rotate(self, key):
+        if key == pygame.K_a:
+            self.turn_agent_left()
+
+        if key == pygame.K_d:
+            self.turn_agent_right()
+
+        if key == pygame.K_s:
+            self.turn_agent_down()
+
+        if key == pygame.K_w:
+            self.turn_agent_up()
+
+    def turn_agent_left(self):
+        self.successor = (self.pos_coord[0]-1, self.pos_coord[1])
+        self.image = self.image_left
+
+    def turn_agent_right(self):
+        self.successor = (self.pos_coord[0]+1, self.pos_coord[1])
+        self.image = self.image_right
+
+    def turn_agent_down(self):
+        self.successor = (self.pos_coord[0], self.pos_coord[1]-1)
+        self.image = self.image_down
+
+    def turn_agent_up(self):
+        self.successor = (self.pos_coord[0], self.pos_coord[1]+1)
+        self.image = self.image_up
+
+    def goal_test(self, state):
+        if state == self.goal:
+            return True
+        else:
+            return False
+
 
 board = pygame.Surface((WIDTH, HEIGHT), pygame.SRCALPHA)  # transparently surface
 
+#
+create_postiions()
+
+#
+
 for x in range(9):
     for y in range(9):
         pygame.draw.rect(board, (0, 0, 0), (x * size, y * size, size, size), 3)
@@ -88,10 +284,13 @@ agent = Agent((40, 640))
 
 
 Shelf_list = [
-    Shelf((640, 340), 'Explosive', 5), Shelf((565, 340), 'Explosive', 5), Shelf((490, 340), 'Explosive', 5), Shelf((415, 340), 'Explosive', 5),
+    Shelf((5, 2), 'Explosive', 5), Shelf((6, 2), 'Explosive', 5), Shelf((7, 2), 'Explosive', 5), Shelf((8, 2), 'Explosive', 5),
-    Shelf((640, 190), 'Grocery', 5), Shelf((565, 190), 'Grocery', 5), Shelf((490, 190), 'Grocery', 5), Shelf((415, 190), 'Grocery', 5),
+    Shelf((5, 4), 'Grocery', 5), Shelf((6, 4), 'Grocery', 5), Shelf((7, 4), 'Grocery', 5), Shelf((8, 4), 'Grocery', 5),
-    Shelf((640, 640), 'Builders', 5), Shelf((565, 640), 'Builders', 5), Shelf((490, 640), 'Builders', 5), Shelf((415, 640), 'Builders', 5),
+    Shelf((5, 6), 'Builders', 5), Shelf((6, 6), 'Builders', 5), Shelf((7, 6), 'Builders', 5), Shelf((8, 6), 'Builders', 5),
-    Shelf((640, 490), 'Electronic', 5), Shelf((565, 490), 'Electronic', 5), Shelf((490, 490), 'Electronic', 5), Shelf((415, 490), 'Electronic', 5)]
+    Shelf((5, 8), 'Electronic', 5), Shelf((6, 8), 'Electronic', 5), Shelf((7, 8), 'Electronic', 5), Shelf((8, 8), 'Electronic', 5)]
+
+
+coord_goals = [(6, 8), (0, 0), (5,8), (0, 0), (7,8), (0, 0), (8,8), (0, 0), (5,6), (0, 0), (6,6), (0, 0), (7,6), (0, 0), (8,6), (0, 8)]
 
 screen = pygame.display.set_mode([WIDTH, HEIGHT])
 
@@ -99,6 +298,8 @@ Package_list = [
     generate_package(40, 40)
 ]
 
+agent.path = breadth_first_search(agent.pos_coord)
+
 running = True
 while running:
 
@@ -109,10 +310,12 @@ while running:
         if event.type == pygame.KEYDOWN:
             agent.move(event.key)
             agent.lift_package(event.key)
+            agent.rotate(event.key)
 
     for package in Package_list:
         if package.is_package_up:
             package.rect.move_ip(agent.pos[0] - package.pos[0], agent.pos[1] - package.pos[1])
+
             package.pos = agent.pos
 
     if len(Package_list) < QTY_OF_PACKAGES:
@@ -128,6 +331,25 @@ while running:
 
     # screen.fill((0, 0, 0))
 
+
+
+
+    # pathing
+    if not agent.goal_achieved:
+        print(agent.path)
+
+        agent.move_bfs()
+
+    else:
+        time.sleep(.25)
+
+        agent.lift_package_bfs()
+        if coord_goals:
+            agent.goal = coord_goals.pop(0)
+            agent.goal_achieved = False
+
+        agent.path = breadth_first_search(agent.pos_coord)
+
     screen.blit(BACKGROUND, [0, 0])
     screen.blit(DOCK, [0, 0])
     screen.blit(board, board.get_rect())
@@ -137,6 +359,8 @@ while running:
     for package in Package_list:
         screen.blit(package.image, package.rect)
 
+
+
     pygame.display.update()
 
 pygame.quit()