Dijkstra Algorithm

  function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)

  cmap = [1 1 1; ...
          0 0 0; ...
          1 0 0; ...
          0 0 1; ...
          0 1 0; ...
          1 1 0; ...
    0.5 0.5 0.5];

  colormap(cmap);

  % variable to control if the map is being visualized on every
  % iteration
  drawMapEveryTime = true;

  [nrows, ncols] = size(input_map);

  % map - a table that keeps track of the state of each grid cell
  map = zeros(nrows,ncols);

  map(~input_map) = 1;   % Mark free cells
  map(input_map)  = 2;   % Mark obstacle cells

  % Generate linear indices of start and dest nodes
  start_node = sub2ind(size(map), start_coords(1), start_coords(2));
  dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

  map(start_node) = 5;
  map(dest_node)  = 6;

  % Initialize distance array
  distanceFromStart = Inf(nrows,ncols);

  % For each grid cell this array holds the index of its parent
  parent = zeros(nrows,ncols);

  distanceFromStart(start_node) = 0;

  % keep track of number of nodes expanded 
  numExpanded = 0;

  % Main Loop
  while true
      
      % Draw current map
      map(start_node) = 5;
      map(dest_node) = 6;
      
      % make drawMapEveryTime = true if you want to see how the 
      % nodes are expanded on the grid. 
      if (drawMapEveryTime)
          image(1.5, 1.5, map);
          grid on;
          axis image;
          drawnow;
      end
      
      % Find the node with the minimum distance
      [min_dist, current] = min(distanceFromStart(:));
      
      if ((current == dest_node) || isinf(min_dist))
          break;
      end;
      
      % Update map
      map(current) = 3;         % mark current node as visited
      distanceFromStart(current) = Inf; % remove this node from further consideration
      
      % Compute row, column coordinates of current node
      [i, j] = ind2sub(size(distanceFromStart), current);
      
    % *********************************************************************
      
      % Visit each neighbor of the current node and update the map, distances
      % and parent tables appropriately.
      
      numExpanded = numExpanded + 1;
      currentNode  = [i,j];

      % This is for putting the nodes around the starting node 
      upNode = [i+1,j];
      downNode = [i-1,j];
      leftNode = [i,j-1];
      rightNode = [i,j+1];

      % For the neighbors
      neighbours = [upNode; downNode; leftNode; rightNode];

      for n = 1:4
          % checks boundaries
          if (neighbours(n,1)>0 && neighbours(n,2)>0) && (neighbours(n,1)<= nrows && neighbours(n,2)<=ncols)
              % checks positions 1- white 2-obstacle 3-visited 4-onlist
              % 5-start 6-destination if the position is not an obstacle,
              % visited or the start or if it's destination, then calculate
              if( map(neighbours(n,1),neighbours(n,2)) ~= 2 && map(neighbours(n,1),neighbours(n,2)) ~= 3 && ...
                      map(neighbours(n,1),neighbours(n,2)) ~= 5 || map(neighbours(n,1),neighbours(n,2)) == 6)
                  % adds to list and calculates distance to nodes
                  if(distanceFromStart(neighbours(n,1),neighbours(n,2)) > 1 + sum(abs(start_coords - currentNode)) || map(neighbours(n,1),neighbours(n,2)) == 6)
                      map(neighbours(n,1),neighbours(n,2)) = 4; % adds to list of expansion
                      distanceFromStart(neighbours(n,1),neighbours(n,2)) = 1 + sum(abs(start_coords-currentNode));
                      parent(neighbours(n,1),neighbours(n,2)) = sub2ind(size(map),current);
                  end
              end
          end
      end


      
      
      
      %*********************************************************************

  end

  %% Construct route from start to dest by following the parent links
  if (isinf(distanceFromStart(dest_node)))
      route = [];
  else
      route = [dest_node];
      
      while (parent(route(1)) ~= 0)
          route = [parent(route(1)), route];
      end
      
          % Snippet of code used to visualize the map and the path
      for k = 2:length(route) - 1        
          map(route(k)) = 7;
          pause(0.1);
          image(1.5, 1.5, map);
          grid on;
          axis image;
      end
  end

  end

%% Define a small map
map = false(10);

% Add an obstacle
map (1:5, 6) = true;

start_coords = [6, 2];
dest_coords  = [9, 8];

%%
close all;
[route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);