#include "aalgorithm.h"
#include "pathqueue.h"
#include "map.h"
+#include <robodim.h>
#include <math.h>
#include <stdlib.h>
+
//#include "robot.h"
/**
* @{
*/
#define SQRT2 M_SQRT2 /**< @brief Root square of two.*/
-#define WALL_COST 1000 /**< Cost of jumping a wall. */
-#define PI M_PI
+#define WALL_COST 1000 /**< Cost of jumping a wall. */
+#define PI M_PI /**< @brief Pi constant.*/
+#define NBR_OF_POLYGON_LINES 4 // number of polygon lines, polygon is shape of robot, if robot is rectagle, then number is 4
/**@}*/
/**
#endif
+
+typedef struct {
+ int x;
+ int y;
+} cell_coordinates_t;
+
+typedef struct {
+ double x;
+ double y;
+} vect_coordinates_t;
+
+
+typedef struct {
+ cell_coordinates_t *p_first_cell;
+ int nbr_mask_cells;
+} mask_t;
+
+typedef struct {
+ vect_coordinates_t points[NBR_OF_POLYGON_LINES];
+} vect_mask_t;
+
+static cell_coordinates_t angle_idx_2_xy[] = {{ 1, 0}, { 1, 1}, { 0, 1}, {-1, 1}, {-1, 0},{-1,-1}, { 0,-1}, { 1,-1}};
+
+static mask_t mask_0pi, mask_pi4, mask_rot;
+
+const struct { int r1; int r2; int r3; int r4;} angle_idx_2_rot_mat[] = { { 1, 0, 1, 0}, { 0, 1, -1, 0}, { -1, 0, 0, -1}, {0, -1, 1,0}};
+
+//int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
+int get_angle_index(double angle);
+
+void vect_m_2_grid_m(vect_mask_t *p_vect_mask, int vect_mask_dim, mask_t *p_grid_mask, int nbr_grid_mask_cells);
+int init_aalgorithm(void);
+void point_2_cell(double x, double y ,cell_coordinates_t *p_new_coor);
+void arc_2_grid(double r, double x1, double y1, double x2, double y2, double x_r, double y_r, mask_t *p_grid_mask, int nbr_grid_mask_cells);
+static bool is_pos_reach(int *p_x_ref, int *p_y_ref, mask_t *p_mask, int angle_index);
+static bool is_angle_idx_even(int angle_index);
+static cell_coordinates_t transform( cell_coordinates_t coor, int angle_index);
+
+
static void pushNodeInOrder(GraphMapCell *cell);
float cFunction(int x1, int y1, int x2, int y2);
void calculateMapHeuristic(int xgoal, int ygoal);
void initGraphStructure(void);
int getPathLength(int xstart, int ystart, int xgoal, int ygoal);
-#ifndef TESTBITMAPS
-void init_bitmap(bitmap_dim bitmap[], int bitmap_ab, int bitmap_af, int bitmap_w2);
-bool check_bot_position_straight(int x_0, int y_0, bitmap_dim bitmap_straight[], int angle);
-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);
+int init_aalgorithm(void) {
+
+
+ vect_mask_t vect_mask_0pi, vect_mask_pi4;
+
+ mask_rot.nbr_mask_cells = 0;
+ mask_0pi.nbr_mask_cells = 0;
+ mask_pi4.nbr_mask_cells = 0;
+
+ // init vect_mask_0pi
+ vect_mask_0pi.points[0].x = - ROBOT_AXIS_TO_BACK_M;
+ vect_mask_0pi.points[0].y = (double) ( - ROBOT_WIDTH_M / 2);
+
+ vect_mask_0pi.points[1].x = ROBOT_AXIS_TO_FRONT_M;
+ vect_mask_0pi.points[1].y = vect_mask_0pi.points[0].y;
+
+ vect_mask_0pi.points[2].x = vect_mask_0pi.points[1].x;
+ vect_mask_0pi.points[2].y = (double)(ROBOT_WIDTH_M / 2);
+
+ vect_mask_0pi.points[3].x = vect_mask_0pi.points[0].x;
+ vect_mask_0pi.points[3].y = vect_mask_0pi.points[2].y;
+
+ // init vect_mask_pi4 as rotation vect_mask_0pi
+ int i;
+ vect_coordinates_t new_coor;
+ for(i = 0; (i < sizeof(vect_mask_0pi.points) / sizeof(vect_mask_0pi.points[0])); i++) {
+ new_coor.x = cos(PI/4) * vect_mask_0pi.points[i].x - sin(PI/4) * vect_mask_0pi.points[i].y;
+ new_coor.y = sin(PI/4) * vect_mask_0pi.points[i].x + cos(PI/4) * vect_mask_0pi.points[i].y;
+ vect_mask_pi4.points[i] = new_coor;
+
+ }
+
+ //init mask_0pi dynamic array
+ int init_size = 0;
+ init_size = abs(2 * (int) ceil((vect_mask_0pi.points[1].x - vect_mask_0pi.points[0].x) / MAP_CELL_SIZE_M) + 1); // 24
+ init_size += abs(2 * (int) ceil((vect_mask_0pi.points[2].y - vect_mask_0pi.points[1].y) / MAP_CELL_SIZE_M) + 1); // 24 + 26
+
+ //init mask_pi4 and mask_rot dynamic array
+ init_size += 4 * init_size; // 50 + 250 = 300
+
#endif
-//int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
-int get_angle_index(double angle);
-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;
-
+ mask_rot.p_first_cell = (cell_coordinates_t *) calloc((size_t)init_size, (size_t)sizeof(cell_coordinates_t));
+ if(mask_rot.p_first_cell == NULL) {
+ printf("init_aalgorithm: malloc error!\n");
+ return 1;
+ }
+
+ vect_coordinates_t ref_point;
+ ref_point.x = 0;
+ ref_point.y = 0;
+
+ double r_f = sqrt(vect_mask_0pi.points[1].x * vect_mask_0pi.points[1].x + vect_mask_0pi.points[1].y * vect_mask_0pi.points[1].y); // radius to front of robot
+
+ mask_rot.nbr_mask_cells = 0;
+ arc_2_grid(r_f, vect_mask_0pi.points[1].x, vect_mask_0pi.points[1].y, vect_mask_pi4.points[1].x, vect_mask_pi4.points[1].y, ref_point.x, ref_point.y, &mask_rot, init_size);
+
+ mask_t tmp_mask_rot;
+ tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+ arc_2_grid(r_f, vect_mask_0pi.points[2].x, vect_mask_0pi.points[2].y, vect_mask_pi4.points[2].x, vect_mask_pi4.points[2].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+
+ double r_b = sqrt(vect_mask_0pi.points[0].x * vect_mask_0pi.points[0].x + vect_mask_0pi.points[0].y * vect_mask_0pi.points[0].y); // radius to back of robot
+
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+ tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ arc_2_grid(r_b, vect_mask_0pi.points[0].x, vect_mask_0pi.points[0].y, vect_mask_pi4.points[0].x, vect_mask_pi4.points[0].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells );
+
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+ tmp_mask_rot.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ arc_2_grid(r_b, vect_mask_0pi.points[3].x, vect_mask_0pi.points[3].y, vect_mask_pi4.points[3].x, vect_mask_pi4.points[3].y, ref_point.x, ref_point.y, &tmp_mask_rot, init_size - mask_rot.nbr_mask_cells);
+
+ mask_rot.nbr_mask_cells += tmp_mask_rot.nbr_mask_cells;
+ tmp_mask_rot.nbr_mask_cells = 0;
+
+ mask_0pi.nbr_mask_cells = 0;
+ mask_0pi.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+
+ vect_m_2_grid_m(&vect_mask_0pi, sizeof(vect_mask_0pi.points)/ sizeof(vect_mask_0pi.points[0]), &mask_0pi, init_size - mask_rot.nbr_mask_cells);
+
+ mask_pi4.nbr_mask_cells = 0;
+ mask_pi4.p_first_cell = mask_0pi.p_first_cell + mask_0pi.nbr_mask_cells;
+
+ vect_m_2_grid_m(&vect_mask_pi4, sizeof(vect_mask_pi4.points)/ sizeof(vect_mask_pi4.points[0]), &mask_pi4, init_size - mask_rot.nbr_mask_cells - mask_0pi.nbr_mask_cells);
+
+ init_size = mask_0pi.nbr_mask_cells + mask_pi4.nbr_mask_cells + mask_rot.nbr_mask_cells;
+
+ tmp_mask_rot.p_first_cell = (cell_coordinates_t *) realloc( mask_rot.p_first_cell, (unsigned int)(init_size * sizeof(cell_coordinates_t)));
+ if(tmp_mask_rot.p_first_cell == NULL) {
+ printf("init_aalgorithm: malloc error!\n");
+ return 1;
+ }
+
+ mask_rot.p_first_cell = tmp_mask_rot.p_first_cell;
+
+ mask_0pi.p_first_cell = mask_rot.p_first_cell + mask_rot.nbr_mask_cells;
+ mask_pi4.p_first_cell = mask_0pi.p_first_cell + mask_0pi.nbr_mask_cells;
+
+ return 0;
}
+void point_2_cell(double x, double y ,cell_coordinates_t *p_new_coor) {
-bool check_bot_position_straight(int x_0, int y_0, bitmap_dim bitmap_straight[], int angle){
-
- angle=(angle % 360);
-// printf("angle_straight %d", 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++){
- for(y=y_0-bitmap_straight[angle].y_min; y<=y_0+bitmap_straight[angle].y_max;y++){
-
- ShmapSetCellFlag(x,y,MAP_FLAG_PLAN_MASK);
-
-
-// printf("x, y -> %d, %d\n", x,y);
-
- if (!ShmapIsFreeCell(x,y)){ //1 no obstacle,-1 no in map, 0 otherwise
- return false;
- }
- }
- }
-
- return true;
+ p_new_coor->x = (int)floor(x / MAP_CELL_SIZE_M);
+ p_new_coor->y = (int)( - floor(y / MAP_CELL_SIZE_M)); // y axis in cells has reverse orientation
+
}
-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,y,MAP_FLAG_PLAN_MASK);
-
-// 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;
+void vect_m_2_grid_m(vect_mask_t *p_vect_mask, int vect_mask_dim, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
+
+ int i_1, i_2;
+ int nbr_of_steps = 0;
+ double step = MAP_CELL_SIZE_M;
+ double dx12 = 0;
+ double k = 0;
+ double q = 0;
+
+ vect_coordinates_t tmp_coor;
+
+ cell_coordinates_t *p_cell = p_grid_mask->p_first_cell;
+
+ for(i_1 = 0; i_1 < vect_mask_dim; i_1++) {
+
+ i_2 = (i_1 == (vect_mask_dim - 1)) ? 0 : (i_1 + 1);
+
+ dx12 = (p_vect_mask->points[i_2].x - p_vect_mask->points[i_1].x); // dx12 = x2 - x1
+ if(dx12 != 0) {
+ k = (p_vect_mask->points[i_2].y - p_vect_mask->points[i_1].y) / dx12; // k = (y2 - y1)/(x2 - x1)
+ q = (p_vect_mask->points[i_2].x * p_vect_mask->points[i_1].y - p_vect_mask->points[i_1].x * p_vect_mask->points[i_2].y) / dx12; // q = (x2 * y1 - x1 * y2) / (x2 -x1)
+ nbr_of_steps = abs((int) ceil(dx12 / step));
+ }
+ else {
+ nbr_of_steps = abs((int) ceil((p_vect_mask->points[i_2].y - p_vect_mask->points[i_1].y) / step));
+ }
+
+ int i;
+ for(i = 0; i <= (nbr_of_steps + 1); i++) {
+
+ if(dx12 != 0) {
+ if(p_vect_mask->points[i_2].x > p_vect_mask->points[i_1].x) {
+ tmp_coor.x = p_vect_mask->points[i_1].x + i * step;
+ tmp_coor.y = k * tmp_coor.x + q;
+ }
+ else{
+ tmp_coor.x = p_vect_mask->points[i_1].x - i * step;
+ tmp_coor.y = k * tmp_coor.x + q;
+ }
+ }
+ else {
+ if(p_vect_mask->points[i_2].y > p_vect_mask->points[i_1].y) {
+ tmp_coor.x = p_vect_mask->points[i_1].x;
+ tmp_coor.y = p_vect_mask->points[i_1].y + i * step;
+ }
+ else {
+ tmp_coor.x = p_vect_mask->points[i_1].x;
+ tmp_coor.y = p_vect_mask->points[i_1].y - i * step;
+ }
+
+ }
+ point_2_cell(tmp_coor.x, tmp_coor.y , p_cell);
+ p_cell++;
+ p_grid_mask->nbr_mask_cells++;
+
+ if(p_cell > (p_grid_mask->p_first_cell + nbr_grid_mask_cells)) {
+ printf("PATHPLAN: vect_m_2_grid_m : end of alocated memory, not possible write after that \n");
+ break;
+ }
+ }
+
}
- 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);
- }
+// point x1 is asigned to smaller angle
+void arc_2_grid(double r, double x1, double y1, double x2, double y2, double x_r, double y_r, mask_t *p_grid_mask, int nbr_grid_mask_cells) {
+
+ double a_step = MAP_CELL_SIZE_M / 2;
+ // vectors end point x1, ref point and end point x2, ref point
+ vect_coordinates_t vect_sx1, vect_sx2;
+
+ vect_sx1.x = (double) (x1 - x_r);
+ vect_sx1.y = (double) (y1 - y_r);
+ vect_sx2.x = (double) (x2 - x_r);
+ vect_sx2.y = (double) (x2 - y_r);
+
+ // radius r is size of vector
+ double radius = sqrt(vect_sx1.x * vect_sx1.x + vect_sx1.y * vect_sx1.y);
+
+ // angle between two vectors, alpha = arccos((vect_sx1 * vect_sx2) / (|vect_sx1| * |vect_sx2|))
+ double alpha = 0;
+ double acos_arg = (vect_sx1.x * vect_sx2.x + vect_sx1.y * vect_sx2.y) / ( radius * radius );
+
+ if((acos_arg <= 1.0) && (acos_arg >= -1.0)) {
+ alpha = acos((vect_sx1.x * vect_sx2.x + vect_sx1.y * vect_sx2.y) / ( radius * radius ));
+ }
+ else {
+ printf("PATHPLAN: arc_2_grid : argument of acos is out of range < -1, 1 > \n");
+ }
+
+ double alpha_0 = atan2(y1, x1);
+ double alpha_i = 0;
+
+ // lenght of arc
+ double a = radius * alpha;
+
+ double alpha_step = alpha / (a / a_step);
+
+ int nbr_of_angle_steps = (int) ceil(a / a_step);
+
+ cell_coordinates_t *p_cell = NULL;
+
+ int i;
+ for(i = 0; i < nbr_of_angle_steps; i++) {
+
+ alpha_i = alpha_0 + i * alpha_step;
+
+ double x = x1 * cos(alpha_i) - y1 * sin(alpha_i);
+ double y = y1 * sin(alpha_i) + y1 * cos(alpha_i);
+
+ p_cell = p_grid_mask->p_first_cell + i;
+
+ if(p_cell < (p_grid_mask->p_first_cell + nbr_grid_mask_cells)) {
+ point_2_cell(x, y, p_cell);
+ p_grid_mask->nbr_mask_cells++;
+ }
+ else {
+ printf("PATHPLAN: arc_2_grid : end of alocated memory, not possible write after that \n");
+ printf("PATHPLAN: arc_2_grid : pointer to cell > max address: %X > %X \n", (unsigned int) p_cell, (unsigned int) (p_grid_mask->p_first_cell + nbr_grid_mask_cells));
+ break;
+ }
+
+ }
+
}
-// int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
-//
-// double angle=0;
-//
-// angle= atan2((double)(y_1-y_0),(double)(x_1-x_0)); //angle in range -pi, pi
-// angle=(angle*360)/(2*pi()); //angle in deg.
-//
-// return (int) angle;
-// }
+
+/**
+ * Returns angle index (0..2*PI -> 0..7, step is PI/4).
+ *
+ * @param[in] angle Angle in radians (0..2*PI).
+ * @return Angle index (0..7, 2*PI = 0).
+ */
int get_angle_index(double angle)
{
int i;
*x = (c - &graph[0][0]) % MAP_WIDTH;
}
+static bool is_pos_reach(int *p_x_ref, int *p_y_ref, mask_t *p_mask, int angle_index)
+{
+ cell_coordinates_t *p_cell_coor;
+ cell_coordinates_t coor;
+
+ p_cell_coor = p_mask->p_first_cell;
+
+ int i;
+ for(i = 0; i < p_mask->nbr_mask_cells; i++) {
+
+ //transform mask to right orientation (mirror and rotation)
+ coor = transform(*p_cell_coor, angle_index);
+
+ coor.x += *p_x_ref;
+ coor.y += *p_y_ref;
+
+ if ((!ShampIsPointInMap(coor.x,coor.y)) || (!ShmapIsFreeCell(coor.x, coor.y))) {
+ return false;
+ }
+
+ p_cell_coor = p_mask->p_first_cell + (sizeof(cell_coordinates_t)) * i;
+
+ }
+ return true;
+}
+static bool is_angle_idx_even(int angle_index)
+{
+ return (((angle_index) % 2) == 0);
+}
+
+static cell_coordinates_t transform( cell_coordinates_t coor, int angle_index)
+{
+ cell_coordinates_t new_coor;
+ // mirror for rotation mask, other masks are immune
+ if (!is_angle_idx_even(angle_index)) {
+ coor.y = -coor.y;
+ }
+
+ angle_index = angle_index % 2;
+
+ new_coor.x = coor.x * angle_idx_2_rot_mat[angle_index].r1 + coor.y * angle_idx_2_rot_mat[angle_index].r2;
+ new_coor.y = coor.x * angle_idx_2_rot_mat[angle_index].r3 + coor.y * angle_idx_2_rot_mat[angle_index].r4;
+
+ return new_coor;
+}
int aAlgorithm_shape(double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, double start_angle, double goal_angle, GraphMapCell **original_path)
{
*/
GraphMapCell *nbest, *start, *goal, *contig;
int i, x, y, x_back, y_back;
- int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
+ int xstart, ystart, xgoal, ygoal;
struct map *map = ShmapIsMapInit();
bool valid;
- int start_angle_index, goal_angle_index, angle_index;
+ int angle_index;
double angle;
+ mask_t mask;
- 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);
+ init_aalgorithm();
+
+ if (!map) return -1;
- // Clear previous masks
for (y=0;y<MAP_HEIGHT;y++){
for(x=0;x<MAP_WIDTH;x++){
map->cells[y][x].flags &= ~(MAP_FLAG_PLAN_MASK);
if (!valid)
return -1;
start = &graph[ystart][xstart];
- start_angle_index = get_angle_index(start_angle);
ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
if (!valid)
return -1;
goal = &graph[ygoal][xgoal];
- goal_angle_index = get_angle_index(goal_angle);
/**@{*/
/// <ol>
nbest->processed = 1;
/// <li> IF nbest == goal ; EXIT
- if (nbest==goal) {
+ if (nbest == goal) {
break;
}
- /// <li> Expand nbest :for all x E Star(nbest) that are not in C
+ /// <li> Expand nbest :for all x E Star(nbest) that are not in C
- // determines angle between cells
- if (nbest != start) {
- GraphCell2XY(nbest->backpointer, &x_back, &y_back); // determines coordinates of previous cell
- angle = PI + atan2((double)(y - y_back), (double)(x - x_back)); // angle between cells, atan2 return angle in the range -pi, pi
- angle_index = get_angle_index(angle);
- }
- else {
- angle_index = start_angle_index;
- }
-
- 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}};
+ // determines angle between cells
+ if (nbest == start) {
+ angle_index = get_angle_index(start_angle);
+ }
+ else {
+ GraphCell2XY(nbest->backpointer, &x_back, &y_back); // determines coordinates of previous cell
+ angle = PI + atan2((double)(y - y_back), (double)(x - x_back)); // angle between cells, atan2 return angle in the close range < -pi, pi >
+ angle_index = get_angle_index(angle);
+ }
+
+ //check position direct before robot, one cell forward
+ if (is_angle_idx_even(angle_index)) {
+ mask = mask_0pi;
+ }
+ else {
+ mask = mask_pi4;
+ }
- for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
- xcontig = x + neighbours[i].x;
- ycontig = y + neighbours[i].y;
-
- if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
- double cost = cFunction(x, y, xcontig, ycontig);
- contig = &graph[ycontig][xcontig];
+ cell_coordinates_t coor_contig;
+ bool rot_left = true;
+ bool rot_right = true;
+
+ for (i = 0; i < 7; i++) {
+
+ angle_index = angle_index + (int) pow((double) (-1), (double) i); // if angle_index before for() is for example 3, then for i = <0..7>, angle_index is 3, 2, 4, 1, 5, 0, 6
+ angle_index = (angle_index < 0) ? (8 - angle_index) : angle_index;
+ angle_index = (angle_index > 8) ? (angle_index - 8) : angle_index;
+
+ coor_contig = angle_idx_2_xy[angle_index]; // get relative coordinates from nbest to contiguous cell
+
+ if(ShmapIsCellInMap(coor_contig.x, coor_contig.y) && (graph[coor_contig.x][coor_contig.y].processed == 0)) {
+ double cost = cFunction(x, y, coor_contig.x, coor_contig.y);
+ contig = &graph[coor_contig.y][coor_contig.x];
/// <li> IF is not in Priorityset add to prioritySet.
if (!contig->in_queue) {
-// if(ShmapIsFreeCell(xcontig, ycontig)) {
-// // add to prioritySet.
-// contig->g=nbest->g + cost;
-// contig->f = contig->g + contig->h;
-// contig->backpointer=nbest;
-// pushNodeInOrder(contig);
-// }
- //int angle=get_angle_between_cells(x,y,xcontig,ycontig);
-
- 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;
- contig->backpointer=nbest;
- pushNodeInOrder(contig);
-
- }
+
+ if((i == 0) && is_pos_reach(&x, &y, &mask, angle_index)) {
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+ mask = mask_rot;
+ }
+ else {
+ if(rot_left && ((int) pow((double) (-1), (double) i) < 0 ) && is_pos_reach(&x, &y, &mask, angle_index)) {
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+ }
+ else {
+ rot_left = false;
+ }
+
+ if(rot_right && ((int) pow((double) (-1), (double) i) > 0 ) && is_pos_reach(&x, &y, &mask, angle_index)) {
+ contig->g=nbest->g + cost;
+ contig->f = contig->g + contig->h;
+ contig->backpointer=nbest;
+ pushNodeInOrder(contig);
+ }
+ else {
+ rot_right = false;
+ }
+ }
} else if (nbest->g + cost < contig->g) {
/// <li> ELSE update backpointer to point to nbest.
contig->backpointer=nbest;
contig->g = nbest->g + cost;
}
}
- }
+
+ if ((!rot_left) && (!rot_right)) {
+ break;
+ }
+ }
+
/// </ol>
#ifndef CONFIG_PP_USES_RBTREE
} while (graph_queue);
} while (!RB_EMPTY_ROOT(&graph_queue));
#endif
- if (nbest==goal) {
+ if (nbest == goal) {
int nbpoints, x, y;
GraphMapCell *cell;
projects / eurobot / public.git / blobdiff