private void PerformStep() { if (openSet.Count == 0) { SetStateToFinished(); return; } currentNode = openSet.Pop(); if (IsPathFound(currentNode)) { HandleFoundPath(currentNode); SetStateToFinished(); return; } List <Node> successors = GetSuccessors(currentNode, DiagonalsEnabled); foreach (Node successor in successors) { successor.MovementCost = currentNode.MovementCost + MovementCost(successor, currentNode); successor.Parent = currentNode; openSet.Add(successor); } openSet.Remove(currentNode); closedSet.Add(currentNode); }