Dijkstra S Algorithm Matlab Formulation