From: pokormat Date: Wed, 20 Apr 2011 13:13:41 +0000 (+0200) Subject: pathplan: repair bugs in check functions X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/ebf459ec08dc421d083f9c673535b67b187eeb7f pathplan: repair bugs in check functions --- diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c index e763ee62..7e8ea6cc 100644 --- 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 } ///
  • 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 ef7310d0..062e5624 100644 --- 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" {