diff --git a/main.py b/main.py index b4ae383..e451477 100644 --- a/main.py +++ b/main.py @@ -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) diff --git a/wyszukiwanie.py b/wyszukiwanie.py index dd83e14..41c65f0 100644 --- a/wyszukiwanie.py +++ b/wyszukiwanie.py @@ -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