public List <Pole> AGwiazdka(int x, int y)
{
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{
file.WriteLine("Log Trasy traktora:");
file.WriteLine("Pozycja startowa x:" + pozycja.x.ToString() + " y:" + pozycja.y.ToString());
file.WriteLine("Pozycja docelowa x:" + x.ToString() + " y:" + y.ToString());
}
Pole PoleX = new Pole(-1, -1, 1000);
Pole PoleTemp = new Pole(-1, -1, 1000);
List <Pole> trasa = new List <Pole>();//this.pozycja
int tentGScore;
bool tentIsBetter;
List <Pole> closedSet = new List <Pole>();
List <Pole> openset = new List <Pole>();
List <Pole> xNeighbor = new List <Pole>();
openset.Add(this.pozycja);
//muszo mieć (rozmiar + 1) na (rozmiar + 1)
int[,] cameFrom = new int[11, 11];
for (int i = 0; i < 11; i++)
{
for (int j = 0; j < 11; j++)
{
cameFrom[i, j] = -1;
}
}
cameFrom[pozycja.x, pozycja.y] = 0;
int[,] gScore = new int[11, 11];
int[,] fScore = new int[11, 11];
fScore[pozycja.x, pozycja.y] = 0;
int[,] hScore = new int[11, 11];
gScore[pozycja.x, pozycja.y] = 0;
while (openset.Count != 0)
{
PoleX = openset.Find(a => a.koszt != null);
foreach (var p in openset)
{
if (fScore[p.x, p.y] < PoleX.koszt)
{
PoleX = p;
}
}
if (PoleX.x == x && PoleX.y == y)
{
trasa = reconstructPath(cameFrom, PoleX, trasa);
break;
//return trasa;
}
openset.Remove(PoleX);
if (!closedSet.Contains(PoleX))
{
closedSet.Add(PoleX);
}
xNeighbor.RemoveAll(a => a.koszt != null);
if (ALeft(PoleX) != null)
{
PoleTemp = ALeft(PoleX);
xNeighbor.Add(PoleTemp);
}
if (ARight(PoleX) != null)
{
PoleTemp = ARight(PoleX);
xNeighbor.Add(PoleTemp);
}
if (AUp(PoleX) != null)
{
PoleTemp = AUp(PoleX);
xNeighbor.Add(PoleTemp);
}
if (ADown(PoleX) != null)
{
PoleTemp = ADown(PoleX);
xNeighbor.Add(PoleTemp);
}
foreach (var p in xNeighbor)
{
if (closedSet.Exists(a => a.x == p.x & a.y == p.y & a.koszt == p.koszt))
{
continue;
}
tentGScore = gScore[PoleX.x, PoleX.y] + p.koszt;
tentIsBetter = false;
if (!openset.Exists(a => a.x == p.x & a.y == p.y & a.koszt == p.koszt))
{
openset.Add(p);
hScore[p.x, p.y] = Math.Abs(p.x - x) + Math.Abs(p.y - y);
tentIsBetter = true;
}
else if (tentGScore < gScore[p.x, p.y])
{
tentIsBetter = true;
}
if (tentIsBetter)
{
//trasa.DodajNaKoniec(PoleX);
cameFrom[p.x, p.y] = PoleX.y * 10 + PoleX.x;
gScore[p.x, p.y] = tentGScore;
fScore[p.x, p.y] = gScore[p.x, p.y] + Math.Abs(p.x - x) + Math.Abs(p.y - y);
}
}
}
for (int i = 0; i < trasa.Count - 1; i++)
{
if (trasa[i + 1].y == trasa[i].y && trasa[i + 1].x == trasa[i].x - 1)
{
trasa[i + 1].kierunekDo = 4;
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{ file.WriteLine("zachód " + "Pozycja x:" + trasa[i + 1].x.ToString() + " y:" + trasa[i + 1].y.ToString() + " Koszt: " + trasa[i + 1].koszt); }
}
else if (trasa[i + 1].y == trasa[i].y && trasa[i + 1].x == trasa[i].x + 1)
{
trasa[i + 1].kierunekDo = 6;
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{ file.WriteLine("wschód " + "Pozycja x:" + trasa[i + 1].x.ToString() + " y:" + trasa[i + 1].y.ToString() + " Koszt: " + trasa[i + 1].koszt); }
}
else if (trasa[i + 1].y == trasa[i].y - 1 && trasa[i + 1].x == trasa[i].x)
{
trasa[i + 1].kierunekDo = 8;
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{ file.WriteLine("północ " + "Pozycja x:" + trasa[i + 1].x.ToString() + " y:" + trasa[i + 1].y.ToString() + " Koszt: " + trasa[i + 1].koszt); }
}
else if (trasa[i + 1].y == trasa[i].y + 1 && trasa[i + 1].x == trasa[i].x)
{
trasa[i + 1].kierunekDo = 2;
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{ file.WriteLine("południe " + "Pozycja x:" + trasa[i + 1].x.ToString() + " y:" + trasa[i + 1].y.ToString() + " Koszt: " + trasa[i + 1].koszt); }
}
else
{
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{
file.WriteLine("BŁĄD TRASOWANIA - PRZERYWAM LOGOWANIE");
file.WriteLine("(Log błędu pozycja aktualna: x " + trasa[i + 1].x.ToString() + " y " + trasa[i + 1].y.ToString() + "pozycja poprzednia: x " + trasa[i].x.ToString() + " y " + trasa[i].y.ToString());
}
break;
}
}
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"..\..\Logs\TrasaLog.txt", true))
{
file.WriteLine("Koniec trasy -> Osiągnięto pozycję końcową(?)");
file.WriteLine("***");
}
return(trasa);
}