JFO_lab_skrzyzowanie/a_gwiazdka.py

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