Added BFS algorithm for pathfinding

This commit is contained in:
s481851 2024-04-24 21:18:09 +02:00
parent 45a159a0fc
commit 91828da0f1

211
main.py
View File

@ -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.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.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.stanPrzestrzeni[1] -= 1
elif self.kierunek == 1:
self.x += 1
self.stanPrzestrzeni[0] += 1
elif self.kierunek == 2:
self.y += 1
self.stanPrzestrzeni[1] += 1
elif self.kierunek == 3:
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,30 +187,32 @@ 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)
@ -159,16 +226,24 @@ while run:
licznik += 1
# ------------weź zamowienie
#------------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:
@ -177,34 +252,29 @@ while run:
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()