125 lines
4.6 KiB
Python
125 lines
4.6 KiB
Python
from collections import deque
|
|
from queue import PriorityQueue
|
|
|
|
from stan_nastepnik import *
|
|
|
|
|
|
class NastepnikZKosztemSciezki:
|
|
def __init__(self, kosztSciezki: int, nastepnik: Nastepnik):
|
|
self.kosztSciezki = kosztSciezki
|
|
self.nastepnik = nastepnik
|
|
|
|
def skopiuj(self):
|
|
return NastepnikZKosztemSciezki(self.kosztSciezki, self.nastepnik.skopiuj())
|
|
|
|
def __lt__(self, other):
|
|
return self.kosztSciezki <= other.kosztSciezki
|
|
|
|
|
|
def goaltest(stan: Stan, cel: Stan):
|
|
if stan.poleStartoweGorne.wiersz != cel.poleStartoweGorne.wiersz:
|
|
return False
|
|
elif stan.poleStartoweGorne.kolumna != cel.poleStartoweGorne.kolumna:
|
|
return False
|
|
else:
|
|
return True
|
|
|
|
|
|
def stan_w_liscie_nastepnikow(stan: Stan, lista_nastepnikow):
|
|
for i in lista_nastepnikow:
|
|
if i.stan.kierunek != stan.kierunek:
|
|
continue
|
|
elif i.stan.poleStartoweGorne.wiersz != stan.poleStartoweGorne.wiersz:
|
|
continue
|
|
elif i.stan.poleStartoweGorne.kolumna != stan.poleStartoweGorne.kolumna:
|
|
continue
|
|
else:
|
|
return True
|
|
return False
|
|
|
|
|
|
def stan_w_kolejce_nastepnikow(stan: Stan, kolejka_nastepnikow: PriorityQueue):
|
|
pom = []
|
|
znaleziono = False
|
|
while not kolejka_nastepnikow.empty():
|
|
element = kolejka_nastepnikow.get()
|
|
pom.append(element)
|
|
nastepnik_z_kosztem_sciezki: NastepnikZKosztemSciezki = element[1]
|
|
n = nastepnik_z_kosztem_sciezki.nastepnik
|
|
if n.stan.kierunek != stan.kierunek:
|
|
continue
|
|
elif n.stan.poleStartoweGorne.wiersz != stan.poleStartoweGorne.wiersz:
|
|
continue
|
|
elif n.stan.poleStartoweGorne.kolumna != stan.poleStartoweGorne.kolumna:
|
|
continue
|
|
else:
|
|
znaleziono = True
|
|
break
|
|
for e in pom:
|
|
kolejka_nastepnikow.put(e)
|
|
return znaleziono
|
|
|
|
|
|
def stos_akcji(stan_koncowy: Nastepnik):
|
|
stos = deque()
|
|
while stan_koncowy.poprzednik is not None:
|
|
stos.append(stan_koncowy.akcja)
|
|
stan_koncowy = stan_koncowy.poprzednik
|
|
return stos
|
|
|
|
|
|
def heurystyka(z: Stan, cel: Stan):
|
|
kroki_w_pionie = abs(cel.poleStartoweGorne.wiersz - z.poleStartoweGorne.wiersz)
|
|
kroki_w_poziomie = abs(cel.poleStartoweGorne.kolumna - z.poleStartoweGorne.kolumna)
|
|
return kroki_w_pionie + kroki_w_poziomie
|
|
|
|
|
|
def koszt_wjechania(z: Stan, do: Stan):
|
|
z_wiersz = z.poleStartoweGorne.wiersz
|
|
z_kolumna = z.poleStartoweGorne.kolumna
|
|
do_wiersz = do.poleStartoweGorne.wiersz
|
|
do_kolumna = do.poleStartoweGorne.kolumna
|
|
krata = z.poleStartoweGorne.krata.krata
|
|
if z_wiersz == do_wiersz and z_kolumna == do_kolumna and z.kierunek == do.kierunek:
|
|
return 0
|
|
elif z_wiersz == do_wiersz and z_kolumna == do_kolumna and z.kierunek != do.kierunek:
|
|
return ZawartoscPolaNaKosztObrotu[krata[do_wiersz][do_kolumna]]
|
|
elif z.kierunek == do.kierunek:
|
|
krok_w_pionie = (abs(z_wiersz - do_wiersz) == 1 and z_kolumna == do_kolumna)
|
|
krok_w_poziomie = (abs(z_kolumna - do_kolumna) == 1 and z_kolumna == z_kolumna)
|
|
if krok_w_pionie or krok_w_poziomie:
|
|
return ZawartoscPolaNaKosztWjechania[krata[do_wiersz][do_kolumna]]
|
|
else:
|
|
raise "Stany nie są połączone."
|
|
|
|
|
|
def priorytet(koszt_sciezki: int, z: Stan, do: Stan, cel: Stan):
|
|
return koszt_sciezki + koszt_wjechania(z, do) + heurystyka(do, cel)
|
|
|
|
|
|
def graphsearch(istate: Stan, cel: Stan):
|
|
fringe = PriorityQueue()
|
|
explored = []
|
|
pom = NastepnikZKosztemSciezki(0, Nastepnik(None, istate, None))
|
|
fringe.put((priorytet(0, istate, istate, cel), pom))
|
|
# fringe.append(Nastepnik(None, istate, None))
|
|
while not fringe.empty():
|
|
# for i in fringe:
|
|
# print("F",i.stan.kierunek,i.stan.poleStartoweGorne.wiersz,i.stan.poleStartoweGorne.kolumna,end=" ")
|
|
# print()
|
|
element: NastepnikZKosztemSciezki = fringe.get()[1]
|
|
koszt_sciezki = element.kosztSciezki
|
|
nastepnik = element.nastepnik
|
|
if goaltest(nastepnik.stan, cel):
|
|
return stos_akcji(nastepnik)
|
|
explored.append(nastepnik)
|
|
for nowy in succ(nastepnik):
|
|
if not stan_w_kolejce_nastepnikow(nowy.stan, fringe) and not stan_w_liscie_nastepnikow(nowy.stan, explored):
|
|
z = nastepnik.stan
|
|
do = nowy.stan
|
|
pom = NastepnikZKosztemSciezki(koszt_sciezki + koszt_wjechania(z, do), nowy)
|
|
pom2 = (priorytet(koszt_sciezki, z, do, cel), pom)
|
|
fringe.put(pom2)
|
|
# print("dodano",pom.nastepnik.stan.kierunek,pom.nastepnik.stan.poleStartoweGorne.wiersz,pom.nastepnik.stan.poleStartoweGorne.kolumna)
|
|
return False
|