public Path FindPath(MapContext map)
{
// List of possible nodes to consider
var openList = new List <AStarNode>();
// List of nodes we've considered
var closedList = new List <AStarNode>();
// Create a node for the starting point
var startNode = new AStarNode()
{
G = 0,
H = CalculateHeuristic(map.StartingPoint, map.TargetPoint),
Parent = null,
Point = map.StartingPoint
};
openList.Add(startNode);
while (openList.Any())
{
var shortestNode = openList.MinBy(n => n.F);
// Move the node to the considered list
closedList.Add(shortestNode);
openList.Remove(shortestNode);
// if the considered list contains the target,
// we've found the shortest path
var targetPointNode = closedList.FirstOrDefault(n => n.Point == map.TargetPoint);
if (targetPointNode != null)
{
return(new Path(
targetPointNode.TraceToStart()
.Reverse()
.ToList()));
}
var adjacent = Adjacent(map, shortestNode.Point);
foreach (var testPoint in adjacent)
{
// Skip this point if its already been considered
if (closedList.Any(n => n.Point == testPoint))
{
continue;
}
var existingNodeForPoint = openList.FirstOrDefault(n => n.Point == testPoint);
if (existingNodeForPoint == null)
{
// If this point is not in the list to consider, we'll
// add it so it can be considered later
openList.Add(new AStarNode()
{
Parent = shortestNode,
G = shortestNode.G + 1,
H = CalculateHeuristic(testPoint, map.TargetPoint),
Point = testPoint
});
}
else
{
// if this point is already in the list to consider,
// we can check to see if the current path to get to this
// node is any better. If its better, we'll replace it in the
// consider list
var potentiallyBetterNode = new AStarNode()
{
G = shortestNode.G + 1,
H = CalculateHeuristic(testPoint, map.TargetPoint),
Parent = shortestNode,
Point = testPoint
};
if (potentiallyBetterNode.F < existingNodeForPoint.F)
{
openList.Remove(existingNodeForPoint);
openList.Add(potentiallyBetterNode);
}
}
}
}
// Couldn't find a path
return(null);
}
private IEnumerable <Point> Adjacent(MapContext map, Point current)
{
return(_possibleAdjacentDeltas
.Select(delta => current.Translate(delta.DeltaX, delta.DeltaY))
.Where(testPoint => map.IsValid(testPoint) && !map.IsObstacle(testPoint)));
}
