Dijkstra algorithm matlab program