From e94fb18442df4a944a8458bb52221f190800f43d Mon Sep 17 00:00:00 2001
From: "Marco P. Nogueira" <marco.nogueira@uwaterloo.ca>
Date: Thu, 5 Apr 2018 22:34:17 -0400
Subject: [PATCH] Fixes for graph search examples (mr-8)

+add motion type in call to wavefront and shortest_wavefront_path
+correction in shortestpath_mr method types comment
+correction to heuristic distance check (Manhattan distance for BFS/DFS)
+relocate Dijkstra's case 4 in switch for code clarity
---
 .../mr-8-graphsearch/wavefront_example.m      |  4 +-
 .../08-planning/shortestpath_mr.m             | 46 +++++++++----------
 2 files changed, 25 insertions(+), 25 deletions(-)

diff --git a/matlab_simulation/01-examples_lecture/mr-8-graphsearch/wavefront_example.m b/matlab_simulation/01-examples_lecture/mr-8-graphsearch/wavefront_example.m
index 0e88077..32d088d 100644
--- a/matlab_simulation/01-examples_lecture/mr-8-graphsearch/wavefront_example.m
+++ b/matlab_simulation/01-examples_lecture/mr-8-graphsearch/wavefront_example.m
@@ -6,12 +6,12 @@
 startpos = [103, 480];
 endpos = [110, 400];
 
-[wavefrontmap, path] = wavefront(testimage, startpos, endpos);
+[wavefrontmap, path] = wavefront(testimage, startpos, endpos, 'urdl');
 
 % you can just run this function without rebuilding the wavefront map if the
 % goal location (or end point) does not change. 
 
-path = shortest_wavefront_path(wavefrontmap, startpos);
+path = shortest_wavefront_path(wavefrontmap, startpos, 'urdl');
 imagesc(wavefrontmap);
 hold on
 plot(path(:,1), path(:,2), '-r');
diff --git a/matlab_simulation/08-planning/shortestpath_mr.m b/matlab_simulation/08-planning/shortestpath_mr.m
index cefc102..a3958c6 100644
--- a/matlab_simulation/08-planning/shortestpath_mr.m
+++ b/matlab_simulation/08-planning/shortestpath_mr.m
@@ -7,7 +7,7 @@
 %           upper triangle
 %   start: index of start node
 %   finish: index of finish node
-%   method: (1 = Astar) (2 = Dijkstra's)
+%   method: (1 = Astar) (2 = BFS) (3 = DFS) (4 = Dijkstra's)
 %   heuristicDist: 1.Euclidean distance  2.Manhattan distance 
 %   createVideo: (1 = YES) (the others: NO) 
 % Outputs:
@@ -50,7 +50,7 @@
 n = length(nodes);
 switch heuristicDist
     case 1
-        if (method == 3) || (method == 4)
+        if (method == 2) || (method == 3)
            disp('Error, use manhattan distance for breadth and depth first search')
            return;
         end
@@ -122,26 +122,6 @@
                 continue;
             end 
             
-         case 4 %Dijkstra's 
-            % Check if open set is empty
-            if (isempty(OpenSet(:,1)))
-                spath = [];
-                sdist = 0;
-                return;
-            end             
-            %--------------------------------
-            % Reminder: OpenSet(:,4)= distance to neighbours;
-            [val, best] = min(OpenSet(:,4));
-            %--------------------------------
-            bestnode = OpenSet(best,:);
-            % Check end condition
-            if (bestnode(1)==finish)
-                done = 1;
-                % Move best to closed set
-                C = [C; bestnode];                
-                continue;
-            end           
-            
          case 2 %Breadth-First
             if (isempty(OpenSet(:,1)))
                 done = 1;
@@ -177,6 +157,26 @@
                 C = [C;bestnode];
                 continue;
             end
+            
+         case 4 %Dijkstra's 
+            % Check if open set is empty
+            if (isempty(OpenSet(:,1)))
+                spath = [];
+                sdist = 0;
+                return;
+            end             
+            %--------------------------------
+            % Reminder: OpenSet(:,4)= distance to neighbours;
+            [val, best] = min(OpenSet(:,4));
+            %--------------------------------
+            bestnode = OpenSet(best,:);
+            % Check end condition
+            if (bestnode(1)==finish)
+                done = 1;
+                % Move best to closed set
+                C = [C; bestnode];                
+                continue;
+            end
      end
    
      % Move best to closed set
@@ -244,7 +244,7 @@
      switch method
         case 1 % Astar
                OpenSet = OpenSet([1:best-1 best+1:end],:); % remove best node from open set                            
-        case 4 %Dijkstra's     
+        case 4 % Dijkstra's     
                OpenSet = OpenSet([1:best-1 best+1:end],:); % remove best node from open set                
      end