public void Enqueue(NodeV5 node)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
int newIndex = this.binaryHeap.Count;
this.binaryHeap.Add(node);
this.hashSet.Add(node);
while (newIndex > 0)
{
int parentIndex = (newIndex - 1) / 2;
if (this.binaryHeap[parentIndex].Cost <= node.Cost)
{
break;
}
this.binaryHeap[newIndex] = this.binaryHeap[parentIndex];
newIndex = parentIndex;
}
this.binaryHeap[newIndex] = node;
}
private static IEnumerable<Step> GetPathFrom(NodeV5 goalNode1)
{
for (NodeV5 current = goalNode1; current.ParentNode != null && current.PreviousStep != null; current = current.ParentNode)
{
yield return current.PreviousStep.Value;
}
}
public bool Contains(NodeV5 node)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
return this.hashSet.Contains(node);
}
