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; } }