private void CalculatePathToGoal()
{
var space = Anchor.Field.Space;
int width = space.Width;
int height = space.Height;
Dictionary <UShortPoint, int> distances = new Dictionary <UShortPoint, int>();
Dictionary <UShortPoint, CardinalMovement> predecessors = new Dictionary <UShortPoint, CardinalMovement>();
List <UShortPoint> unvisited = new List <UShortPoint>();
List <UShortPoint> visited = new List <UShortPoint>();
UShortPoint goalPosition = new UShortPoint((ushort)Goal.Position.X, (ushort)Goal.Position.Y);
for (ushort y = 0; y < height; y++)
{
for (ushort x = 0; x < width; x++)
{
var point = new UShortPoint(x, y);
distances[point] = int.MaxValue;
predecessors[point] = default;
if (space[x, y].IsEmpty || space[x, y].HasAgent)
{
unvisited.Add(point);
}
}
}
void removeUnvisitedMin()
{
int minDistance = int.MaxValue;
UShortPoint minVisited = default;
UShortPoint minUnvisited = default;
CardinalMovement minMove = default;
foreach (UShortPoint visitedPoint in visited)
{
foreach (CardinalMovement move in CardinalMovement.AllExceptNothing)
{
UShortPoint neighbor = visitedPoint.Add(move);
if (unvisited.Contains(neighbor))
{
var neighborField = space[neighbor.X, neighbor.Y];
var distanceOverNeighbor = distances[visitedPoint] + 1 + neighbor.Distance(goalPosition);
if (distanceOverNeighbor < minDistance)
{
minDistance = distanceOverNeighbor;
minVisited = visitedPoint;
minUnvisited = neighbor;
minMove = move;
}
}
}
}
distances[minUnvisited] = distances[minVisited] + 1;
predecessors[minUnvisited] = minMove;
unvisited.Remove(minUnvisited);
visited.Add(minUnvisited);
}
UShortPoint startPosition = new UShortPoint((ushort)Anchor.Field.X, (ushort)Anchor.Field.Y);
distances[startPosition] = 0;
unvisited.Remove(startPosition);
visited.Add(startPosition);
while (visited.Contains(goalPosition) == false)
{
removeUnvisitedMin();
}
while (goalPosition.Equals(startPosition) == false)
{
var action = predecessors[goalPosition];
UShortPoint predecessor = goalPosition.Add(action.Opposite);
PathToGoal.AddFirst(action);
goalPosition = predecessor;
}
}
private void CalculatePathToGoal()
{
var space = Anchor.Field.Space;
int width = space.Width;
int height = space.Height;
Dictionary <UShortPoint, int> distances = new Dictionary <UShortPoint, int>();
Dictionary <UShortPoint, CardinalMovement> predecessors = new Dictionary <UShortPoint, CardinalMovement>();
HashSet <UShortPoint> unvisited = new HashSet <UShortPoint>();
UShortPoint removeUnvisitedMin()
{
int minDistance = int.MaxValue;
UShortPoint minPoint = default;
foreach (UShortPoint point in unvisited)
{
int distance = distances[point];
if (distance < minDistance)
{
minDistance = distance;
minPoint = point;
}
}
unvisited.Remove(minPoint);
return(minPoint);
}
for (ushort y = 0; y < height; y++)
{
for (ushort x = 0; x < width; x++)
{
var point = new UShortPoint(x, y);
distances[point] = int.MaxValue;
predecessors[point] = default;
if (space[x, y].IsEmpty || space[x, y].HasAgent)
{
unvisited.Add(point);
}
}
}
UShortPoint goalPosition = new UShortPoint((ushort)Goal.Position.X, (ushort)Goal.Position.Y);
UShortPoint startPosition = new UShortPoint((ushort)Anchor.Field.X, (ushort)Anchor.Field.Y);
distances[startPosition] = 0;
while (unvisited.Count > 0)
{
UShortPoint currentMin = removeUnvisitedMin();
int currentDistance = distances[currentMin];
if (currentMin == goalPosition)
{
break;
}
foreach (CardinalMovement move in CardinalMovement.All)
{
UShortPoint neighbor = currentMin.Add(move);
var neighborField = space[neighbor.X, neighbor.Y];
if (neighborField.IsEmpty || neighborField.HasAgent)
{
int newDistance = currentDistance + 1;
int neighborDistance = distances[neighbor];
if (newDistance < neighborDistance)
{
distances[neighbor] = newDistance;
predecessors[neighbor] = move;
}
}
}
}
while (goalPosition.Equals(startPosition) == false)
{
var action = predecessors[goalPosition];
UShortPoint predecessor = goalPosition.Add(action.Opposite);
PathToGoal.AddFirst(action);
goalPosition = predecessor;
}
}