Inteligentny_Wozek/wyszukiwanie.py

from grid import GridCellType

class Stan:
    def __init__(self, x, y, kierunek):
        self.x = x
        self.y = y
        self.kierunek = kierunek


class Wezel:
    def __init__(self, stan, rodzic):
        self.stan = stan
        self.rodzic = rodzic

def nastepnik(wezel, licznik, search_grid):
    # 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)

    if wezel.stan.kierunek == 0:
        y -= 70
    elif wezel.stan.kierunek == 1:
        x += 70
    elif wezel.stan.kierunek == 2:
        y += 70
    elif wezel.stan.kierunek == 3:
        x -= 70

    wezly = [obrot_w_prawo, obrot_w_lewo]
    ruch_w_przod = Wezel(Stan(x, y, wezel.stan.kierunek), licznik)

    # 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:
            wezly.append(ruch_w_przod)

    return wezly

def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_grid):
    pierwszy_wezel = Wezel(stan_poczatkowy, 0)
    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)
        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:
                continue
            else:
                fringe.append(obecny_wezel)
                odwiedzone.append(obecny_wezel)
    return pierwszy_wezel
def znajdz_sciezke(wezel):
    sciezka = [wezel]
    index = wezel.rodzic
    while index != 0:
        wezel = mapa[index]
        sciezka.append(wezel)
        index = wezel.rodzic
    sc = list(reversed(sciezka))
    return sc