Fixed KeyNotFoundException when re-prioritizing a node in the frontier

[davidaayers]

I ran into a problem, and I think this fix I'm proposing is the correct solution, but my grasp on the algorithm isn't great yet. This fixed the problem and seems to produce good paths still.

It seems as if it was replacing a Node in the frontier with a new f value in the priority queue, but not changing the f value, so if that node came up again, it couldn't be found in the priority queue.

The exception I was seeing:

KeyNotFoundException: The given key was not present in the dictionary.
System.Collections.Generic.SortedDictionary`2[System.Single,System.Collections.Generic.LinkedList`1[SearchNode`2[MapSquare,Direction]]].get_Item (Single key)
PriorityQueue`2[System.Single,SearchNode`2[MapSquare,Direction]].Replace (.SearchNode`2 value, Single oldPriority, Single newPriority) (at Assets/Scripts/unityutils/pathfinding/PriorityQueue.cs:48)
ShortestPathGraphSearch`2[MapSquare,Direction].GetShortestPath (.MapSquare fromState, .MapSquare toState) (at Assets/Scripts/unityutils/pathfinding/ShortestPathGraphSearch.cs:115)

[davidaayers]

Also, all of the tests still pass. I tried to come up with a simple test that illustrated the but, but couldn't.