Use Dijkstra's algorithm. Based on patches from Max Rijevskiy.