adding a*

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)
 

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:
+        if ktory_algorytm == 1:
             x1 = x / 70
             y1 = y / 70
-        if search_grid.grid[(x1,y1)] is GridCellType.FREE:
+            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
+    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