From 91828da0f1accdd4216e3dfd7d77b8bf5cabe44b Mon Sep 17 00:00:00 2001 From: s481851 Date: Wed, 24 Apr 2024 21:18:09 +0200 Subject: [PATCH] Added BFS algorithm for pathfinding --- main.py | 231 ++++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 150 insertions(+), 81 deletions(-) diff --git a/main.py b/main.py index d410ea6..b1103a0 100644 --- a/main.py +++ b/main.py @@ -3,56 +3,37 @@ import random import time import numpy import threading -import collections -from collections import deque kuchnia_xy = 0 pozycja_startowa = 0 -# ------------Ustawienia siatki +#------------Ustawienia siatki blockSize = 60 rows = 14 columns = 24 - -# -----------------------------Inicjacja klas -class Stan: - def __init__(self,position, kierunek): - self.position = position - self.kierunek = kierunek - - toVisit = deque() - visited = deque() - - def succ(self): - newStan: Stan(x+1, y, 1) - queue = deque([x, y, self.kierunek]) - print(queue[0]) - -#def BFS: - - +#-----------------------------Inicjacja klas class Kelner: def __init__(self, x, y): self.x = x self.y = y - self.speed = 70 # od 0 do 100, preferowane 70 + self.speed = 70 # od 0 do 100, preferowane 70 + self.stanPrzestrzeni = [0,0,0] self.stan = "stoi" # Stan kelnera: stoi, odbiera lub wraca self.stolik_docelowy = None # Stolik, do którego idzie kelner self.chodzi = True self.cel_x = x self.cel_y = y - self.kierunek = 0 # 0 - północ, 1 - wschód, 2 - południe, 3 - zachód + self.kierunek = 0 # 0 - północ, 1 - wschód, 2 - południe, 3 - zachód + self.indexRuchu = 0 def wklej(self): kelnerRotated = pygame.transform.rotate(kelnerImg, -90 * kelner.kierunek) screen.blit(kelnerRotated, (self.x * blockSize, self.y * blockSize)) - #def planuj_ruch(self): - - def idz_do_stolika(self): - self.cel_x, self.cel_y = self.stolik_docelowy.x, self.stolik_docelowy.y - kelner.stan = "odbiera" + # def idz_do_stolika(self): + # self.cel_x, self.cel_y = self.stolik_docelowy.x, self.stolik_docelowy.y + # kelner.stan = "odbiera" def idz_do_kuchni(self): self.cel_x, self.cel_y = kuchnia_xy, kuchnia_xy @@ -60,26 +41,40 @@ class Kelner: kelner.stan = "wraca" def obrot_w_lewo(self): - if self.kierunek == 0: - self.kierunek = 3 - else: - self.kierunek -= 1 + self.kierunek = (self.kierunek - 1) % 4 + self.stanPrzestrzeni[2] = (self.stanPrzestrzeni[2] - 1) % 4 def obrot_w_prawo(self): - if self.kierunek == 3: - self.kierunek = 0 - else: - self.kierunek += 1 + self.kierunek = (self.kierunek + 1) % 4 + self.stanPrzestrzeni[2] = (self.stanPrzestrzeni[2] + 1) % 4 def idz_do_przodu(self): if self.kierunek == 0: - self.y -= 1 + self.y -= 1 + self.stanPrzestrzeni[1] -= 1 elif self.kierunek == 1: - self.x += 1 + self.x += 1 + self.stanPrzestrzeni[0] += 1 elif self.kierunek == 2: - self.y += 1 + self.y += 1 + self.stanPrzestrzeni[1] += 1 elif self.kierunek == 3: - self.x -= 1 + self.x -= 1 + self.stanPrzestrzeni[0] -= 1 + + + def wykonajAkcje(self, ruchy): + if self.indexRuchu < len(ruchy): + akcja = ruchy[self.indexRuchu] + if akcja == 'F': + self.idz_do_przodu() + elif akcja == 'L': + self.obrot_w_lewo() + elif akcja == 'R': + self.obrot_w_prawo() + self.indexRuchu += 1 + if self.indexRuchu >= len(ruchy): # Reset po zakończeniu wszystkich ruchów + self.indexRuchu = 0 class Stolik: @@ -92,13 +87,83 @@ class Stolik: screen.blit(stolikImg, (self.x * blockSize, self.y * blockSize)) -# --------------Inicjacja obiektów -kelner = Kelner(pozycja_startowa, pozycja_startowa) +#-----------------Przeszukiwanie przestrzeni stanów -# -----------wspolrzedne stolikow -coords = ["8 8", "4 12", "16 8", "12 12", "20 12"] +from collections import deque -# Tworzenie listy stolikow +def bfs(start, cel, stoliki): + queue = deque([start]) + odwiedzone = set([start]) + poprzednik = {start: (None, None)} # Słownik ścieżek (stan, ruch) + + while queue: + obecny = queue.popleft() + + if obecny[:2] == cel: + return odtworz_ruchy(poprzednik, obecny) + + for sasiad, ruch in generuj_nastepniki_i_ruchy(obecny, stoliki): + if sasiad not in odwiedzone: + odwiedzone.add(sasiad) + queue.append(sasiad) + poprzednik[sasiad] = (obecny, ruch) + + return [] # Cel nie został znaleziony + + +#----------Funkcja generowania następników dla poszczególnych stanów +def generuj_nastepniki_i_ruchy(stan, stoliki): + x, y, kierunek = stan + ruchy = [] + + # Obrot w lewo + nowy_kierunek = (kierunek - 1) % 4 + ruchy.append(((x, y, nowy_kierunek), 'L')) + + # Obrot w prawo + nowy_kierunek = (kierunek + 1) % 4 + ruchy.append(((x, y, nowy_kierunek), 'R')) + + # Krok do przodu + if kierunek == 0: + nowy_x, nowy_y = x, y - 1 + elif kierunek == 1: + nowy_x, nowy_y = x + 1, y + elif kierunek == 2: + nowy_x, nowy_y = x, y + 1 + elif kierunek == 3: + nowy_x, nowy_y = x - 1, y + + #sprawdzamy, czy następny stan jest w granicach planszy + if 0 <= nowy_x < columns and 0 <= nowy_y < rows: + #sprawdzamy, czy następny stan nie wchodzi w stolik + if (nowy_x, nowy_y) not in stoliki: + ruchy.append(((nowy_x, nowy_y, kierunek), 'F')) + + return ruchy + +#-----Funkcja tworząca listę kroków potrzebnych do uzyskania celu +def odtworz_ruchy(poprzednicy, cel): + ruchy = [] + krok = cel + while poprzednicy[krok][0] is not None: + ruchy.append(poprzednicy[krok][1]) + krok = poprzednicy[krok][0] + ruchy.reverse() + return ruchy + + +start = (0, 0, 0) # Początkowy stan +cel = (0, 0) # Docelowe współrzędne + + +#--------------Inicjacja obiektów +kelner = Kelner(pozycja_startowa,pozycja_startowa) + +#-----------wspolrzedne stolikow +coords = ["8 10", "4 12", "16 10", "12 12", "20 12", "12 9", "0 6", "8 4", "16 4", "23 6"] + +#Tworzenie listy stolikow stoliki = [] for coord in coords: x, y = map(int, coord.split()) @@ -107,7 +172,7 @@ for coord in coords: pygame.init() pygame.display.set_caption("Automatyczny kelner") -# ----------------wymiary okna +#----------------wymiary okna width = columns * blockSize height = rows * blockSize @@ -122,89 +187,94 @@ menuImg = pygame.transform.scale(menuImg, (blockSize / 2, blockSize / 2)) kitchenImg = pygame.image.load("kitchen.png") kitchenImg = pygame.transform.scale(kitchenImg, (blockSize * 2, blockSize * 2)) - def kuchnia(x, y): screen.blit(kitchenImg, (x * blockSize, y * blockSize)) - def menu(x, y): screen.blit(menuImg, (x * blockSize, y * blockSize)) - def wypiszOkno(): screen.fill((0, 0, 0)) for x in range(0, width, blockSize): for y in range(0, height, blockSize): rect = pygame.Rect(x, y, blockSize, blockSize) - pygame.draw.rect(screen, (200, 200, 200), rect, 1) # -------------Wypisz kratę -TA - + pygame.draw.rect(screen, (200, 200, 200), rect, 1) #-------------Wypisz kratę -TA run = True # czcionka = pygame.font.SysFont('Arial',50) licznik = 0 +ruchy = [] +cel2 = [] while run: + stoliki_pozycje = [(stolik.x, stolik.y) for stolik in stoliki] + cel2 = list(cel) + + #print(f"{kelner.stanPrzestrzeni}, {cel2}, {kelner.indexRuchu} {kelner.stan}") wypiszOkno() kuchnia(kuchnia_xy, kuchnia_xy) for stolik in stoliki: stolik.wklej() - + kelner.wklej() if kelner.stan == "wraca": - menu(kelner.x, kelner.y) + menu(kelner.x, kelner.y) licznik += 1 - # ------------weź zamowienie - for stolik in stoliki: + #------------weź zamowienie + for stolik in stoliki: if stolik.zamowione == True: menu(stolik.x, stolik.y) if kelner.stan == "stoi": kelner.stolik_docelowy = stolik - #kelner.planuj_ruch() - kelner.idz_do_stolika() + kelner.cel_x, kelner.cel_y = kelner.stolik_docelowy.x, kelner.stolik_docelowy.y - 1 + cel = (kelner.cel_x, kelner.cel_y) + print("Szukam ścieżki do stolika...") + ruchy = bfs(tuple(kelner.stanPrzestrzeni), cel, stoliki_pozycje) + kelner.stan = "odbiera" + if ruchy: + print("Znaleziono ścieżkę ruchów: ", ruchy) + else: + print("Nie znaleziono ścieżki do celu.") + - # ----------Losuje stoliki, które dokonają zamówienia + #----------Losuje stoliki, które dokonają zamówienia if kelner.stan == "stoi": for stolik in stoliki: - if stolik.zamowione == True: + if stolik.zamowione == True: break for i in range(len(stoliki)): if random.randrange(2) == 1: stoliki[i].zamowione = True - # print(kelner.stan)--------------------------Wypisuje stan kelnera - # print(f"{kelner.x} {kelner.y}")-------------Wypisuje wspolrzedne kelnera + #print(kelner.stan)--------------------------Wypisuje stan kelnera + #print(f"{kelner.x} {kelner.y}")-------------Wypisuje wspolrzedne kelnera - # --------------Zmiana kierunku kelnera - if kelner.x != kelner.cel_x: - if kelner.x < kelner.cel_x: - kelner.kierunek = 1 - else: - kelner.kierunek = 3 - else: - if kelner.y < kelner.cel_y: # zmieniamy pozycje Y dopiero, gdy pozycja X bedzie prawidlowa - kelner.kierunek = 2 - else: - kelner.kierunek = 0 + #----------------Zmiana pozycji kelnera + if kelner.chodzi == True and licznik % (101 - kelner.speed) == 0 and kelner.stanPrzestrzeni[:2] != cel2: #ograniczenie prędkości + kelner.wykonajAkcje(ruchy) - # ----------------Zmiana pozycji kelnera - if kelner.chodzi == True and licznik % (101 - kelner.speed) == 0: # ograniczenie prędkości - kelner.idz_do_przodu() - # ----------------Sprawdzenie, czy kelner jest u celu - if kelner.x == kelner.cel_x and kelner.y == kelner.cel_y: - if kelner.stan == "odbiera": + if kelner.stanPrzestrzeni[:2] == cel2: + if kelner.stan == "odbiera" and kelner.x == kelner.stolik_docelowy.x and kelner.y == kelner.stolik_docelowy.y - 1: kelner.stolik_docelowy.zamowione = False kelner.idz_do_kuchni() - - elif kelner.stan == "wraca": + cel = (kelner.cel_x, kelner.cel_y) + print("Szukam ścieżki do kuchni...") + ruchy = bfs(tuple(kelner.stanPrzestrzeni), cel, stoliki_pozycje) + if ruchy: + print("Znaleziono ścieżkę ruchów: ", ruchy) + else: + print("Nie znaleziono ścieżki do celu.") + elif kelner.stan == "wraca" and kelner.x == kuchnia_xy and kelner.y == kuchnia_xy: kelner.stan = "stoi" + time.sleep(0.001) key = pygame.key.get_pressed() @@ -212,5 +282,4 @@ while run: for event in pygame.event.get(): if event.type == pygame.QUIT: run = False - pygame.quit() \ No newline at end of file