From 7a45ab408d4369ef611e1bcee812de90c4400dd0 Mon Sep 17 00:00:00 2001
From: "Marco P. Nogueira" <marco.nogueira@uwaterloo.ca>
Date: Thu, 5 Apr 2018 11:37:27 -0400
Subject: [PATCH] Restore box functionality in wavefront +prefers diagonal
 paths

---
 .../08-planning/shortest_wavefront_path.m          | 12 ++++++------
 matlab_simulation/08-planning/wavefront.m          | 14 +++++++-------
 2 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/matlab_simulation/08-planning/shortest_wavefront_path.m b/matlab_simulation/08-planning/shortest_wavefront_path.m
index f332c6e..842077a 100644
--- a/matlab_simulation/08-planning/shortest_wavefront_path.m
+++ b/matlab_simulation/08-planning/shortest_wavefront_path.m
@@ -23,14 +23,14 @@
                    [-1, 0];                  
                    ];
     case 'box'
-        test_locations = [ [0, 1];
-                   [1, 1];
-                   [1, 0];
+        test_locations = [ [1, 1];
                    [1, -1];
+                   [-1, -1];
+                   [-1, 1];
+                   [0, 1];
+                   [1, 0];
                    [0, -1];                   
-                   [1, -1];                   
-                   [1, 0];                   
-                   [1, 1];                  
+                   [-1, 0];
                    ];
     otherwise
         disp('No valid motion method provided')
diff --git a/matlab_simulation/08-planning/wavefront.m b/matlab_simulation/08-planning/wavefront.m
index ed69e43..2f5d858 100644
--- a/matlab_simulation/08-planning/wavefront.m
+++ b/matlab_simulation/08-planning/wavefront.m
@@ -50,14 +50,14 @@
                    [-1, 0];                  
                    ];
     case 'box'
-        test_locations = [ [0, 1];
-                   [1, 1];
-                   [1, 0];
+        test_locations = [ [1, 1];
                    [1, -1];
+                   [-1, -1];
+                   [-1, 1];
+                   [0, 1];
+                   [1, 0];
                    [0, -1];                   
-                   [1, -1];                   
-                   [1, 0];                   
-                   [1, 1];                  
+                   [-1, 0];
                    ];
     otherwise
         disp('No valid motion method provided')
@@ -82,7 +82,7 @@
 
     % for each of the neighboring cells check if it has been already
     % explored
-    for i = 1:4
+    for i = 1:length(wavefrontpnts(:,1))
             if (wavefrontmap(wavefrontpnts(i,1),wavefrontpnts(i,2)) ~= -10)
                 continue;
             end