Dijkstra's algorithm problem

Hello!

I'm trying to build a previous nod vector and I'm handling some problems. not clear, right? :) that's way I'll explain the problem by example.

For example: please look at the image above. Suppose A=city number 1,B=city number 2,C=city number 3 and etc.

SRC = 1; If the optimal path previous nod is equal to the source city (in this case: 1) then the previous vector will get -1 in the specific place(in this case in place 1)

So the vector I'm trying to bulid is: [-1 1 2 1 2 5 4 5 8 9]
Reminder in the image: A=1,B=2,...,J=10.
1 ->1 -> optimal path: 1-->1        [-1                ] 
1 ->2 -> optimal path: 1-->2        [-1 2              ]
1 ->3 -> optima; path: 1->2->3      [-1 1 2]           ]
1 ->4 -> optimal path: 1-->4        [-1 1 2 1          ]
1 ->5 -> optimal path: 1->2>5       [-1 1 2 1 2        ]
1 ->6 -> optimal path: 1-->2-->5->6 [-1 1 2 1 2 5      ]
   *
   *
   *
1-->10

This is the code:

% Compute the shortest paths from SRC to all other nodes % using Dijkstra's algorithm. function [d previous] = dijkstra(SRC,D)

      % Initialize shortest distances
      N = size(D,1); % number of cities
      d = ones(1,N)*Inf;
      d(SRC) = 0;
      active = 1:N;  % all nodes are active
      previous = [];
      L = size(previous,2);

      % while there is a nodes that was not handled
      while length(active)>0
          % find the city with smallest d
          % d(active) 'selects' the d-values for the active nodes
          [val,active_idx] = min(d(active));
          chosen = active(active_idx);

          % now handle the city
          for j=find(D(chosen,:)<Inf)
             % now I need to update a distance (relaxation)
                  if d(chosen)+D(chosen,j) < d(j)
*                      d(j) = d(chosen) + D(chosen,j);
                      L=L+1;
                      if SRC == j
                          previous(L) = -1;
                      else
                          previous(L) = D(chosen);   *            
                      end
                   end

          end % done with all neighbors    

          % take out treated node from active
          active(active_idx) = [];
      end
  end

Well, I'm doing something wrong in:

                        L=L+1;
                        if SRC == j
                            previous(L) = -1;
                        else
                            previous(L) = D(chosen);   *            
                        end

and that's give me wrong output.

Can you please help me help me finding the right way to do this? thank you very much!

Answer 1

[John BG]

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

Answer 2

[Christine Tobler]

Since R2015b, MATLAB has a class graph which provides a function shortestpathtree that does this algorithm for you:

>> g = graph(D);
>> [t, d] = shortestpathtree(g, SRC);

I haven't tried out the code you posted, but on first glance, I think the line you point to should be

previous(L) = j;

since previous is typically an array of node indices on the path, not an array of distances.

Answer 3

[Hardi Mohammed]

Hi how to use dijkstra to find longest path?

[Christine Tobler]

Hi,

It's probably better to start a new question, as this is quite a different problem. The Dijkstra algorithm can't find the longest path for a general graph, because this is an NP-hard problem, and Dijkstra is a polynomial algorithm.

If your graph is directed acyclic, you could use the 'acyclic' method of the graph/shortestpath method in MATLAB, and just revert the sign of the edge weights.

For more information, see here: Wikipedia page for Longest Path Problem