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