Implement Global Path Plan Finding

[elvout]

Due to time constraints I think we should stick with familiar graph search algorithms.

Dijkstra's Algorithm with Lazy Relaxation

Decrease-key operations are cumbersome to implement correctly (as log n), so maintain a vertex-cost mapping alongside the (binary heap) priority queue to prune extraneous vertices popped from the priority queue.

A*

Manhattan distance should be an admissible heuristic and it's also fast to compute.

[elvout]

The "octile" distance should be used for the A* heuristic instead, since the diagonal distance exists.