public List<Wspolrzedne> obliczSciezke(Wspolrzedne _poczatek, Wspolrzedne _koniec)
{
_poczatek.X = _poczatek.X / SKALA;
_poczatek.Y = _poczatek.Y / SKALA;
_koniec.X = _koniec.X / SKALA;
_koniec.Y = _koniec.Y / SKALA;
if
(
(mapa[_koniec.X, _koniec.Y] == 1)
||
(
(_poczatek.X == _koniec.X)
&&
(_poczatek.Y == _koniec.Y)
)
)
{
return new List<Wspolrzedne>();
}
sciezkaPrzeszukiwana = new List<Wezel>();
Wezel obecny = new Wezel();
obecny.Wspolrzedne.X = _poczatek.X;
obecny.Wspolrzedne.Y = _poczatek.Y;
obecny.Waga = kosztSciezki(obecny) + funkcjaHeurystyczna(_poczatek, _koniec);
sciezkaPrzeszukiwana.Add(obecny);
bool liczDalej = true;//liczDalej = !obliczDzieci(obecny, _koniec);
bool cofa = false;
while (liczDalej)
{
if (obecny.Sprawdzony)
{
obecny = obecny.Ojciec;
if (obecny == null)
{
Console.WriteLine("Nie znaleziono drogi.");
return new List<Wspolrzedne>();
}
cofa = true;
}
else
{
if ((!cofa) && (obecny.Dzieci.Count == 0))
{
liczDalej = !obliczDzieci(obecny, _koniec);
}
cofa = false;
int iloscDzieciSprawdzonych = 0;
for (int dziecko = 0; dziecko < obecny.Dzieci.Count; ++dziecko)
{
if (obecny.Dzieci[dziecko].Sprawdzony)
{
++iloscDzieciSprawdzonych;
}
}
int idNajlepszegoWezla = 0;
for (int najlepszy = 0; najlepszy < sciezkaPrzeszukiwana.Count; ++najlepszy)
{
if
(
(
(sciezkaPrzeszukiwana[idNajlepszegoWezla].Wspolrzedne.X != sciezkaPrzeszukiwana[najlepszy].Wspolrzedne.X)
||
(sciezkaPrzeszukiwana[idNajlepszegoWezla].Wspolrzedne.Y != sciezkaPrzeszukiwana[najlepszy].Wspolrzedne.Y)
)
&&
(
!sciezkaPrzeszukiwana[najlepszy].Sprawdzony
)
)
{
idNajlepszegoWezla = najlepszy;
break;
}
}
for (int najlepszy = 0; najlepszy < sciezkaPrzeszukiwana.Count; ++najlepszy)
{
if (
(
(sciezkaPrzeszukiwana[idNajlepszegoWezla].Wspolrzedne.X != sciezkaPrzeszukiwana[najlepszy].Wspolrzedne.X)
||
(sciezkaPrzeszukiwana[idNajlepszegoWezla].Wspolrzedne.Y != sciezkaPrzeszukiwana[najlepszy].Wspolrzedne.Y)
)
&&
(
!sciezkaPrzeszukiwana[najlepszy].Sprawdzony
)
&&
(
sciezkaPrzeszukiwana[idNajlepszegoWezla].Waga > sciezkaPrzeszukiwana[najlepszy].Waga
)
)
{
idNajlepszegoWezla = najlepszy;
}
}
obecny = sciezkaPrzeszukiwana[idNajlepszegoWezla];
}
};
sciezka = new List<Wspolrzedne>();
for (int wezel = 0; wezel < sciezkaPrzeszukiwana.Count; ++wezel)
{
if (sciezkaPrzeszukiwana[wezel].NalezyDoRozwiazania)
{
sciezka.Add(new Wspolrzedne(sciezkaPrzeszukiwana[wezel].Wspolrzedne.X * SKALA, sciezkaPrzeszukiwana[wezel].Wspolrzedne.Y * SKALA));
}
}
return sciezka;
}
private bool obliczDzieci(Wezel _obecny, Wspolrzedne _warunekStopu)
{
_obecny.Sprawdzony = true;
for (int _kolejny = 0; _kolejny < 8; ++_kolejny)
{
//Pominięcie drogi, którą się przyszło.
if ((_obecny.Ojciec != null) && ((KOLEJNOSC)_kolejny == odwrocRuch(_obecny.OstatniRuch)))
{
continue;
}
// Jeśli ruch w daną stronę natrafia na ścianę.
if (czySciana(_obecny.Wspolrzedne, (KOLEJNOSC)_kolejny))
{
continue;
}
Wezel nowy = new Wezel();
switch ((KOLEJNOSC)_kolejny)
{
case KOLEJNOSC.GORA:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y - 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X;
break;
case KOLEJNOSC.GORA_PRAWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y - 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X + 1;
break;
case KOLEJNOSC.PRAWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X + 1;
break;
case KOLEJNOSC.DOL_PRAWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y + 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X + 1;
break;
case KOLEJNOSC.DOL:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y + 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X;
break;
case KOLEJNOSC.DOL_LEWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y + 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X - 1;
break;
case KOLEJNOSC.LEWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X - 1;
break;
case KOLEJNOSC.GORA_LEWO:
nowy.Wspolrzedne.Y = _obecny.Wspolrzedne.Y - 1;
nowy.Wspolrzedne.X = _obecny.Wspolrzedne.X - 1;
break;
}
// Jeśli nie powiodło się obliczenie współrzędnych nowego węzła.
if ((nowy.Wspolrzedne.X == -1) || (nowy.Wspolrzedne.Y == -1))
{
continue;
}
nowy.Waga = kosztSciezki(_obecny) + funkcjaHeurystyczna(_obecny.Wspolrzedne, _warunekStopu);
nowy.OstatniRuch = obliczRuch(_obecny.Wspolrzedne, nowy.Wspolrzedne);
if (wezelJuzIstnieje(nowy))
{
//Połączenie węzłów ojca i dziecka.
_obecny.Dzieci.Add(nowy);
nowy.Ojciec = _obecny;
sciezkaPrzeszukiwana.Add(nowy);
}
// Sprawdzenie, czy nie odnaleziono rozwiązania.
if ((nowy.Wspolrzedne.X == _warunekStopu.X) && (nowy.Wspolrzedne.Y == _warunekStopu.Y))
{
Wezel licznikSciezki = nowy;
while (licznikSciezki.Ojciec != null)
{
licznikSciezki.NalezyDoRozwiazania = true;
licznikSciezki = licznikSciezki.Ojciec;
}
return true;
}
}
return false;
}
private bool wezelJuzIstnieje(Wezel _nowy)
{
foreach (Wezel _wezel in sciezkaPrzeszukiwana)
{
if ((_nowy.Wspolrzedne.X == _wezel.Wspolrzedne.X) && (_nowy.Wspolrzedne.Y == _wezel.Wspolrzedne.Y))
{
return false;
}
}
return true;
}
private double kosztSciezki(Wezel _obecny)
{
Wezel licznikSciezki = _obecny;
int dlugoscSciezki = 0;
while (licznikSciezki.Ojciec != null)
{
++dlugoscSciezki;
licznikSciezki = licznikSciezki.Ojciec;
}
return dlugoscSciezki;
}