%% function path = dijkstra(src, dst, D, n)%% Arguments:% src, dst  - numbers of src and dst nodes% D 		   - an array of nlx3 where D(i,1) is the source of ith link, D(i,2) is the destination and D(i,3) is the length.% n		   - number of nodes (vertices) in the graph.%% Returns:% path	   - a set of nodes npx1 to pass in order to go from src to dst (starting with src and enduing with dst).%% Note: All links are assumed bi-directional, hence D(i,j) and D(j,i) don't have to be entered both % into the D matrice.%% See http://ciips.ee.uwa.edu.au/~morris/Year2/PLDS210/dijkstra.html for detailsfunction path = dijkstra(src, dst, D, n)d = zeros(n,1) + Inf;pi = zeros(n,1);d(src) = 0;S = [];        % Make S empty Q = (1:n)';while size(Q,2) > 0	[m,i] = min(d(Q));	u = Q(i);	S = [S, u];	Q = [Q(1:i-1); Q(i+1:size(Q,1))];	for j=1:size(D,1)		if D(j,1) == u			v = D(j,2);			if d(v) > d(u) + D(j,3)				d(v) = d(u) + D(j,3);				pi(v) = u;			end        elseif D(j,2) == u			v = D(j,1);			if d(v) > d(u) + D(j,3)				d(v) = d(u) + D(j,3);				pi(v) = u;			end		end    endendpath = dst;node = dst;while node ~= src	path = [pi(node); path];	node = pi(node);end