Two different issues were causing a path search to not explore cells in order of the cheapest estimated route first. This meant the search could sometimes miss a cheaper route and return a suboptimal path.
- PriorityQueue had a bug which would cause it to not check some elements when restoring the heap property of its internal data structure. Failing to do this would invalidate the heap property, meaning it would not longer return the items in correct priority order. Additional tests ensure this is covered.
- When a path search encountered the same cell again with a lower cost, it would not update the priority queue with the new cost. This meant the cell was not explored early enough as it was in the queue with its original, higher cost. Exploring other paths might close off surrounding cells, preventing the cell with the lower cost from progressing. Instead we now add a duplicate with the lower cost to ensure it gets explored at the right time. We remove the duplicate with the higher cost in CanExpand by checking for already Closed cells.
