From 95f0e6751610eece66a266d9b903b53a8b4dbbc0 Mon Sep 17 00:00:00 2001 From: SaluSL Date: Fri, 23 Apr 2021 17:31:49 +0200 Subject: [PATCH] bfs graph implementation --- src/main.py | 234 ++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 229 insertions(+), 5 deletions(-) diff --git a/src/main.py b/src/main.py index a25e64e..3048fa0 100644 --- a/src/main.py +++ b/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((640, 190), 'Grocery', 5), Shelf((565, 190), 'Grocery', 5), Shelf((490, 190), 'Grocery', 5), Shelf((415, 190), 'Grocery', 5), - Shelf((640, 640), 'Builders', 5), Shelf((565, 640), 'Builders', 5), Shelf((490, 640), 'Builders', 5), Shelf((415, 640), 'Builders', 5), - Shelf((640, 490), 'Electronic', 5), Shelf((565, 490), 'Electronic', 5), Shelf((490, 490), 'Electronic', 5), Shelf((415, 490), 'Electronic', 5)] + Shelf((5, 2), 'Explosive', 5), Shelf((6, 2), 'Explosive', 5), Shelf((7, 2), 'Explosive', 5), Shelf((8, 2), 'Explosive', 5), + Shelf((5, 4), 'Grocery', 5), Shelf((6, 4), 'Grocery', 5), Shelf((7, 4), 'Grocery', 5), Shelf((8, 4), 'Grocery', 5), + Shelf((5, 6), 'Builders', 5), Shelf((6, 6), 'Builders', 5), Shelf((7, 6), 'Builders', 5), Shelf((8, 6), 'Builders', 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()