private static IEnumerable<Step> GetPathFrom(NodeV1 goalNode1)
{
for (NodeV1 current = goalNode1; current.ParentNode != null && current.PreviousStep != null; current = current.ParentNode)
{
yield return current.PreviousStep.Value;
}
}
private NodeV1(NodeV1 parentNode, Step previousStep, BoardState state, int distanceFromInitialNode, int estimatedDistanceToGoal)
{
this.ParentNode = parentNode;
this.PreviousStep = previousStep;
this.State = state;
this.distanceFromInitialNode = distanceFromInitialNode;
this.Cost = distanceFromInitialNode + estimatedDistanceToGoal;
}
public void Enqueue(NodeV1 node)
{
if (node == null)
{
throw new ArgumentNullException(nameof(node));
}
int newIndex = this.binaryHeap.Count;
this.binaryHeap.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;
}
