Added an optimal path function

Using Dijkstra's. UNTESTED!