Inteligentny_Wozek/wyszukiwanie.py

153 lines
4.5 KiB
Python
Raw Normal View History

2023-05-06 18:06:30 +02:00
from grid import GridCellType
2023-05-14 15:21:09 +02:00
import heapq
2023-04-26 18:59:38 +02:00
2023-04-20 20:54:58 +02:00
class Stan:
def __init__(self, x, y, kierunek):
self.x = x
self.y = y
self.kierunek = kierunek
2023-05-14 15:21:09 +02:00
def __eq__(self, other):
return self.x == other.x and self.y == other.y and self.kierunek == other.kierunek
2023-04-26 18:59:38 +02:00
2023-04-20 20:54:58 +02:00
class Wezel:
2023-05-14 15:21:09 +02:00
def __init__(self, stan, waga=0, g=0, h=0, rodzic=None, ):
2023-04-20 20:54:58 +02:00
self.stan = stan
2023-05-14 15:21:09 +02:00
self.waga = waga
self.g = g # koszt dotarcia do wezla
self.h = h # heurystyka
2023-05-06 13:53:01 +02:00
self.rodzic = rodzic
2023-05-14 15:21:09 +02:00
self.f = g + h # koszt calkowity
def __lt__(self, other):
return self.f < other.f
def __eq__(self, other):
return self.stan == other.stan
2023-04-20 20:54:58 +02:00
2023-05-14 15:21:09 +02:00
def znajdz_nastepcow(wezel, search_grid, ktory_algorytm):
2023-04-26 18:59:38 +02:00
# gora -> prawo -> dol -> lewo | obrot w prawo
2023-04-20 20:54:58 +02:00
# gora -> lewo -> dol -> prawo | obrot w lewo
# 0 gora 1 prawo 2 dol 3 lewo
2023-05-06 13:53:01 +02:00
x = wezel.stan.x
y = wezel.stan.y
2023-05-14 15:21:09 +02:00
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
2023-04-20 20:54:58 +02:00
if wezel.stan.kierunek == 0:
2023-05-06 13:53:01 +02:00
y -= 70
2023-04-20 20:54:58 +02:00
elif wezel.stan.kierunek == 1:
2023-05-06 13:53:01 +02:00
x += 70
2023-04-20 20:54:58 +02:00
elif wezel.stan.kierunek == 2:
2023-05-06 13:53:01 +02:00
y += 70
2023-04-20 20:54:58 +02:00
elif wezel.stan.kierunek == 3:
2023-05-06 13:53:01 +02:00
x -= 70
2023-04-20 20:54:58 +02:00
wezly = [obrot_w_prawo, obrot_w_lewo]
2023-05-14 15:21:09 +02:00
ruch_w_przod = Wezel(Stan(x, y, wezel.stan.kierunek))
ruch_w_przod.rodzic = wezel
2023-04-20 20:54:58 +02:00
# sprawdzenie czy nie wyjdzie poza plansze
2023-05-06 13:53:01 +02:00
if 0 <= x <= 910 and 0 <= y <= 910:
2023-05-14 15:21:09 +02:00
if ktory_algorytm == 1:
x1 = x / 70
y1 = y / 70
2023-05-29 00:13:02 +02:00
if search_grid.grid[(x1, y1)] is GridCellType.FREE or search_grid.grid[(x1, y1)] is GridCellType.PLACE:
2023-05-14 15:21:09 +02:00
wezly.append(ruch_w_przod)
else:
2023-05-06 18:06:30 +02:00
wezly.append(ruch_w_przod)
2023-04-20 20:54:58 +02:00
return wezly
2023-05-14 15:21:09 +02:00
2023-05-06 18:06:30 +02:00
def wyszukiwanie_bfs(stan_poczatkowy, stan_docelowy, search_grid):
2023-05-14 15:21:09 +02:00
pierwszy_wezel = Wezel(stan_poczatkowy)
2023-04-20 20:54:58 +02:00
fringe = [pierwszy_wezel]
2023-05-06 13:53:01 +02:00
odwiedzone = [pierwszy_wezel]
2023-04-20 20:54:58 +02:00
while fringe:
wezel = fringe.pop(0)
if stan_docelowy.x == wezel.stan.x and stan_docelowy.y == wezel.stan.y:
2023-05-06 13:53:01 +02:00
return wezel
2023-05-14 15:21:09 +02:00
lista1 = znajdz_nastepcow(wezel, search_grid, 1)
2023-05-06 13:53:01 +02:00
for obecny_wezel in lista1:
2023-05-14 15:21:09 +02:00
if obecny_wezel in odwiedzone:
2023-04-26 18:59:38 +02:00
continue
2023-05-14 15:21:09 +02:00
fringe.append(obecny_wezel)
odwiedzone.append(obecny_wezel)
return None
2023-05-06 13:53:01 +02:00
def znajdz_sciezke(wezel):
2023-05-14 15:21:09 +02:00
sciezka = []
while wezel:
2023-05-06 13:53:01 +02:00
sciezka.append(wezel)
2023-05-14 15:21:09 +02:00
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
2023-05-29 00:13:02 +02:00
# print(index)
2023-05-14 15:21:09 +02:00
# 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:
2023-05-29 00:13:02 +02:00
wezel.waga = 1
2023-05-14 15:21:09 +02:00
return None