pathplan: repair bugs in check functions

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index e763ee627b561be1c4e7bb16665979733694c508..7e8ea6cc3bc8d70933622d3353b987963c1053f5 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -73,43 +73,47 @@ bool check_bot_position_straight(int x_0, int y_0, bitmap_dim bitmap_straight[],
 bool check_bot_position_across(int x_0, int y_0, bitmap_dim bitmap_across[], int angle);
 bool check_bot_position(int x_0, int y_0, bitmap_dim bitmap_par[], int angle);
 #endif
-void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]);
-bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle);
 //int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
 //int get_round_actual_angle();
 
-void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]){
- 
-  bitmap_straight[0].x_min=-BITMAP_AB;
-  bitmap_straight[0].x_max=BITMAP_AF;
-  bitmap_straight[0].y_min=-BITMAP_W2;
-  bitmap_straight[0].y_max=BITMAP_W2;
-  
-  bitmap_straight[1].x_min=-BITMAP_W2;
-  bitmap_straight[1].x_max=BITMAP_W2;
-  bitmap_straight[1].y_min=-BITMAP_AB;
-  bitmap_straight[1].y_max=BITMAP_AF;
-  
-  bitmap_straight[2].x_min=-BITMAP_AF;
-  bitmap_straight[2].x_max=BITMAP_AB;
-  bitmap_straight[2].y_min=-BITMAP_W2;
-  bitmap_straight[2].y_max=BITMAP_W2;
-  
-  bitmap_straight[3].x_min=-BITMAP_W2;
-  bitmap_straight[3].x_max=BITMAP_W2;
-  bitmap_straight[3].y_min=-BITMAP_AF;
-  bitmap_straight[3].y_max=BITMAP_AB;
+void init_bitmap(bitmap_dim bitmap[], int bitmap_ab, int bitmap_af, int bitmap_w2){
+
+  //0deg, 45deg
+  bitmap[0].x_min=bitmap_ab;
+  bitmap[0].x_max=bitmap_af;
+  bitmap[0].y_min=bitmap_w2;
+  bitmap[0].y_max=bitmap_w2;
+  //90deg, 135deg
+  bitmap[1].x_min=bitmap_w2;
+  bitmap[1].x_max=bitmap_w2;
+  bitmap[1].y_min=bitmap_ab;
+  bitmap[1].y_max=bitmap_af;
+  //180deg, 225deg
+  bitmap[2].x_min=bitmap_af;
+  bitmap[2].x_max=bitmap_ab;
+  bitmap[2].y_min=bitmap_w2;
+  bitmap[2].y_max=bitmap_w2;
+  //270deg, 315 deg
+  bitmap[3].x_min=bitmap_w2;
+  bitmap[3].x_max=bitmap_w2;
+  bitmap[3].y_min=bitmap_af;
+  bitmap[3].y_max=bitmap_ab;
   
 }
 
-bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle){
+
+bool check_bot_position_straight(int x_0, int y_0, bitmap_dim bitmap_straight[], int angle){
   
-  if (angle==0){       //angle must be in range 90-360
-    angle=360;
-  }
+  angle=(angle % 360);
+//   printf("angle_straight %d", angle);
   
-  angle=360/angle;
+  angle=(angle/90);
   
+//   printf(" -> %d", angle);
+//   printf(" x,y -> %d, %d\n", x_0,y_0);
+//   
+//   printf("x_min, x_max,y_min, y_max -> %d, %d, %d, %d\n",(x_0-bitmap_straight[angle].x_min),(x_0+bitmap_straight[angle].x_max),(y_0-bitmap_straight[angle].y_min),(y_0+bitmap_straight[angle].y_max));
+//   
   int x,y;
   
   for(x=x_0-bitmap_straight[angle].x_min; x<=x_0+bitmap_straight[angle].x_max;x++){
@@ -117,6 +121,9 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
       
       ShmapSetCellFlag(x,y,MAP_FLAG_PATH_SHAPE);
       
+
+//       printf("x, y -> %d, %d\n", x,y);
+      
       if (!ShmapIsFreeCell(x,y)){      //1 no obstacle,-1 no in map, 0 otherwise
        return false;
       }
@@ -126,9 +133,62 @@ bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_st
   return true;
 }
 
+bool check_bot_position_across(int x_0, int y_0, bitmap_dim bitmap_across[], int angle){
+  
+  angle=(angle % 360);  
+  
+//   printf("angle_across %d", angle);
+  angle=(angle/45)/2;
+//   printf(" -> %d\n", angle);
+  
+  int x,q,y,x_min,x_max,q_min,q_max,i;
+  int x_step=0;
+  
+  q_max=y_0+bitmap_across[angle].y_max-bitmap_across[angle].x_min;     
+  q_min=y_0-bitmap_across[angle].y_min-bitmap_across[angle].x_min;     
+  x_min=x_0-bitmap_across[angle].y_max-bitmap_across[angle].x_min;     
+  x_max=x_0-bitmap_across[angle].y_max+bitmap_across[angle].x_max;     
+  
+//   printf("x0, y0 -> %d, %d\n", x_0,y_0);
+//   printf("qmin, qmax -> %d, %d\n", q_min,q_max);
+//   printf("xmin, xmax -> %d, %d\n", x_min,x_max);
+  
+  for(i=0;i<=1;i++){
+      for(q=q_max; q>=q_min;q--){
+       y=q;
+       for(x=x_min+x_step; x<=x_max+x_step;x++){
       
-       ShmapSetCellFlag(x,x+q,MAP_FLAG_PATH_SHAPE);
-      
+       ShmapSetCellFlag(x,y,MAP_FLAG_PATH_SHAPE);
+       
+//     printf("x, q -> %d, %d\t", x,q);
+//     printf("x, y -> %d, %d\n", x,(y));
+  
+       if (!ShmapIsFreeCell(x,y)){     //1 no obstacle,-1 no in map, 0 otherwise
+         return false;
+       }
+       y++;
+      }
+      x_step++;
+    }
+    q_max--;
+    x_max++;
+    x_step=0;
+  }
+  
+  return true;
+}
+
+bool check_bot_position(int x_0, int y_0, bitmap_dim bitmap_straight[],bitmap_dim bitmap_across[], int angle){
+  
+  angle=angle % 360;
+  
+  if(angle % 90){
+    return check_bot_position_across(x_0,y_0,bitmap_across,angle);
+  }
+  else{          
+    return check_bot_position_straight(x_0,y_0,bitmap_straight,angle);
+  }
+}
 // int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
 //   
 //   double angle=0; 
@@ -186,10 +246,12 @@ int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, d
         struct map *map = ShmapIsMapInit();
        bool valid;
        
-       bitmap_dim_straight bitmap_straight[4];
-       
-       init_bitmap_straight(bitmap_straight);
+       bitmap_dim bitmap_straight[4];
+       bitmap_dim bitmap_across[4];
        
+       init_bitmap(bitmap_straight, BITMAP_AB, BITMAP_AF, BITMAP_W2);
+       init_bitmap(bitmap_across, BITMAP_AB_ACROSS, BITMAP_AF_ACROSS, BITMAP_W2_ACROSS);
+               
         if (!map) return -1;
        // Transform real data in cell data
         ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
@@ -244,8 +306,7 @@ int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, d
                }
 
     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
-        const struct { int x; int y; int angle;} neighbours[] = { { 0, 1, 90}, /*{ 1, 1},*/ { 1, 0, 0}, /*{-1, 1},*/
-                                                          {-1, 0, 180}/*, {-1,-1}*/, { 0,-1, 270}/*, { 1,-1}*/};
+        const struct { int x; int y; int angle;} neighbours[] = { { 0, 1, 90}, { 1, 1, 45}, { 1, 0, 0}, {-1, 1, 135}, {-1, 0, 180},{-1,-1, 225}, { 0,-1, 270}, { 1,-1, 315}};
 
                for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
                         xcontig = x + neighbours[i].x;
@@ -264,7 +325,8 @@ int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, d
 //                                                 pushNodeInOrder(contig);
 //                                         }
                                          //int angle=get_angle_between_cells(x,y,xcontig,ycontig);
-                                         if(check_bot_position_streight(xcontig,ycontig,bitmap_straight,neighbours[i].angle)){
+                                         
+                                         if(check_bot_position(xcontig,ycontig,bitmap_straight,bitmap_across,neighbours[i].angle)){
                                             // add to prioritySet.
                                              contig->g=nbest->g + cost;
                                              contig->f = contig->g + contig->h;

diff --git a/src/pathplan/aalgorithm.h b/src/pathplan/aalgorithm.h
index ef7310d05d55d269cc6f7154854ae72600bb6d18..062e56241ad2be204028b9ec56e7c1695405a527 100644 (file)
--- a/src/pathplan/aalgorithm.h
+++ b/src/pathplan/aalgorithm.h
@@ -39,12 +39,16 @@ typedef struct _GraphMapCell {
 #define BITMAP_AF 2    //AF/MAP_CELL_SIZE_MM 
 #define BITMAP_W2 6    //W/2/MAP_CELL_SIZE_MM 
 
+#define BITMAP_AB_ACROSS 6     //AB/MAP_CELL_SIZE_MM 
+#define BITMAP_AF_ACROSS 2     //AF/MAP_CELL_SIZE_MM 
+#define BITMAP_W2_ACROSS 5     //W/2/MAP_CELL_SIZE_MM 
+
 typedef struct {
   int x_min;
   int x_max;
   int y_min;
   int y_max;
-} bitmap_dim_straight;
+} bitmap_dim;
 
 #ifdef __cplusplus
 extern "C" {