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