Fixed Dijkstra Optimization

...destination is not the same as initial