In GitHub I found the following:
function [dist,path] = dijkstra(nodes,segments,start_id,finish_id)
%DIJKSTRA Calculates the shortest distance and path between points on a map
% using Dijkstra's Shortest Path Algorithm
%
% [DIST, PATH] = DIJKSTRA(NODES, SEGMENTS, SID, FID)
% Calculates the shortest distance and path between start and finish nodes SID and FID
%
% [DIST, PATH] = DIJKSTRA(NODES, SEGMENTS, SID)
% Calculates the shortest distances and paths from the starting node SID to all
% other nodes in the map
%
% Note:
% DIJKSTRA is set up so that an example is created if no inputs are provided,
% but ignores the example and just processes the inputs if they are given.
%
% Inputs:
% NODES should be an Nx3 or Nx4 matrix with the format [ID X Y] or [ID X Y Z]
% where ID is an integer, and X, Y, Z are cartesian position coordinates)
% SEGMENTS should be an Mx3 matrix with the format [ID N1 N2]
% where ID is an integer, and N1, N2 correspond to node IDs from NODES list
% such that there is an [undirected] edge/segment between node N1 and node N2
% SID should be an integer in the node ID list corresponding with the starting node
% FID (optional) should be an integer in the node ID list corresponding with the finish
%
% Outputs:
% DIST is the shortest Euclidean distance
% If FID was specified, DIST will be a 1x1 double representing the shortest
% Euclidean distance between SID and FID along the map segments. DIST will have
% a value of INF if there are no segments connecting SID and FID.
% If FID was not specified, DIST will be a 1xN vector representing the shortest
% Euclidean distance between SID and all other nodes on the map. DIST will have
% a value of INF for any nodes that cannot be reached along segments of the map.
% PATH is a list of nodes containing the shortest route
% If FID was specified, PATH will be a 1xP vector of node IDs from SID to FID.
% NAN will be returned if there are no segments connecting SID to FID.
% If FID was not specified, PATH will be a 1xN cell of vectors representing the
% shortest route from SID to all other nodes on the map. PATH will have a value
% of NAN for any nodes that cannot be reached along the segments of the map.
%
% Example:
% dijkstra; % calculates shortest path and distance between two nodes
% % on a map of randomly generated nodes and segments
%
% Example:
% nodes = [(1:10); 100*rand(2,10)]';
% segments = [(1:17); floor(1:0.5:9); ceil(2:0.5:10)]';
% figure; plot(nodes(:,2), nodes(:,3),'k.');
% hold on;
% for s = 1:17
% if (s <= 10) text(nodes(s,2),nodes(s,3),[' ' num2str(s)]); end
% plot(nodes(segments(s,2:3)',2),nodes(segments(s,2:3)',3),'k');
% end
% [d, p] = dijkstra(nodes, segments, 1, 10)
% for n = 2:length(p)
% plot(nodes(p(n-1:n),2),nodes(p(n-1:n),3),'r-.','linewidth',2);
% end
% hold off;
%
% Author: Joseph Kirk
% Email: jdkirk630 at gmail dot com
% Release: 1.3
% Release Date: 5/18/07
if (nargin < 3) % SETUP
% (GENERATE RANDOM EXAMPLE OF NODES AND SEGMENTS IF NOT GIVEN AS INPUTS)
% Create a random set of nodes/vertices,and connect some of them with
% edges/segments. Then graph the resulting map.
num_nodes = 40; L = 100; max_seg_length = 30; ids = (1:num_nodes)';
nodes = [ids L*rand(num_nodes,2)]; % create random nodes
h = figure; plot(nodes(:,2),nodes(:,3),'k.') % plot the nodes
text(nodes(num_nodes,2),nodes(num_nodes,3),...
[' ' num2str(ids(num_nodes))],'Color','b','FontWeight','b')
hold on
num_segs = 0; segments = zeros(num_nodes*(num_nodes-1)/2,3);
for i = 1:num_nodes-1 % create edges between some of the nodes
text(nodes(i,2),nodes(i,3),[' ' num2str(ids(i))],'Color','b','FontWeight','b')
for j = i+1:num_nodes
d = sqrt(sum((nodes(i,2:3) - nodes(j,2:3)).^2));
if and(d < max_seg_length,rand < 0.6)
plot([nodes(i,2) nodes(j,2)],[nodes(i,3) nodes(j,3)],'k.-')
% add this link to the segments list
num_segs = num_segs + 1;
segments(num_segs,:) = [num_segs nodes(i,1) nodes(j,1)];
end
end
end
segments(num_segs+1:num_nodes*(num_nodes-1)/2,:) = [];
axis([0 L 0 L])
% Calculate Shortest Path Using Dijkstra's Algorithm
% Get random starting/ending nodes,compute the shortest distance and path.
start_id = ceil(num_nodes*rand); disp(['start id = ' num2str(start_id)]);
finish_id = ceil(num_nodes*rand); disp(['finish id = ' num2str(finish_id)]);
[distance,path] = dijkstra(nodes,segments,start_id,finish_id);
disp(['distance = ' num2str(distance)]); disp(['path = [' num2str(path) ']']);
% If a Shortest Path exists,Plot it on the Map.
figure(h)
for k = 2:length(path)
m = find(nodes(:,1) == path(k-1));
n = find(nodes(:,1) == path(k));
plot([nodes(m,2) nodes(n,2)],[nodes(m,3) nodes(n,3)],'ro-','LineWidth',2);
end
title(['Shortest Distance from ' num2str(start_id) ' to ' ...
num2str(finish_id) ' = ' num2str(distance)])
hold off
else %--------------------------------------------------------------------------
% MAIN FUNCTION - DIJKSTRA'S ALGORITHM
% initializations
node_ids = nodes(:,1);
[num_map_pts,cols] = size(nodes);
table = sparse(num_map_pts,2);
shortest_distance = Inf(num_map_pts,1);
settled = zeros(num_map_pts,1);
path = num2cell(NaN(num_map_pts,1));
col = 2;
pidx = find(start_id == node_ids);
shortest_distance(pidx) = 0;
table(pidx,col) = 0;
settled(pidx) = 1;
path(pidx) = {start_id};
if (nargin < 4) % compute shortest path for all nodes
while_cmd = 'sum(~settled) > 0';
else % terminate algorithm early
while_cmd = 'settled(zz) == 0';
zz = find(finish_id == node_ids);
end
while eval(while_cmd)
% update the table
table(:,col-1) = table(:,col);
table(pidx,col) = 0;
% find neighboring nodes in the segments list
neighbor_ids = [segments(node_ids(pidx) == segments(:,2),3);
segments(node_ids(pidx) == segments(:,3),2)];
% calculate the distances to the neighboring nodes and keep track of the paths
for k = 1:length(neighbor_ids)
cidx = find(neighbor_ids(k) == node_ids);
if ~settled(cidx)
d = sqrt(sum((nodes(pidx,2:cols) - nodes(cidx,2:cols)).^2));
if (table(cidx,col-1) == 0) || ...
(table(cidx,col-1) > (table(pidx,col-1) + d))
table(cidx,col) = table(pidx,col-1) + d;
tmp_path = path(pidx);
path(cidx) = {[tmp_path{1} neighbor_ids(k)]};
else
table(cidx,col) = table(cidx,col-1);
end
end
end
% find the minimum non-zero value in the table and save it
nidx = find(table(:,col));
ndx = find(table(nidx,col) == min(table(nidx,col)));
if isempty(ndx)
break
else
pidx = nidx(ndx(1));
shortest_distance(pidx) = table(pidx,col);
settled(pidx) = 1;
end
end
if (nargin < 4) % return the distance and path arrays for all of the nodes
dist = shortest_distance';
path = path';
else % return the distance and path for the ending node
dist = shortest_distance(zz);
path = path(zz);
path = path{1};
end
end
NOT THIS ONE courtesy of https://github.com/andrehacker/fu-muster/blob/master/u12-randforests/@tree/findpath.m
function path = findpath(obj, n1, n2)
%%FINDPATH Shortest path between two nodes
% PATH = T.FINDPATH(N1, N2) return the sequence of indices that iterates
% through the tree T from the node of index N1 to the node of index N2.
%
% The path returned is the shortest one in terms of number of edges. It
% always starts with index N1 and ends with index N2. Such a path always
% exists since all nodes are connected in a tree.
%
% EXAMPLE:
% % Find the path between node 'ABplp' and node 'Ca'
% lineage = tree.example;
% n1 = find(lineage.strcmp('ABplp'));
% n2 = find(lineage.strcmp('Ca'));
% path = lineage.findpath(n1, n2)
% pt = tree(lineage, 'clear');
% index = 1;
% for i = path
% pt = pt.set(i, index);
% index = index + 1;
% end
% disp(pt.tostring)
if n1 == n2
path = n1;
elseif any( n2 == obj.depthfirstiterator(n1) )
% n2 is in the children of n1
path = [ n1 descend(n1) ];
else
% n2 is elsewhere in the tree
path = [ n1 ascend(n1) ];
end
function p = ascend(n)
if n == n2
p = [];
return;
end
parent = obj.getparent(n);
if any( n2 == obj.depthfirstiterator(parent) )
% it is in the parent descendant, so descend from the parent
p = [ parent descend(parent) ];
else
% no, so we still have to go up
p = [ parent ascend(parent) ];
end
end
function p = descend(n)
if n == n2
p = [];
return
end
children = obj.depthfirstiterator(n);
for c = children(2 : end) % Get rid of current node in the sequence
if any( n2 == obj.depthfirstiterator(c) )
p = [ c descend(c) ];
break
end
end
end
end
function [path,goal,gfound] = findPath(map,V,E,start,waypoints,ECwaypoints,radius)
% FINDPATH: Returns the shortest path between a start node and
% the closest waypoint given a set of nodes V and edges E.
%
% [PATH,GOAL,GFOUND] = FINDPATH(MAP,V,E,START,WAYPOINTS,ECWAYPOINTS,RADIUS) returns
% the shortest path between a start node and closest waypoint
% given a set of nodes V and edges E.
%
% INPUTS
% map N-by-4 matrix containing the coordinates of walls in the
% environment: [x1, y1, x2, y2]
% V Set of nodes in roadmap
% E Set of edges in roadmap
% start 1-by-2 array containing x/y coordinates of start location
% waypoints N-by-2 matrix containing the coordinates of waypoints in
% the environment: [x, y]
% ECwaypoints N-by-2 matrix containing the coordinates of extra credit
% waypoints in the environment: [x, y]
% radius radius of robot
%
% OUTPUTS
% path N-by-2 array containing a series of points representing the
% shortest path connecting initial and closest goal points
% goal 1-by-2 array containing x/y coordinates of closest goal node
% gfound An integer denoting the number of goals found
%
% Cornell University
% MAE 4180/5180: Autonomous Mobile Robots
% Final Competition
% Pu, Kenneth (kp295)
%%============================================================================
% INITIALIZE VARIABLES
%==============================================================================
% For all possible edges between start node and vertices in V, check if the
% edge is valid and if so add it to E
for i=1:size(V,1)
v = V(i,:);
if (edgeIsFree([start,v],map,radius))
E = [E;start,v];
% REMOVE:
% plot([start(1),v(1)],[start(2),v(2)],'b');
end
end
% Add start point to V
V = [V;start];
% Construct list of goal nodes
goals = [waypoints;ECwaypoints];
% Find minimum path using dijkstras
[path,goal,gfound] = dijkstra(V,E,start,goals);
end
And also found another one https://github.com/kennethpu/iRoombot/blob/172347c1be1d87b6e4138d1dfc848abff899af38/dijkstra.m
function [path,goal,gfound] = dijkstra(V,E,start,goals)
% DIJKSTRA: Takes as input a graph represented by a set of nodes V and
% edges E, a start position, and a list of goal positions, then returns
% the shortest path between start node and the closest goal node. Uses
% Dijkstra's algorithm
%
% [PATH,GOAL,GFOUND] = DIJKSTRA(V,E,START,GOALS) returns
% the shortest path between a start and goal node given a set of nodes V
% and edges E
%
% INPUTS
% V Set of nodes in graph
% E Set of edges in graph
% start 1-by-2 array containing x/y coordinates of start node
% goals N-by-2 array containing x/y coordinates of goal nodes
%
% OUTPUTS
% path N-by-2 array containing a series of points representing the
% shortest path connecting initial and closest goal points
% goal 1-by-2 array containing x/y coordinates of closest goal node
% gfound An integer denoting the number of goals found
%
% Cornell University
% MAE 4180/5180: Autonomous Mobile Robots
% Final Competition
% Pu, Kenneth (kp295)
%%============================================================================
% INITIALIZE VARIABLES
%==============================================================================
V_cost = ones(size(V,1),1).*9999; % Array of node costs (initially HI)
V_prev = zeros(size(V,1),1); % List of node parent indices (initially 0)
% Integer to keep track of the number of goals found
gfound = 0;
% Set cost (distance from start) of start to 0
V_cost = setCost(start,V,V_cost,0);
% All nodes are initially in Q
Q = V; % List of nodes to be optimized
%%============================================================================
% RUN DJIKSTRAS
%==============================================================================
% Loop while there are still nodes to be optimized
while size(Q,1)
% Get node u in Q with smallest cost
qidx = minNode(Q,V,V_cost);
u = Q(qidx,:);
% Remove node u from Q
Q(qidx,:) = [];
% Break out of loop if u's cost is MAX. No more nodes are reachable
% from current node
if (getCost(u,V,V_cost) == 9999)
break;
end
% Get list of neighbor node indices
n_ids = getNeighbors(u,V,E);
% Iterate through all neighbors of u
for i=1:size(n_ids,1)
% Get neighbor node n
n = V(n_ids(i),:);
% Calculate alternative cost of n as cost of u + distance between u
% and n
u_cost = getCost(u,V,V_cost);
n_dist = pdist([u;n],'euclidean');
alt = u_cost + n_dist;
% If calculated alternative cost is cheaper than stored cost for n,
% replace stored cost for n and set u as parent node for n
if alt < getCost(n,V,V_cost)
V_cost = setCost(n,V,V_cost,alt);
V_prev = setPrev(n,V,V_prev,u);
end
end
if ismember(u,goals,'rows')
gfound = gfound+1;
if gfound == size(goals,1)
break;
end
end
end
%%============================================================================
% FIND CLOSEST GOAL
%==============================================================================
% Initialize array to hold goal nodes and their corresponding costs
goal_costs = zeros(size(goals,1),size(goals,2)+1);
for i=1:size(goals,1)
% Get costs for each goal and store in goal_costs
goal_costs(i,:) = [goals(i,:),getCost(goals(i,:),V,V_cost)];
end
% Sort goal_costs by cost
goal_costs = sortrows(goal_costs,3);
% Return the cheapest goal
goal = goal_costs(1,1:2);
%%============================================================================
% RECONSTRUCT MINIMUM PATH TO CLOSEST GOAL
%==============================================================================
% Start from goal point
u = goal;
% plot(u(1),u(2),'go','LineWidth',2);
% Add goal point to path
path = u;
% Step backward from goal point adding parent nodes until parent node index
% is no longer valid
while getPrev(u,V,V_prev)
prev_idx = getPrev(u,V,V_prev);
u = V(prev_idx,:);
% plot(u(1),u(2),'go','LineWidth',2);
% plot([u(1),path(1,1)],[u(2),path(1,2)],'g','LineWidth',2);
path = [u;path];
end
end
%%============================================================================
% minNode
%==============================================================================
% Helper function to get the index of the node with smallest cost in a
% list of nodes
%
% INPUTS
% Q list of nodes to search over
% V list of nodes in graph
% V_cost list of costs associated with V
%
% OUTPUTS
% nidx index of node in Q with smallest cost
function nidx = minNode(Q,V,V_cost)
% Initialize variables
nidx = 1;
min_cost = 9999;
% Iterate through all nodes in Q
for i=1:size(Q,1)
node = Q(i,:);
cost = getCost(node,V,V_cost);
% If node cost is less than minimum, save minimum cost and node index
if (cost<min_cost)
nidx = i;
min_cost = cost;
end
end
end
%%============================================================================
% getCost
%==============================================================================
% Helper function to get the cost of a node
%
% INPUTS
% n a given node
% V list of nodes in graph
% V_cost list of costs associated with V
%
% OUTPUTS
% cost cost associated with a node
function cost = getCost(n,V,V_cost)
% Find index corresponding to n in V
[~,idx] = ismember(n,V,'rows');
% Look up corresponding cost in V_cost
cost = V_cost(idx);
end
%%============================================================================
% setCost
%==============================================================================
% Helper function to set the cost of a node
%
% INPUTS
% n a given node
% V list of nodes in graph
% V_cost list of costs associated with V
% cost cost associated with a node
%
% OUTPUTS
% ret updated list of costs associated with V
function ret = setCost(n,V,V_cost,cost)
% Find index corresponding to n in V
[~,idx] = ismember(n,V,'rows');
% Set corresponding cost in V_cost to our target cost
V_cost(idx) = cost;
% Return updated list so our changes are not lost
ret = V_cost;
end
%%============================================================================
% getPrev
%==============================================================================
% Helper function to get the index of parent node of a given node
%
% INPUTS
% n a given node
% V list of nodes in graph
% V_prev list of parents associated with V
%
% OUTPUTS
% prev_idx index of parent node of a given node
function prev_idx = getPrev(n,V,V_prev)
% Find index corresponding to n in V
[~,idx] = ismember(n,V,'rows');
% Look up corresponding parent index in V_prev
prev_idx = V_prev(idx);
end
%%============================================================================
% setPrev
%==============================================================================
% Helper function to set the parent node of a given node
%
% INPUTS
% n a given node
% V list of nodes in graph
% V_prev list of parents associated with V
% prev parent node of a given node
%
% OUTPUTS
% ret updated list of parents associated with V
function ret = setPrev(n,V,V_prev,prev)
% Find index corresponding to n in V
[~,idx] = ismember(n,V,'rows');
% Find index corresponding to prev in V
[~,prev_idx] = ismember(prev,V,'rows');
% Set corresponding prev_idx in V_prev to our target prev_idx
V_prev(idx) = prev_idx;
% Return updated list so our changes are not lost
ret = V_prev;
end
%%============================================================================
% getNeighbors
%==============================================================================
% Helper function to get the indices of all neighboring nodes of a given
% node
%
% INPUTS
% n a given node
% V list of nodes in graph
% E list of edges in graph
%
% OUTPUTS
% ids N-by-1 array of indices of nodes in V neighboring node n
function ids = getNeighbors(n,V,E)
% Initialize empty list
ids = [];
% Iterate through all edges
for e=1:size(E,1)
% Get endpoints of edges
v1 = E(e,1:2);
v2 = E(e,3:4);
% If target node corresponds to first endpoint, save second
% endpoint as a neighbor
if (all(v1==n))
[~,idx] = ismember(v2,V,'rows');
ids = [ids;idx];
% If target node corresponds to second endpoint, save first
% endpoint as a neighbor
elseif (all(v2==n))
[~,idx] = ismember(v1,V,'rows');
ids = [ids;idx];
end
end
end
