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