private Point exploreFrontier(PriorityQueue <Point> frontier, Point target, Dictionary <Point, Point> cameFrom, Dictionary <Point, double> costSoFar) { Point closestTile = new Point(); Point currentTile; double closestDistance = double.PositiveInfinity; while (frontier.Count > 0) { currentTile = frontier.Dequeue(); double currentDistance = Math.Sqrt( Math.Abs(currentTile.X - target.X) + Math.Abs(currentTile.Y - target.Y)); if (currentDistance < closestDistance) { closestDistance = currentDistance; closestTile = currentTile; } if (closestTile == target) { break; } List <Point> neighbors = gameMap.GetValidNeighbors(currentTile); foreach (Point neighborTile in neighbors) { GameTile tileObject = gameMap.game_tiles[(int)neighborTile.Y, (int)neighborTile.X]; if (tileObject.IsPathable()) { double tileModifier = 1.0; double distanceFromTarget = Math.Sqrt( Math.Abs(tileObject.Coordinates.X - target.X) + Math.Abs(tileObject.Coordinates.Y - target.Y)); double newCost = (costSoFar[currentTile] + tileModifier) + distanceFromTarget; if (!cameFrom.ContainsKey(neighborTile) || newCost < costSoFar[currentTile]) { cameFrom[neighborTile] = currentTile; costSoFar[neighborTile] = newCost; frontier.Enqueue(neighborTile, newCost); } } } } return(closestTile); }
public List <Point> GetPath(Point start, Point target) { List <Point> path = new List <Point>(); Dictionary <Point, Point> cameFrom = new Dictionary <Point, Point>(); Dictionary <Point, double> costSoFar = new Dictionary <Point, double>(); Point closestTile = new Point(); cameFrom[start] = start; costSoFar[start] = 0; PriorityQueue <Point> frontier = new PriorityQueue <Point>(); List <Point> neighbors = gameMap.GetValidNeighbors(start); foreach (Point tile in neighbors) { GameTile tileObject = gameMap.game_tiles[(int)tile.Y, (int)tile.X]; if (tileObject.IsPathable()) { frontier.Enqueue(start, 0); } } closestTile = exploreFrontier( frontier, target, cameFrom, costSoFar); List <Point> newPathSteps = new List <Point>(); newPathSteps.Add(closestTile); while (true) { Point nextTile = newPathSteps[0]; if (nextTile == start) { break; } newPathSteps.Insert(0, cameFrom[nextTile]); } return(newPathSteps); }