s474139/Inteligentny_Wozek
4
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

adding a*

Browse Source
This commit is contained in:
Mateusz Kantorski 2023-05-14 15:21:09 +02:00
parent 895c9b3569
commit d8b997126d
2 changed files with 111 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
main.py
View File

@ -24,8 +24,9 @@ def main():
y = pygame.mouse.get_pos()[1]
wiersz = ekran.sprawdz_ktory_wiersz(x)
kolumna = ekran.sprawdz_ktora_kolumna(y)
docelowy_stan = wyszukiwanie.Stan(wiersz * 70, kolumna * 70, 0)
wezel = wyszukiwanie.wyszukiwanie_bfs(wozek.obecnyStan, docelowy_stan, grid_points)
docelowy_stan = wyszukiwanie.Stan(wiersz * 70, kolumna * 70, 1)
# wezel = wyszukiwanie.wyszukiwanie_bfs(wozek.obecnyStan, docelowy_stan, grid_points)
wezel = wyszukiwanie.wyszukiwanie_a_star(wozek.obecnyStan, docelowy_stan, grid_points)
sciezka = wyszukiwanie.znajdz_sciezke(wezel)
wozek.przemiesc_wozek_po_sciezce(sciezka)

145
wyszukiwanie.py
View File

@ -1,4 +1,6 @@
from grid import GridCellType
import heapq
class Stan:
def __init__(self, x, y, kierunek):
@ -6,21 +8,38 @@ class Stan:
self.y = y
self.kierunek = kierunek
def __eq__(self, other):
return self.x == other.x and self.y == other.y and self.kierunek == other.kierunek
class Wezel:
def __init__(self, stan, rodzic):
def __init__(self, stan, waga=0, g=0, h=0, rodzic=None, ):
self.stan = stan
self.waga = waga
self.g = g # koszt dotarcia do wezla
self.h = h # heurystyka
self.rodzic = rodzic
self.f = g + h # koszt calkowity
def nastepnik(wezel, licznik, search_grid):
def __lt__(self, other):
return self.f < other.f
def __eq__(self, other):
return self.stan == other.stan
def znajdz_nastepcow(wezel, search_grid, ktory_algorytm):
# gora -> prawo -> dol -> lewo | obrot w prawo
# gora -> lewo -> dol -> prawo | obrot w lewo
# 0 gora 1 prawo 2 dol 3 lewo
x = wezel.stan.x
y = wezel.stan.y
obrot_w_prawo = Wezel(Stan(x, y, (wezel.stan.kierunek + 1) % 4), licznik)
obrot_w_lewo = Wezel(Stan(x, y, 3 if wezel.stan.kierunek == 0 else wezel.stan.kierunek - 1), licznik)
obrot_w_prawo = Wezel(Stan(x, y, (wezel.stan.kierunek + 1) % 4))
obrot_w_prawo.rodzic = wezel
obrot_w_prawo.waga = 1
obrot_w_lewo = Wezel(Stan(x, y, 3 if wezel.stan.kierunek == 0 else wezel.stan.kierunek - 1))
obrot_w_lewo.rodzic = wezel
obrot_w_lewo.waga = 1
if wezel.stan.kierunek == 0:
y -= 70
elif wezel.stan.kierunek == 1:
@ -31,51 +50,103 @@ def nastepnik(wezel, licznik, search_grid):
x -= 70
wezly = [obrot_w_prawo, obrot_w_lewo]
ruch_w_przod = Wezel(Stan(x, y, wezel.stan.kierunek), licznik)
ruch_w_przod = Wezel(Stan(x, y, wezel.stan.kierunek))
ruch_w_przod.rodzic = wezel
# sprawdzenie czy nie wyjdzie poza plansze
if 0 <= x <= 910 and 0 <= y <= 910:
x1 = x / 70
y1 = y / 70
if search_grid.grid[(x1,y1)] is GridCellType.FREE:
if ktory_algorytm == 1:
x1 = x / 70
y1 = y / 70
if search_grid.grid[(x1, y1)] is GridCellType.FREE:
wezly.append(ruch_w_przod)
else:
wezly.append(ruch_w_przod)
return wezly
def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_grid):
pierwszy_wezel = Wezel(stan_poczatkowy, 0)
pierwszy_wezel = Wezel(stan_poczatkowy)
fringe = [pierwszy_wezel]
odwiedzone = [pierwszy_wezel]
licznik = 0
global mapa
mapa = dict()
while fringe:
wezel = fringe.pop(0)
licznik = licznik + 1
mapa[licznik] = wezel
if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
return wezel
lista1 = nastepnik(wezel, licznik, search_grid)
lista1 = znajdz_nastepcow(wezel, search_grid, 1)
for obecny_wezel in lista1:
flag_juz_odwiedzony = 0
for odwiedzony_wezel in odwiedzone:
# sprawdzenie czy odwiedzilismy juz ten stan
if odwiedzony_wezel.stan.x == obecny_wezel.stan.x and odwiedzony_wezel.stan.y == obecny_wezel.stan.y\
and odwiedzony_wezel.stan.kierunek == obecny_wezel.stan.kierunek:
flag_juz_odwiedzony = 1
break
if flag_juz_odwiedzony == 1:
if obecny_wezel in odwiedzone:
continue
else:
fringe.append(obecny_wezel)
odwiedzone.append(obecny_wezel)
return pierwszy_wezel
fringe.append(obecny_wezel)
odwiedzone.append(obecny_wezel)
return None
def znajdz_sciezke(wezel):
sciezka = [wezel]
index = wezel.rodzic
while index != 0:
wezel = mapa[index]
sciezka = []
while wezel:
sciezka.append(wezel)
index = wezel.rodzic
sc = list(reversed(sciezka))
return sc
wezel = wezel.rodzic
sciezka.reverse()
return sciezka
def oblicz_heurystyke(obecnyStan, docelowyStan):
dx = abs(obecnyStan.x - docelowyStan.x)
dy = abs(obecnyStan.y - docelowyStan.y)
return dx + dy
def wyszukiwanie_a_star(poczatkowyStan, docelowyStan, search_grid):
fringe = []
heapq.heapify(fringe)
odwiedzone = list()
heapq.heappush(fringe, Wezel(poczatkowyStan))
while fringe:
obecny_wezel = heapq.heappop(fringe)
if obecny_wezel.stan == docelowyStan:
return obecny_wezel
odwiedzone.append(obecny_wezel)
nastepcy = znajdz_nastepcow(obecny_wezel, search_grid, 2)
for nastepca in nastepcy:
dobierz_wage_do_wezla(nastepca, search_grid)
h = oblicz_heurystyke(nastepca.stan, docelowyStan)
g = nastepca.waga + obecny_wezel.g
f = g + h
if nastepca not in fringe and nastepca not in odwiedzone:
nastepca.f = f
nastepca.g = g
nastepca.h = h
heapq.heappush(fringe, nastepca)
elif nastepca in fringe:
index = fringe.index(nastepca)
stary_koszt = fringe[index].f
if stary_koszt > f:
nastepca.f = f
nastepca.g = g
nastepca.h = h
fringe[index] = nastepca
print(index)
# heapq.heapify(fringe)
return None
def dobierz_wage_do_wezla(wezel, search_grid):
# sprawdzenie czy to obrot
if wezel.waga == 1:
return None
x1 = wezel.stan.x / 70
y1 = wezel.stan.y / 70
if search_grid.grid[(x1, y1)] is GridCellType.FREE:
wezel.waga = 1
elif search_grid.grid[(x1, y1)] is GridCellType.RACK:
wezel.waga = 99999
elif search_grid.grid[(x1, y1)] is GridCellType.PLACE:
wezel.waga = 99999
return None