Diff: main/main.cpp

Revision:

49:89f0f54aa46b

Parent:

48:76cf4521d342

Child:

50:fa124ea1df9f

--- a/main/main.cpp	Fri Apr 17 13:20:19 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 14:45:13 2020 +0000
@@ -478,6 +478,43 @@
         // if it does, check the number of nodes between before and after
         // which ever is shorter, set the 0 of those nodes
         
+        //int short_length = dead_end_removal(point[d_node], point[curr_index]);
+//        char dir_diff;
+//        
+//        for(int j = 0; j < short_length; j++) {
+//            int curr_node = path_to_point_index(shortest[j]);
+//            int next_node = path_to_point_index(shortest[j+1]);
+//            if(coords_x[next_node] != coords_x[curr_node]) {
+//                if(coords_x[next_node] - coords_x[curr_node] > 0){
+//                    dir_diff = 'E';
+//                } else {
+//                    dir_diff = 'W';
+//                }
+//            } else if( coords_y[next_node] != coords_y[curr_node] ) {
+//                if(coords_y[next_node] - coords_y[curr_node] > 0){
+//                    dir_diff = 'N';
+//                } else {
+//                    dir_diff = 'S';
+//                }
+//            }
+//            
+//            if( dir_diff == 'N' ) { 
+//                explored[curr_node] &= 0b1011;
+//            }
+//            else if( dir_diff == 'E' ) { 
+//                explored[curr_node] &= 0b1101; 
+//            }
+//            else if( dir_diff == 'S' ) { 
+//                explored[curr_node] &= 0b1110;
+//            }
+//            else if( dir_diff == 'W' ) { 
+//                explored[curr_node] &= 0b0111;
+//            }
+//            if(point[next_node] == point[d_node]) { break; }
+//        }
+        
+        
+
         int curr_node = curr_index;
         bool before = false;
         bool after = false;
@@ -577,12 +614,39 @@
     }
 }
 
-void dead_end_removal( int index1, int index2)   // change into dead_end_removal and have it take two indexes and a path and return an array
+void dead_end_removal( int point1, int point2 )   // change into dead_end_removal and have it take two indexes and a path and return an array
 {   
     // dead end removal between start and final node
     // add the inverse of the result to end of the looped_path
     // then separate into before and after, simplify both and compare
     
+    int index1, index2;
+    int d_index, before_index, after_index;
+    bool before = false;
+    
+    bool desired_discovered = false;
+        
+    for(int k = 0; k <= path_length; k++) {
+        if(looped_path[k] == point[point1]) { desired_discovered = true; d_index = k; }  // maybe set a variable
+        if(desired_discovered == false && looped_path[k] == point[point2]) { before = true; before_index = k; }
+        if(desired_discovered == true && looped_path[k] == point[point2]) { after_index = k; break; }
+    }
+    
+    if( before_index < after_index ){
+        before = true;
+    }
+    else{
+        before = false;
+    }
+    
+    if(before) {
+        index1 = before_index;
+        index2 = d_index;
+    }else{
+        index1 = d_index;
+        index2 = after_index;
+    }
+    
     int temp_array[100];
     
     for( int x = 0; x <= path_length; x++ ) { shortest[x] = NULL; }
@@ -924,7 +988,7 @@
     // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
     // loop between start and goal
     robot.stop();
-    dead_end_removal( 0, path_length );
+    dead_end_removal( point[0], point[total_points] );
 //    simplify_looped();
     
     while( button_enter.read() == 1 ) {