I have some nodes distributed somewhere. I want to use dijkstra algorithm to find distances and the paths between them. Here is the code:
N=16; %number of nodes, There is one sink node in addition to these N nodes.
AreaMax=200;
xyNode=randi([1,200],N,2);
xyNode2=[xyNode;[100*sqrt(N)/2,100*sqrt(N)/2]]; % Adds the sink node's coordination.
ID=[1:N+1]';
nodes=[ID,xyNode2(:,1),xyNode2(:,2)];
[testt segments]=q43223243(N+1, 100*sqrt(5), 200); % Finds segments, which is an input needed in dijkstra algorithm.
%% Find the distance and path between nodes and the sink using CSDG_Dijkstra_SPT algorithm
start_id=n;
finish_id=N+1;
[distance,path] = dijkstra(nodes,segments,start_id,finish_id);
And these are the functions used:
function varargout = q43223243(N, TRSH, MAX_DST)
if nargin == 0
%% DEFINITIONS:
N = 50;
TRSH = 100*sqrt(5); % If the distance between nodes is less than this, they can communicate;
MAX_DST = 1000;
end
%% The rest...
xyNode=randi([1,100],N,2);
xyNode2=[xyNode;[100*sqrt(N)/2,100*sqrt(N)/2]];
ID = 1:N+1;
nodes = [ID(:),xyNode2(:,1),xyNode2(:,2)];
% Find pairwise distance:
D = pdist2(xyNode2,xyNode2); % symmetric, so we only take the bottom. %tril
D(D > TRSH | D == 0) = NaN;
% Find positions of valid edges:
[I,J] = ind2sub(size(D), find(~isnan(D)));
% Finally construct the segments array:
segments = [I, J, D(~isnan(D))];
% Outputs
if nargout > 0
varargout{1} = nodes;
varargout{2} = segments;
% else
% figure();
% line(xyNode(:,1),xyNode(:,2),'LineStyle','none','Marker','o'); hold on;
% gplot(~isnan(D),xyNode,'-r');
end
end
And
function [dist,path] = dijkstra(nodes,segments,start_id,finish_id)
I think the nodes are not connected to each other because all the outputs are [Inf NaN].
What should I do to get to a correct output?
Another thing that is wrong is that using function function varargout = q43223243(N, TRSH, MAX_DST) ,segments output must be in this format: [ID,N1,N2] and ID must be increasing and ordered integer number starting from 1. But I get unsorted numbers.
Example of a correct segments:
A=q43223243(16, 100*sqrt(5), 200);
A =
1 90 36
2 96 84
3 55 59
4 14 55
5 15 92
6 26 29
7 85 76
8 26 76
9 82 39
10 25 57
11 93 8
12 35 6
13 20 54
14 26 78
15 62 94
16 48 13
17 200 200
And in my code I get this :
segments =
2.0000 1.0000 93.4345
3.0000 1.0000 117.1537
4.0000 1.0000 16.0312
5.0000 1.0000 64.8460
6.0000 1.0000 68.5930
7.0000 1.0000 15.0000
8.0000 1.0000 22.1359
9.0000 1.0000 78.7718
10.0000 1.0000 83.8153
11.0000 1.0000 63.8905
12.0000 1.0000 70.4557
13.0000 1.0000 53.9351
14.0000 1.0000 80.9506
15.0000 1.0000 55.3173
16.0000 1.0000 92.3959
17.0000 1.0000 9.4868
1.0000 2.0000 93.4345
3.0000 2.0000 84.0536
4.0000 2.0000 95.3362
5.0000 2.0000 92.6553
6.0000 2.0000 25.0000
7.0000 2.0000 93.1933
8.0000 2.0000 72.0278
9.0000 2.0000 106.0424
10.0000 2.0000 36.4005
11.0000 2.0000 66.6033
12.0000 2.0000 116.0431
13.0000 2.0000 103.6002
14.0000 2.0000 98.4733
15.0000 2.0000 102.6158
16.0000 2.0000 7.2801
17.0000 2.0000 84.2140
18.0000 2.0000 161.5947
1.0000 3.0000 117.1537
2.0000 3.0000 84.0536
4.0000 3.0000 106.7801
5.0000 3.0000 63.3246
6.0000 3.0000 86.8332
7.0000 3.0000 127.2792
8.0000 3.0000 107.1681
9.0000 3.0000 63.0079
10.0000 3.0000 50.9902
11.0000 3.0000 53.2635
12.0000 3.0000 82.1523
13.0000 3.0000 82.9699
14.0000 3.0000 53.0848
15.0000 3.0000 80.7775
16.0000 3.0000 91.0055
17.0000 3.0000 112.4500
1.0000 4.0000 16.0312
2.0000 4.0000 95.3362
3.0000 4.0000 106.7801
5.0000 4.0000 50.4777
6.0000 4.0000 71.5122
7.0000 4.0000 31.0161
8.0000 4.0000 30.4795
9.0000 4.0000 63.5610
10.0000 4.0000 79.5110
11.0000 4.0000 54.1202
12.0000 4.0000 54.4518
13.0000 4.0000 38.0789
14.0000 4.0000 66.5733
15.0000 4.0000 39.5601
16.0000 4.0000 95.5196
17.0000 4.0000 20.6155
1.0000 5.0000 64.8460
2.0000 5.0000 92.6553
3.0000 5.0000 63.3246
4.0000 5.0000 50.4777
6.0000 5.0000 77.5242
7.0000 5.0000 78.5493
8.0000 5.0000 65.4905
9.0000 5.0000 16.1245
10.0000 5.0000 60.7454
11.0000 5.0000 26.2488
12.0000 5.0000 24.1868
13.0000 5.0000 19.6469
14.0000 5.0000 16.1245
15.0000 5.0000 17.4642
16.0000 5.0000 96.5401
17.0000 5.0000 64.1950
1.0000 6.0000 68.5930
2.0000 6.0000 25.0000
3.0000 6.0000 86.8332
4.0000 6.0000 71.5122
5.0000 6.0000 77.5242
7.0000 6.0000 68.2642
8.0000 6.0000 47.0425
9.0000 6.0000 92.5743
10.0000 6.0000 36.0555
11.0000 6.0000 53.4883
12.0000 6.0000 99.2018
13.0000 6.0000 84.8999
14.0000 6.0000 86.7295
15.0000 6.0000 84.2912
16.0000 6.0000 24.0416
17.0000 6.0000 59.3043
18.0000 6.0000 179.4314
1.0000 7.0000 15.0000
2.0000 7.0000 93.1933
3.0000 7.0000 127.2792
4.0000 7.0000 31.0161
5.0000 7.0000 78.5493
6.0000 7.0000 68.2642
8.0000 7.0000 22.4722
9.0000 7.0000 93.0054
10.0000 7.0000 89.4427
11.0000 7.0000 74.4110
12.0000 7.0000 85.3756
13.0000 7.0000 68.7314
14.0000 7.0000 94.5410
15.0000 7.0000 70.0357
16.0000 7.0000 91.0055
17.0000 7.0000 15.0000
1.0000 8.0000 22.1359
2.0000 8.0000 72.0278
3.0000 8.0000 107.1681
4.0000 8.0000 30.4795
5.0000 8.0000 65.4905
6.0000 8.0000 47.0425
7.0000 8.0000 22.4722
9.0000 8.0000 81.2219
10.0000 8.0000 67.1193
11.0000 8.0000 55.5698
12.0000 8.0000 78.0897
13.0000 8.0000 61.0000
14.0000 8.0000 80.6040
15.0000 8.0000 61.6604
16.0000 8.0000 70.5762
17.0000 8.0000 12.6491
18.0000 8.0000 217.9354
1.0000 9.0000 78.7718
2.0000 9.0000 106.0424
3.0000 9.0000 63.0079
4.0000 9.0000 63.5610
5.0000 9.0000 16.1245
6.0000 9.0000 92.5743
7.0000 9.0000 93.0054
8.0000 9.0000 81.2219
10.0000 9.0000 72.1803
11.0000 9.0000 39.6611
12.0000 9.0000 19.4165
13.0000 9.0000 27.0185
14.0000 9.0000 10.1980
15.0000 9.0000 25.0000
16.0000 9.0000 110.4355
17.0000 9.0000 79.0759
1.0000 10.0000 83.8153
2.0000 10.0000 36.4005
3.0000 10.0000 50.9902
4.0000 10.0000 79.5110
5.0000 10.0000 60.7454
6.0000 10.0000 36.0555
7.0000 10.0000 89.4427
8.0000 10.0000 67.1193
9.0000 10.0000 72.1803
11.0000 10.0000 34.8855
12.0000 10.0000 84.9058
13.0000 10.0000 75.3923
14.0000 10.0000 63.7024
15.0000 10.0000 73.9256
16.0000 10.0000 42.4500
17.0000 10.0000 76.3217
18.0000 10.0000 195.9042
1.0000 11.0000 63.8905
2.0000 11.0000 66.6033
3.0000 11.0000 53.2635
4.0000 11.0000 54.1202
5.0000 11.0000 26.2488
6.0000 11.0000 53.4883
7.0000 11.0000 74.4110
8.0000 11.0000 55.5698
9.0000 11.0000 39.6611
10.0000 11.0000 34.8855
12.0000 11.0000 50.2195
13.0000 11.0000 40.8044
14.0000 11.0000 33.2415
15.0000 11.0000 39.2173
16.0000 11.0000 70.8025
17.0000 11.0000 59.4643
1.0000 12.0000 70.4557
2.0000 12.0000 116.0431
3.0000 12.0000 82.1523
4.0000 12.0000 54.4518
5.0000 12.0000 24.1868
6.0000 12.0000 99.2018
7.0000 12.0000 85.3756
8.0000 12.0000 78.0897
9.0000 12.0000 19.4165
10.0000 12.0000 84.9058
11.0000 12.0000 50.2195
13.0000 12.0000 17.1172
14.0000 12.0000 29.0689
15.0000 12.0000 16.4924
16.0000 12.0000 119.5199
17.0000 12.0000 73.0068
1.0000 13.0000 53.9351
2.0000 13.0000 103.6002
3.0000 13.0000 82.9699
4.0000 13.0000 38.0789
5.0000 13.0000 19.6469
6.0000 13.0000 84.8999
7.0000 13.0000 68.7314
8.0000 13.0000 61.0000
9.0000 13.0000 27.0185
10.0000 13.0000 75.3923
11.0000 13.0000 40.8044
12.0000 13.0000 17.1172
14.0000 13.0000 33.0151
15.0000 13.0000 2.2361
16.0000 13.0000 106.4049
17.0000 13.0000 56.0089
1.0000 14.0000 80.9506
2.0000 14.0000 98.4733
3.0000 14.0000 53.0848
4.0000 14.0000 66.5733
5.0000 14.0000 16.1245
6.0000 14.0000 86.7295
7.0000 14.0000 94.5410
8.0000 14.0000 80.6040
9.0000 14.0000 10.1980
10.0000 14.0000 63.7024
11.0000 14.0000 33.2415
12.0000 14.0000 29.0689
13.0000 14.0000 33.0151
15.0000 14.0000 30.8058
16.0000 14.0000 103.2473
17.0000 14.0000 80.0562
1.0000 15.0000 55.3173
2.0000 15.0000 102.6158
3.0000 15.0000 80.7775
4.0000 15.0000 39.5601
5.0000 15.0000 17.4642
6.0000 15.0000 84.2912
7.0000 15.0000 70.0357
8.0000 15.0000 61.6604
9.0000 15.0000 25.0000
10.0000 15.0000 73.9256
11.0000 15.0000 39.2173
12.0000 15.0000 16.4924
13.0000 15.0000 2.2361
14.0000 15.0000 30.8058
16.0000 15.0000 105.5509
17.0000 15.0000 57.0789
1.0000 16.0000 92.3959
2.0000 16.0000 7.2801
3.0000 16.0000 91.0055
4.0000 16.0000 95.5196
5.0000 16.0000 96.5401
6.0000 16.0000 24.0416
7.0000 16.0000 91.0055
8.0000 16.0000 70.5762
9.0000 16.0000 110.4355
10.0000 16.0000 42.4500
11.0000 16.0000 70.8025
12.0000 16.0000 119.5199
13.0000 16.0000 106.4049
14.0000 16.0000 103.2473
15.0000 16.0000 105.5509
17.0000 16.0000 83.0241
18.0000 16.0000 157.9060
1.0000 17.0000 9.4868
2.0000 17.0000 84.2140
3.0000 17.0000 112.4500
4.0000 17.0000 20.6155
5.0000 17.0000 64.1950
6.0000 17.0000 59.3043
7.0000 17.0000 15.0000
8.0000 17.0000 12.6491
9.0000 17.0000 79.0759
10.0000 17.0000 76.3217
11.0000 17.0000 59.4643
12.0000 17.0000 73.0068
13.0000 17.0000 56.0089
14.0000 17.0000 80.0562
15.0000 17.0000 57.0789
16.0000 17.0000 83.0241
2.0000 18.0000 161.5947
6.0000 18.0000 179.4314
8.0000 18.0000 217.9354
10.0000 18.0000 195.9042
16.0000 18.0000 157.9060
which is for N=16 nodes plus one sink node.
This is the dijkstra algorithm:
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
