public int CompareTo([AllowNull] AStarNode <T> other)
{
return(Compare(this, other));
}
/// <summary>
/// Uses the A* pathfinding algorithm to find a path between the
/// <paramref name="startPoint"/> and <paramref name="endPoint"/>.
/// </summary>
/// <typeparam name="T"></typeparam>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="heuristic"></param>
/// <returns></returns>
public static PathResult <T> GetPath <T>(
T startPoint,
T endPoint,
Func <T, int> Heuristic,
Func <T, IList <T> > GetNeighbors,
Func <T, T, int> GetEdgeCost)
{
// Pseudocode found here:
// https://brilliant.org/wiki/a-star-search/
// make an openlist containing only the starting node
// make an empty closed list
// while (the destination node has not been reached):
// consider the node with the lowest f score in the open list
// if (this node is our destination node) :
// we are finished
// if not:
// put the current node in the closed list and look at all of its neighbors
// for (each neighbor of the current node):
// if (neighbor has lower g value than current and is in the closed list) :
// replace the neighbor with the new, lower, g value
// current node is now the neighbor's parent
// else if (current g value is lower and this neighbor is in the open list ) :
// replace the neighbor with the new, lower, g value
// change the neighbor's parent to our current node
//
// else if this neighbor is not in both lists:
// add it to the open list and set its g
var startNode = new AStarNode <T>(startPoint, 0, Heuristic(startPoint));
int startPointHScore = Heuristic(startPoint);
var openSet = new SortedDictionary <int, HashSet <T> >();
openSet.Add(startPointHScore, new HashSet <T>()
{
startPoint
});
var currentFScores = new Dictionary <T, int>()
{
{ startPoint, startPointHScore }
};
var cameFrom = new Dictionary <T, T>();
var gScore = new Dictionary <T, int>()
{
{ startPoint, 0 }
};
while (openSet.Count > 0)
{
var lowestFScore = openSet.Keys.First();
var current = openSet[lowestFScore].First();
if (endPoint.Equals(current))
{
var result = PathResult <T> .ReconstructPath(current, cameFrom, gScore);
return(result);
}
openSet[lowestFScore].Remove(current);
if (openSet[lowestFScore].Count == 0)
{
openSet.Remove(lowestFScore);
}
currentFScores.Remove(current);
var neighbors = GetNeighbors(current);
foreach (var neighbor in neighbors)
{
if (!gScore.ContainsKey(neighbor))
{
gScore.Add(neighbor, int.MaxValue);
}
var neighborGScore = gScore[current] + GetEdgeCost(current, neighbor);
if (neighborGScore < gScore[neighbor])
{
if (!cameFrom.ContainsKey(neighbor))
{
cameFrom.Add(neighbor, current);
}
cameFrom[neighbor] = current;
gScore[neighbor] = neighborGScore;
var neighborFScore = gScore[neighbor] + Heuristic(neighbor);
if (currentFScores.ContainsKey(neighbor))
{
int currentNeighborFScore = currentFScores[neighbor];
if (currentNeighborFScore != neighborFScore)
{
openSet[currentNeighborFScore].Remove(neighbor);
if (openSet[currentNeighborFScore].Count == 0)
{
openSet.Remove(currentNeighborFScore);
}
}
currentFScores[neighbor] = neighborFScore;
}
else
{
currentFScores.Add(neighbor, neighborFScore);
}
if (!openSet.ContainsKey(neighborFScore))
{
openSet.Add(neighborFScore, new HashSet <T>());
}
if (!openSet[neighborFScore].Contains(neighbor))
{
openSet[neighborFScore].Add(neighbor);
}
if (!currentFScores.ContainsKey(neighbor))
{
currentFScores.Add(neighbor, neighborFScore);
}
}
}
}
return(new PathResult <T>(new List <T>(), 0));
}
public int Compare(AStarNode <T> x, AStarNode <T> y)
{
return(x.FScore - y.FScore);
}
