src/pathplan/aalgorithm.c

   1 /**
   2  * @file        aalgorithm.c
   3  * @brief       Implementation of the A* algorithm. Main functions.
   4  * @author      Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
   5  *
   6  * @note The author asumes that the reader knows how A* algorithm
   7  *       works. In this documentation only the implementation is
   8  *       explained. More information "Principles of Robot Motion" by
   9  *       H. Choset&others (Algorithm 24, p531)
  10  *
  11 */
  12 /** @addtogroup ppi*/
  13 /**@{*/
  14 #define _ISOC99_SOURCE              /* To have INFINITY */
  15 #define _XOPEN_SOURCE               /* To have M_SQRT2 etc. */
  16 #include "aalgorithm.h"
  17 #include "pathqueue.h"
  18 #include "map.h"
  19 #include <math.h>
  20 #include <stdlib.h>
  21 //#include "robot.h"
  22
  23 /**
  24  * @name A* Constants
  25  * @{
  26  */
  27 #define SQRT2 M_SQRT2           /**< @brief Root square of two.*/
  28 #define WALL_COST       1000 /**< Cost of jumping a wall. */
  29 /**@}*/
  30
  31 /**
  32  * @name A* Main Structure
  33  * @{
  34  */
  35 static GraphMapCell graph[MAP_HEIGHT][MAP_WIDTH]; /**< @brief Graph structure used in A*.  */
  36 /**@}*/
  37
  38 #ifndef CONFIG_PP_USES_RBTREE
  39 static GraphMapCell *graph_queue;   /**< Pointer to the first cell in the list */
  40 #else
  41 static struct rb_root graph_queue;   /**< rbtree root */
  42
  43 #define __compiler_offsetof(a,b) __builtin_offsetof(a,b) /* For GCC 4 */
  44 #ifdef __compiler_offsetof
  45 #define offsetof(TYPE,MEMBER) __compiler_offsetof(TYPE,MEMBER)
  46 #else
  47 #define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
  48 #endif
  49
  50 /**
  51  * container_of - cast a member of a structure out to the containing structure
  52  * @ptr:        the pointer to the member.
  53  * @type:       the type of the container struct this is embedded in.
  54  * @member:     the name of the member within the struct.
  55  *
  56  */
  57 #define container_of(ptr, type, member) ({                      \
  58         const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
  59         (type *)( (char *)__mptr - offsetof(type,member) );})
  60
  61 #endif
  62
  63 static void pushNodeInOrder(GraphMapCell *cell);
  64 float cFunction(int x1, int y1, int x2, int y2);
  65 void calculateMapHeuristic(int xgoal, int ygoal);
  66 float calculateCost(int x1, int y1, int x2, int y2);
  67 void initGraphStructure(void);
  68 int getPathLength(int xstart, int ystart, int xgoal, int ygoal);
  69
  70 #ifndef TESTBITMAPS
  71 void init_bitmap(bitmap_dim bitmap[], int bitmap_ab, int bitmap_af, int bitmap_w2);
  72 bool check_bot_position_straight(int x_0, int y_0, bitmap_dim bitmap_straight[], int angle);
  73 bool check_bot_position_across(int x_0, int y_0, bitmap_dim bitmap_across[], int angle);
  74 bool check_bot_position(int x_0, int y_0, bitmap_dim bitmap_par[], int angle);
  75 #endif
  76 void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]);
  77 bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle);
  78 //int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1);
  79 //int get_round_actual_angle();
  80
  81 void init_bitmap_straight(bitmap_dim_straight bitmap_straight[]){
  82
  83   bitmap_straight[0].x_min=-BITMAP_AB;
  84   bitmap_straight[0].x_max=BITMAP_AF;
  85   bitmap_straight[0].y_min=-BITMAP_W2;
  86   bitmap_straight[0].y_max=BITMAP_W2;
  87
  88   bitmap_straight[1].x_min=-BITMAP_W2;
  89   bitmap_straight[1].x_max=BITMAP_W2;
  90   bitmap_straight[1].y_min=-BITMAP_AB;
  91   bitmap_straight[1].y_max=BITMAP_AF;
  92
  93   bitmap_straight[2].x_min=-BITMAP_AF;
  94   bitmap_straight[2].x_max=BITMAP_AB;
  95   bitmap_straight[2].y_min=-BITMAP_W2;
  96   bitmap_straight[2].y_max=BITMAP_W2;
  97
  98   bitmap_straight[3].x_min=-BITMAP_W2;
  99   bitmap_straight[3].x_max=BITMAP_W2;
 100   bitmap_straight[3].y_min=-BITMAP_AF;
 101   bitmap_straight[3].y_max=BITMAP_AB;
 102
 103 }
 104
 105 bool check_bot_position_streight(int x_0, int y_0, bitmap_dim_straight bitmap_straight[], int angle){
 106
 107   if (angle==0){        //angle must be in range 90-360
 108     angle=360;
 109   }
 110
 111   angle=360/angle;
 112
 113   int x,y;
 114
 115   for(x=x_0-bitmap_straight[angle].x_min; x<=x_0+bitmap_straight[angle].x_max;x++){
 116     for(y=y_0-bitmap_straight[angle].y_min; y<=y_0+bitmap_straight[angle].y_max;y++){
 117
 118       ShmapSetCellFlag(x,y,MAP_FLAG_PATH_SHAPE);
 119
 120       if (!ShmapIsFreeCell(x,y)){       //1 no obstacle,-1 no in map, 0 otherwise
 121         return false;
 122       }
 123     }
 124   }
 125
 126   return true;
 127 }
 128
 129
 130         ShmapSetCellFlag(x,x+q,MAP_FLAG_PATH_SHAPE);
 131
 132 // int get_angle_between_cells(int x_0, int y_0, int x_1, int y_1){
 133 //
 134 //   double angle=0;
 135 //
 136 //   angle= atan2((double)(y_1-y_0),(double)(x_1-x_0)); //angle in range -pi, pi
 137 //   angle=(angle*360)/(2*pi());        //angle in deg.
 138 //
 139 //   return (int) angle;
 140 // }
 141
 142 // int get_round_actual_angle(){
 143 //
 144 //   double x, y, phi;
 145 //   int phi_int;
 146 //
 147 //   robot_get_est_pos(&x,&y,&phi);
 148 //
 149 //   phi_int=(int)phi;
 150 //   phi_int=phi_int-(phi_int % 45);
 151 //
 152 //   return phi_int;
 153 // }
 154
 155 /**
 156  * Returns X and Y index of a graph cell identified by a pointer.
 157  *
 158  * @param[in] c Pointer to the cell in question.
 159  * @param[out] x Cell's X index.
 160  * @param[out] y Cell's Y index.
 161  */
 162 void GraphCell2XY(GraphMapCell *c, int *x, int *y)
 163 {
 164         *y = (c - &graph[0][0]) / MAP_WIDTH;
 165         *x = (c - &graph[0][0]) % MAP_WIDTH;
 166 }
 167
 168
 169
 170 int aAlgorithm_shape(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
 171 {
 172         /**
 173          * prioritySet : Set of nodes to process order by heuristic distance
 174          */
 175 #ifndef CONFIG_PP_USES_RBTREE
 176         graph_queue = NULL;
 177 #else
 178         graph_queue = RB_ROOT;
 179 #endif
 180         /**
 181          * nbest : Best node to reach the goal
 182          */
 183         GraphMapCell *nbest, *start, *goal, *contig;
 184         int i, x, y;
 185         int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
 186         struct map *map = ShmapIsMapInit();
 187         bool valid;
 188
 189         bitmap_dim_straight bitmap_straight[4];
 190
 191         init_bitmap_straight(bitmap_straight);
 192
 193         if (!map) return -1;
 194         // Transform real data in cell data
 195         ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
 196         if (!valid)
 197                 return -1;
 198         start = &graph[ystart][xstart];
 199         ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
 200         if (!valid)
 201                 return -1;
 202         goal = &graph[ygoal][xgoal];
 203
 204         /**@{*/
 205         /// <ol>
 206         /// <li> Init graph structure with default values.
 207         initGraphStructure();
 208         /// <li> Calculate Heuristic distances.
 209         calculateMapHeuristic(xgoal, ygoal);
 210
 211         /// <li> Initial values : put start node in queue.
 212         start->g = 0;
 213         start->f = start->g + start->h;
 214         start->backpointer = start;
 215         pushNodeInOrder(start);
 216
 217     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
 218
 219     //printf("*cntr %d \n", cntr);
 220
 221         /// <li> REPEAT until the queue is empty.
 222
 223         do {
 224                 /// <ol>
 225                 /// <li> Pick nbest from prioritySet and remove
 226 #ifndef CONFIG_PP_USES_RBTREE
 227                 nbest = graph_queue;
 228                 graph_queue = graph_queue->next;
 229 #else
 230                 nbest = container_of(rb_first(&graph_queue), GraphMapCell, node);
 231                 rb_erase(&nbest->node, &graph_queue);
 232 #endif
 233
 234                 GraphCell2XY(nbest, &x, &y);
 235 /*                 map->cells[y][x].detected_obstacle = 255; */
 236 /*                 usleep(100000); */
 237
 238                 /// <li> add to processed
 239                 nbest->processed = 1;
 240
 241                 ///  <li> IF nbest == goal ; EXIT
 242                 if (nbest==goal) {
 243                         break;
 244                 }
 245
 246     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
 247         const struct { int x; int y; int angle;} neighbours[] = { { 0, 1, 90}, /*{ 1, 1},*/ { 1, 0, 0}, /*{-1, 1},*/
 248                                                           {-1, 0, 180}/*, {-1,-1}*/, { 0,-1, 270}/*, { 1,-1}*/};
 249
 250                 for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
 251                         xcontig = x + neighbours[i].x;
 252                         ycontig = y + neighbours[i].y;
 253
 254                         if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
 255                                 double cost = cFunction(x, y, xcontig, ycontig);
 256                                 contig = &graph[ycontig][xcontig];
 257                                 /// <li> IF is not in Priorityset add to prioritySet.
 258                                 if (!contig->in_queue) {
 259 //                                         if(ShmapIsFreeCell(xcontig, ycontig)) {
 260 //                                                 //  add to prioritySet.
 261 //                                                 contig->g=nbest->g + cost;
 262 //                                                 contig->f = contig->g + contig->h;
 263 //                                                 contig->backpointer=nbest;
 264 //                                                 pushNodeInOrder(contig);
 265 //                                         }
 266                                           //int angle=get_angle_between_cells(x,y,xcontig,ycontig);
 267                                           if(check_bot_position_streight(xcontig,ycontig,bitmap_straight,neighbours[i].angle)){
 268                                              // add to prioritySet.
 269                                              contig->g=nbest->g + cost;
 270                                              contig->f = contig->g + contig->h;
 271                                              contig->backpointer=nbest;
 272                                              pushNodeInOrder(contig);
 273
 274                                           }
 275                                 } else if (nbest->g + cost < contig->g) {
 276                                         /// <li>  ELSE update backpointer to point to nbest.
 277                                         contig->backpointer=nbest;
 278                                         contig->g = nbest->g + cost;
 279                                 }
 280                         }
 281                 }
 282                 /// </ol>
 283 #ifndef CONFIG_PP_USES_RBTREE
 284         } while (graph_queue);
 285 #else
 286         } while (!RB_EMPTY_ROOT(&graph_queue));
 287 #endif
 288
 289         if (nbest==goal) {
 290                 int nbpoints, x, y;
 291                 GraphMapCell *cell;
 292
 293                 // Clear previous path
 294                 for (y=0;y<MAP_HEIGHT;y++){
 295                         for(x=0;x<MAP_WIDTH;x++){
 296                                 map->cells[y][x].flags &= ~(MAP_FLAG_PATH | MAP_FLAG_START | MAP_FLAG_GOAL);
 297                         }
 298                 }
 299
 300
 301                 /* Construct forward list for the found path. */
 302                 cell = goal;
 303                 cell->next = NULL;
 304                 nbpoints = 1;
 305                 while (cell != start) {
 306                         cell->backpointer->next = cell;
 307                         cell = cell->backpointer;
 308                         nbpoints++;
 309                         GraphCell2XY(cell, &x, &y);
 310                         map->cells[y][x].flags |= MAP_FLAG_PATH;
 311                 }
 312
 313                 map->cells[ystart][xstart].flags |= MAP_FLAG_START | MAP_FLAG_PATH;
 314                 map->cells[ygoal][xgoal].flags |= MAP_FLAG_GOAL  | MAP_FLAG_PATH;
 315
 316                 *original_path = start;
 317                 return nbpoints;
 318         } else {
 319                 return -1;
 320         }
 321 }
 322 /**
 323  * @name A* Main function
 324  * @{
 325  */
 326
 327 /**
 328  * @brief Search the shortest path between two points
 329  * @param xstart_real   Coordonate X of the start point
 330  * @param ystart_real   Coordonate Y of the start point
 331  * @param xgoal_real    Coordonate X of the goal point
 332  * @param ygoal_real    Coordonate Y of the goal point
 333  * @param original_path [out] Pointer to the first map cell of the
 334  *                            path is stored here. Use
 335  *                            GraphMapCell.next to traverse the found
 336  *                            path.
 337  * @see graph
 338  * @return              -1 if the goal is not founded, the number of points to the goal is founded on success.
 339  *
 340  * This is an implementation of A* Algorithm (Algorithm 24, p531)
 341  * defined in the book "Principles of Robot Motion" by
 342  * H. Choset&others
 343  */
 344
 345 int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
 346 {
 347         /**
 348          * prioritySet : Set of nodes to process order by heuristic distance
 349          */
 350 #ifndef CONFIG_PP_USES_RBTREE
 351         graph_queue = NULL;
 352 #else
 353         graph_queue = RB_ROOT;
 354 #endif
 355         /**
 356          * nbest : Best node to reach the goal
 357          */
 358         GraphMapCell *nbest, *start, *goal, *contig;
 359         int i, x, y;
 360         int xcontig, ycontig, xstart, ystart, xgoal, ygoal;
 361         struct map *map = ShmapIsMapInit();
 362         bool valid;
 363
 364         if (!map) return -1;
 365         // Transform real data in cell data
 366         ShmapPoint2Cell(xstart_real, ystart_real, &xstart, &ystart, &valid);
 367         if (!valid)
 368                 return -1;
 369         start = &graph[ystart][xstart];
 370         ShmapPoint2Cell(xgoal_real, ygoal_real, &xgoal, &ygoal, &valid);
 371         if (!valid)
 372                 return -1;
 373         goal = &graph[ygoal][xgoal];
 374
 375         /**@{*/
 376         /// <ol>
 377         /// <li> Init graph structure with default values.
 378         initGraphStructure();
 379         /// <li> Calculate Heuristic distances.
 380         calculateMapHeuristic(xgoal, ygoal);
 381
 382         /// <li> Initial values : put start node in queue.
 383         start->g = 0;
 384         start->f = start->g + start->h;
 385         start->backpointer = start;
 386         pushNodeInOrder(start);
 387
 388     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
 389
 390     //printf("*cntr %d \n", cntr);
 391
 392         /// <li> REPEAT until the queue is empty.
 393
 394         do {
 395                 /// <ol>
 396                 /// <li> Pick nbest from prioritySet and remove
 397 #ifndef CONFIG_PP_USES_RBTREE
 398                 nbest = graph_queue;
 399                 graph_queue = graph_queue->next;
 400 #else
 401                 nbest = container_of(rb_first(&graph_queue), GraphMapCell, node);
 402                 rb_erase(&nbest->node, &graph_queue);
 403 #endif
 404
 405                 GraphCell2XY(nbest, &x, &y);
 406 /*                 map->cells[y][x].detected_obstacle = 255; */
 407 /*                 usleep(100000); */
 408
 409                 /// <li> add to processed
 410                 nbest->processed = 1;
 411
 412                 ///  <li> IF nbest == goal ; EXIT
 413                 if (nbest==goal) {
 414                         break;
 415                 }
 416
 417     ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
 418         const struct { int x; int y; } neighbours[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
 419                                                           {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
 420
 421                 for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
 422                         xcontig = x + neighbours[i].x;
 423                         ycontig = y + neighbours[i].y;
 424
 425                         if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
 426                                 double cost = cFunction(x, y, xcontig, ycontig);
 427                                 contig = &graph[ycontig][xcontig];
 428                                 /// <li> IF is not in Priorityset add to prioritySet.
 429                                 if (!contig->in_queue) {
 430                                         if(ShmapIsFreeCell(xcontig, ycontig)) {
 431                                                 //  add to prioritySet.
 432                                                 contig->g=nbest->g + cost;
 433                                                 contig->f = contig->g + contig->h;
 434                                                 contig->backpointer=nbest;
 435                                                 pushNodeInOrder(contig);
 436                                         }
 437                                 } else if (nbest->g + cost < contig->g) {
 438                                         /// <li>  ELSE update backpointer to point to nbest.
 439                                         contig->backpointer=nbest;
 440                                         contig->g = nbest->g + cost;
 441                                 }
 442                         }
 443                 }
 444                 /// </ol>
 445 #ifndef CONFIG_PP_USES_RBTREE
 446         } while (graph_queue);
 447 #else
 448         } while (!RB_EMPTY_ROOT(&graph_queue));
 449 #endif
 450
 451         if (nbest==goal) {
 452                 int nbpoints, x, y;
 453                 GraphMapCell *cell;
 454
 455                 // Clear previous path
 456                 for (y=0;y<MAP_HEIGHT;y++){
 457                         for(x=0;x<MAP_WIDTH;x++){
 458                                 map->cells[y][x].flags &= ~(MAP_FLAG_PATH | MAP_FLAG_START | MAP_FLAG_GOAL);
 459                         }
 460                 }
 461
 462
 463                 /* Construct forward list for the found path. */
 464                 cell = goal;
 465                 cell->next = NULL;
 466                 nbpoints = 1;
 467                 while (cell != start) {
 468                         cell->backpointer->next = cell;
 469                         cell = cell->backpointer;
 470                         nbpoints++;
 471                         GraphCell2XY(cell, &x, &y);
 472                         map->cells[y][x].flags |= MAP_FLAG_PATH;
 473                 }
 474
 475                 map->cells[ystart][xstart].flags |= MAP_FLAG_START | MAP_FLAG_PATH;
 476                 map->cells[ygoal][xgoal].flags |= MAP_FLAG_GOAL  | MAP_FLAG_PATH;
 477
 478                 *original_path = start;
 479                 return nbpoints;
 480         } else {
 481                 return -1;
 482         }
 483 }
 484
 485 /** @} */
 486 /**
 487  * @name A* related functions
 488  * This functions have a meaning in theorical A*.
 489  * @{
 490  */
 491
 492 /**
 493  * @brief Calculates the lenght of edge connecting two points
 494  * @param x1    Coordonate X of the first cell
 495  * @param y1    Coordonate Y of the first cell
 496  * @param x2    Coordonate X of the second cell
 497  * @param y2    Coordonate Y of the second cell
 498  * @return      Lenght of edge
 499 */
 500
 501 float cFunction(int x1, int y1, int x2, int y2)
 502 {
 503         float c;
 504         //if(GETMAPPOS(x2, y2)== MAP_WALL) c = WALL_COST;
 505         if(!ShmapIsFreeCell(x2,y2)) c = WALL_COST;
 506         else if( (x1==x2) || (y1 == y2) ) c=1.0;
 507         else c=SQRT2;
 508
 509         return c;
 510
 511 }
 512
 513
 514 /**
 515  *
 516  * @brief Calculates Map Heuristic values.
 517  * @param xgoal Coordonate X of the goal
 518  * @param ygoal Coordonate Y of the goal
 519  *
 520  * This function calculates the map heuristic values (the distance to
 521  * the goal that is supoused to be the shortest).
 522  *
 523  * The chosen method has been the euclidean distance.  The distance
 524  * between a point of the grid \f$(x,y)\f$ and the
 525  * goal\f$(x_(goal),y_(goal))\f$ is
 526  * \f$\sqrt{(x_(goal)-x)^2+(y_(goal)-y)^2}\f$.
 527  *
 528  * Another method explained in the book is coded but not used.It is
 529  * called Brushfire algorithm. (section 4.3.2 of the book)
 530  *
 531  */
 532 void calculateMapHeuristic(int xgoal, int ygoal)
 533 {
 534         int i, j;
 535         for(i=0;i<MAP_HEIGHT;i++){
 536                 for (j=0;j<MAP_WIDTH;j++){
 537                         graph[i][j].h=sqrt(abs(xgoal-j)*abs(xgoal-j)+abs(ygoal-i)*abs(ygoal-i));
 538                 }
 539         }
 540 #if 0
 541 /*      This is another way to compute heuristic without SQRT but it don't work properly */
 542
 543         Node n;
 544         NodeQueue * toProcess = malloc(sizeof(NodeQueue));
 545         int xcontig, ycontig;
 546         float h; // Heuristic distance of cell
 547         float newh;                             // New Heuristic distance of cell
 548         newQueue(toProcess);
 549         graph[ygoal][xgoal].h=0;
 550         graph[ygoal][xgoal].processed=1;
 551
 552         pushNode(toProcess, xgoal, ygoal);
 553         while(!queueIsEmpty(toProcess)){
 554                 //printPrioritySet(toProcess);
 555                 // Pop the first node in the queue
 556                 popNode(toProcess,&n);
 557                 graph[n.y][n.x].processed=1;
 558                 // Take node's heuristic distance
 559                 h = graph[n.y][n.x].h;
 560
 561                 // Cross contiguous cells
 562                 for (xcontig = n.x-1 ; xcontig <= n.x + 1 ; xcontig++){
 563                         for (ycontig = n.y - 1 ; ycontig <= n.y + 1 ; ycontig++){
 564                                 if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed==0)) {
 565                                         //Add node to priority queue
 566                                         if ((!isInQueue(toProcess,xcontig,ycontig))) pushNode(toProcess,xcontig,ycontig);
 567                                         // Update heuristic if  [ (Heuristic not set) OR [oldHeuristic is higher than newer)]
 568                                         newh= h + calculateCost(n.x, n.y, xcontig, ycontig);
 569                                         if( ( (graph[ycontig][xcontig].h < 0) || (graph[ycontig][xcontig].h > newh )) )
 570                                                 graph[ycontig][xcontig].h=newh ;
 571                                 }
 572
 573                         }
 574                 }
 575         }
 576
 577         // Clear proccessed flag
 578         for(i=0;i<MAP_HEIGHT;i++){
 579                 for (j=0;j<MAP_WIDTH;j++){
 580                         graph[i][j].processed=0;
 581                 }
 582         }
 583 #endif
 584
 585 }
 586
 587 /**
 588  * @brief Calculates the cost of moving from the first cell to the second cell.
 589  * @param x1    Coordonate X of the first cell
 590  * @param y1    Coordonate Y of the first cell
 591  * @param x2    Coordonate X of the second cell
 592  * @param y2    Coordonate Y of the second cell
 593  * @return      Cross cost
 594  * @warning Cells must be contiguous
 595 */
 596 float calculateCost(int x1, int y1, int x2, int y2){
 597
 598         if( (x1==x2) || (y1 == y2) ) return 1.0;
 599         else return SQRT2;
 600
 601 }
 602
 603 /** @} */
 604
 605 /**
 606  * @name A* Implementation Functions
 607  * This functions have not a meaning in A* algorithm, but are needed for implementation.
 608  * @{
 609  */
 610
 611 /**
 612  * @brief Init Graph structure with default values
 613  * @see graph
 614 */
 615 void initGraphStructure(void){
 616         int i,j;
 617
 618         GraphMapCell g = {  .h = -1, .g = -1, .f = INFINITY, .backpointer = NULL,
 619                             .processed = false, .in_queue = false, .next = NULL };
 620
 621         //Initialisation of graph structure
 622         for(i=0;i<MAP_HEIGHT;i++){
 623                 for (j=0;j<MAP_WIDTH;j++){
 624                         graph[i][j] = g;
 625                 }
 626         }
 627 }
 628
 629 /**
 630  * @brief Push the cell c in the queue ordered by heuristic value of the cell
 631  *
 632  * @param c     The cell
 633  *
 634  * @todo Use some tree based data structure for this. -- Sojka
 635  */
 636
 637 void pushNodeInOrder(GraphMapCell *c)
 638 {
 639         c->in_queue = true;
 640
 641 #ifndef CONFIG_PP_USES_RBTREE
 642         GraphMapCell *current, *last;
 643         if (!graph_queue) {
 644                 graph_queue = c;
 645         } else {
 646                 // Put the node in right position
 647                 current=graph_queue;
 648                 last=NULL;
 649                 while (current!=NULL){
 650                         if (c->f < current->f ) {
 651                                 if (last==NULL) graph_queue = c;
 652                                 else last->next = c;
 653                                 c->next=current;
 654                                 return;
 655                         } else {
 656                                 last=current;
 657                                 current=current->next;
 658                         }
 659                 }
 660                 // the node goes in the last position
 661                last->next=c;
 662                c->next = NULL;
 663         }
 664 #else
 665         struct rb_node **new = &(graph_queue.rb_node), *parent = NULL;
 666         /* Figure out where to put new node */
 667         while (*new) {
 668                 GraphMapCell *this = container_of(*new, GraphMapCell, node);
 669
 670                 parent = *new;
 671                 if (c->f < this->f)
 672                         new = &((*new)->rb_left);
 673                 else
 674                         new = &((*new)->rb_right);
 675         }
 676
 677         /* Add new node and rebalance tree. */
 678         rb_link_node(&c->node, parent, new);
 679         rb_insert_color(&c->node, &graph_queue);
 680 #endif
 681 }
 682 /**@}*/
 683
 684 /** @} */
 685