forked from s474139/Inteligentny_Wozek
adding a*
This commit is contained in:
parent
895c9b3569
commit
d8b997126d
5
main.py
5
main.py
@ -24,8 +24,9 @@ def main():
|
|||||||
y = pygame.mouse.get_pos()[1]
|
y = pygame.mouse.get_pos()[1]
|
||||||
wiersz = ekran.sprawdz_ktory_wiersz(x)
|
wiersz = ekran.sprawdz_ktory_wiersz(x)
|
||||||
kolumna = ekran.sprawdz_ktora_kolumna(y)
|
kolumna = ekran.sprawdz_ktora_kolumna(y)
|
||||||
docelowy_stan = wyszukiwanie.Stan(wiersz * 70, kolumna * 70, 0)
|
docelowy_stan = wyszukiwanie.Stan(wiersz * 70, kolumna * 70, 1)
|
||||||
wezel = wyszukiwanie.wyszukiwanie_bfs(wozek.obecnyStan, docelowy_stan, grid_points)
|
# 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)
|
sciezka = wyszukiwanie.znajdz_sciezke(wezel)
|
||||||
wozek.przemiesc_wozek_po_sciezce(sciezka)
|
wozek.przemiesc_wozek_po_sciezce(sciezka)
|
||||||
|
|
||||||
|
135
wyszukiwanie.py
135
wyszukiwanie.py
@ -1,4 +1,6 @@
|
|||||||
from grid import GridCellType
|
from grid import GridCellType
|
||||||
|
import heapq
|
||||||
|
|
||||||
|
|
||||||
class Stan:
|
class Stan:
|
||||||
def __init__(self, x, y, kierunek):
|
def __init__(self, x, y, kierunek):
|
||||||
@ -6,21 +8,38 @@ class Stan:
|
|||||||
self.y = y
|
self.y = y
|
||||||
self.kierunek = kierunek
|
self.kierunek = kierunek
|
||||||
|
|
||||||
|
def __eq__(self, other):
|
||||||
|
return self.x == other.x and self.y == other.y and self.kierunek == other.kierunek
|
||||||
|
|
||||||
|
|
||||||
class Wezel:
|
class Wezel:
|
||||||
def __init__(self, stan, rodzic):
|
def __init__(self, stan, waga=0, g=0, h=0, rodzic=None, ):
|
||||||
self.stan = stan
|
self.stan = stan
|
||||||
|
self.waga = waga
|
||||||
|
self.g = g # koszt dotarcia do wezla
|
||||||
|
self.h = h # heurystyka
|
||||||
self.rodzic = rodzic
|
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 -> prawo -> dol -> lewo | obrot w prawo
|
||||||
# gora -> lewo -> dol -> prawo | obrot w lewo
|
# gora -> lewo -> dol -> prawo | obrot w lewo
|
||||||
# 0 gora 1 prawo 2 dol 3 lewo
|
# 0 gora 1 prawo 2 dol 3 lewo
|
||||||
x = wezel.stan.x
|
x = wezel.stan.x
|
||||||
y = wezel.stan.y
|
y = wezel.stan.y
|
||||||
obrot_w_prawo = Wezel(Stan(x, y, (wezel.stan.kierunek + 1) % 4), licznik)
|
obrot_w_prawo = Wezel(Stan(x, y, (wezel.stan.kierunek + 1) % 4))
|
||||||
obrot_w_lewo = Wezel(Stan(x, y, 3 if wezel.stan.kierunek == 0 else wezel.stan.kierunek - 1), licznik)
|
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:
|
if wezel.stan.kierunek == 0:
|
||||||
y -= 70
|
y -= 70
|
||||||
elif wezel.stan.kierunek == 1:
|
elif wezel.stan.kierunek == 1:
|
||||||
@ -31,51 +50,103 @@ def nastepnik(wezel, licznik, search_grid):
|
|||||||
x -= 70
|
x -= 70
|
||||||
|
|
||||||
wezly = [obrot_w_prawo, obrot_w_lewo]
|
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
|
# sprawdzenie czy nie wyjdzie poza plansze
|
||||||
if 0 <= x <= 910 and 0 <= y <= 910:
|
if 0 <= x <= 910 and 0 <= y <= 910:
|
||||||
|
if ktory_algorytm == 1:
|
||||||
x1 = x / 70
|
x1 = x / 70
|
||||||
y1 = y / 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)
|
wezly.append(ruch_w_przod)
|
||||||
|
else:
|
||||||
|
wezly.append(ruch_w_przod)
|
||||||
return wezly
|
return wezly
|
||||||
|
|
||||||
|
|
||||||
def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_grid):
|
def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_grid):
|
||||||
pierwszy_wezel = Wezel(stan_poczatkowy, 0)
|
pierwszy_wezel = Wezel(stan_poczatkowy)
|
||||||
fringe = [pierwszy_wezel]
|
fringe = [pierwszy_wezel]
|
||||||
odwiedzone = [pierwszy_wezel]
|
odwiedzone = [pierwszy_wezel]
|
||||||
licznik = 0
|
|
||||||
global mapa
|
|
||||||
mapa = dict()
|
|
||||||
while fringe:
|
while fringe:
|
||||||
wezel = fringe.pop(0)
|
wezel = fringe.pop(0)
|
||||||
licznik = licznik + 1
|
|
||||||
mapa[licznik] = wezel
|
|
||||||
if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
|
if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
|
||||||
return wezel
|
return wezel
|
||||||
lista1 = nastepnik(wezel, licznik, search_grid)
|
lista1 = znajdz_nastepcow(wezel, search_grid, 1)
|
||||||
for obecny_wezel in lista1:
|
for obecny_wezel in lista1:
|
||||||
flag_juz_odwiedzony = 0
|
if obecny_wezel in odwiedzone:
|
||||||
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
|
continue
|
||||||
else:
|
|
||||||
fringe.append(obecny_wezel)
|
fringe.append(obecny_wezel)
|
||||||
odwiedzone.append(obecny_wezel)
|
odwiedzone.append(obecny_wezel)
|
||||||
return pierwszy_wezel
|
return None
|
||||||
|
|
||||||
|
|
||||||
def znajdz_sciezke(wezel):
|
def znajdz_sciezke(wezel):
|
||||||
sciezka = [wezel]
|
sciezka = []
|
||||||
index = wezel.rodzic
|
while wezel:
|
||||||
while index != 0:
|
|
||||||
wezel = mapa[index]
|
|
||||||
sciezka.append(wezel)
|
sciezka.append(wezel)
|
||||||
index = wezel.rodzic
|
wezel = wezel.rodzic
|
||||||
sc = list(reversed(sciezka))
|
sciezka.reverse()
|
||||||
return sc
|
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
|
||||||
|
Loading…
Reference in New Issue
Block a user