Path GetPathFromNodeParents(EntitySpot v)
{
Path path = new Path();
path.spots.Add(v);
while (v.parent != null)
{
path.spots.Add(v.parent);
v = v.parent;
}
return(path);
}
public Path FindPath(EntitySpot start, EntitySpot end, List <List <EntitySpot> > matrix)
{
int ofst = 0;// MainWindow.pointOffset;
Queue <EntitySpot> queue = new Queue <EntitySpot>();
queue.Enqueue(start);
while (queue.Count > 0)
{
var v = queue.Dequeue();
v.IsVisited = true;
int startX = v.X / MainWindow.fieldSize;
int startY = v.Y / MainWindow.fieldSize;
if (v == end)
{
return(GetPathFromNodeParents(v));
}
if (startX + ofst + 1 < 400)
{
var n = matrix[startX + ofst + 1][startY];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startX + ofst - 1 >= 0 && startX + ofst - 1 < 400)
{
var n = matrix[startX + ofst - 1][startY];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startY + ofst + 1 < 400)
{
var n = matrix[startX][startY + ofst + 1];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startY + ofst - 1 >= 0)
{
var n = matrix[startX][startY + ofst - 1];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
}
return(new Path());
}
public static EntitySpot GetClosestEmptySpot(EntitySpot start, List <List <EntitySpot> > matrix)
{
for (int i = 0; i < matrix.Count; i++)
{
for (int j = 0; j < matrix[i].Count; j++)
{
matrix[i][j].IsVisited = false;
matrix[i][j].parent = null;
}
}
Queue <EntitySpot> queue = new Queue <EntitySpot>();
queue.Enqueue(start);
while (queue.Count > 0)
{
var v = queue.Dequeue();
v.IsVisited = true;
int startX = v.X / MainWindow.fieldSize;
int startY = v.Y / MainWindow.fieldSize;
if (v.Entities.Count == 0)
{
return(v);
}
if (startX + 1 < 400)
{
var n = matrix[startX + 1][startY];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startX - 1 >= 0 && startX - 1 < 400)
{
var n = matrix[startX - 1][startY];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startY + 1 < 400)
{
var n = matrix[startX][startY + 1];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
if (startY - 1 >= 0)
{
var n = matrix[startX][startY - 1];
if (n.IsOccupied == false && n.IsVisited == false)
{
n.IsVisited = true;
n.parent = v;
queue.Enqueue(n);
}
}
}
return(null);
}
