public void GlobalSetup()
{
Random rng = new Random(34829061);
this.fastBinaryHeap = new FastBinaryHeap <HeapNode>();
this.lifoBinaryHeap = new LifoBinaryHeap <HeapNode>();
this.priorityQueueWithSortedDictionary = new PriorityQueueWithSortedDictionary <HeapNode, float>();
this.nodes = new HeapNode[this.HeapSize];
for (int i = 0; i < this.HeapSize; i++)
{
int value = rng.Next(16777216);
this.nodes[i] = new HeapNode(value);
}
}
/// <summary>
/// Computes a path between the two specified nodes.
/// </summary>
/// <param name="startNode">The start node.</param>
/// <param name="endNode">The end node.</param>
/// <returns>A path from the start node to the end node.</returns>
public AStarPath FindPath(AStarNode startNode, AStarNode endNode)
{
if (startNode is null || endNode is null)
{
return(null);
}
startNode.GCost = 0;
startNode.HCost = AStarGraph.Heuristic(startNode, endNode);
startNode.PreviousNode = null;
// The set of discovered nodes that may need to be expanded. Initially, only the start
// node is known.
FastBinaryHeap <AStarNode> openSet = new FastBinaryHeap <AStarNode>();
openSet.Push(startNode);
// The set of nodes already expanded.
HashSet <AStarNode> closedSet = new HashSet <AStarNode>();
while (!openSet.IsEmpty())
{
AStarNode currentNode = openSet.Pop();
if (currentNode == endNode)
{
return(new AStarPath(startNode, endNode));
}
closedSet.Add(currentNode);
foreach (AStarNode neighbour in currentNode.GetNeighbours())
{
if (closedSet.Contains(neighbour))
{
continue;
}
int gCost = currentNode.GCost + 1;
bool visited = openSet.Contains(neighbour);
if (gCost < neighbour.GCost || !visited)
{
// This path to neighbour is better than any previous one. Record it!
neighbour.GCost = gCost;
neighbour.HCost = AStarGraph.Heuristic(neighbour, endNode);
neighbour.PreviousNode = currentNode;
if (visited)
{
openSet.Heapify(neighbour);
}
else
{
openSet.Push(neighbour);
}
}
}
}
// Open set is empty but goal was never reached.
return(null);
}
