src/pathplan/pathqueue.c

   1 /**
   2  * @file        pathqueue.c
   3  * @brief       Functions used to manage a queue used by A* Algorithm.
   4  * @author      Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>
   5  *
   6  *
   7 */
   8
   9 /** @addtogroup ppi*/
  10 /**@{*/
  11 #include "pathqueue.h"
  12
  13 /**
  14  * @name Queue functions
  15  * This functions implements a queue needed by other functions.
  16  * @{
  17  */
  18 /**
  19  * @brief Tests if a queue is empty
  20  * @param q A queue
  21  * @return 1 if the queue is empty, 0 otherwise
  22  */
  23 int queueIsEmpty(NodeQueue *q){
  24         if ((q->first==NULL) && (q->last==NULL))        return 1;
  25         else return 0;
  26 }
  27
  28 /**
  29  * @brief Push a node into the queue in a FIFO mode
  30  * @param q     Queue
  31  * @param x     Coordonate X of a cell
  32  * @param y     Coordonate Y of a cell
  33 */
  34 void pushNode(NodeQueue * q, int x, int y){
  35         Node *n= malloc(sizeof(Node));
  36         if (n == NULL) {
  37                 /* Memory could not be allocated, so print an error and exit. */
  38                 fprintf(stderr, "Couldn't allocate memory\n");
  39                 exit(EXIT_FAILURE);
  40         }
  41
  42         n->x=x;
  43         n->y=y;
  44         n->next=NULL;
  45
  46         if (queueIsEmpty(q)) {
  47                 q->first=n;
  48                 q->last=n;
  49         } else {
  50                 (q->last)->next=n;
  51                 q->last=n;
  52         }
  53 }
  54
  55 /**
  56  * @brief Push a node into the queue in a LIFO mode
  57  * @param q     Queue
  58  * @param x     Coordonate X of a cell
  59  * @param y     Coordonate Y of a cell
  60 */
  61 void pushNodeLast(NodeQueue * q, int x, int y){
  62         Node *n= malloc(sizeof(Node));
  63         if (n == NULL) {
  64                 /* Memory could not be allocated, so print an error and exit. */
  65                 fprintf(stderr, "Couldn't allocate memory\n");
  66                 exit(EXIT_FAILURE);
  67         }
  68
  69         n->x=x;
  70         n->y=y;
  71
  72         if (queueIsEmpty(q)) {
  73                 q->first=n;
  74                 q->last=n;
  75                 n->next = NULL;
  76         } else {
  77                 n->next=q->first;
  78                 q->first=n;
  79         }
  80 }
  81
  82 /**
  83  * @brief Pop the first node of the queue
  84  * @param q     Queue
  85  * @param n     Pointer to a node
  86  * @return 1 if node was succesfully poped 0 if the queue is empty
  87 */
  88 int popNode(NodeQueue * q, Node * n){
  89         Node * tmp;
  90         if (!queueIsEmpty(q)){
  91                 n->x = (q->first)->x;
  92                 n->y = (q->first)->y;
  93                 tmp = (q->first)->next;
  94                 free((q->first));
  95                 q->first = tmp;
  96                 if ( (q->first) == NULL) q->last = NULL;
  97                 return 1;
  98         } else return 0;
  99 }
 100
 101 /**
 102  * @brief Print the queue
 103  * @param q     Queue
 104  * @note Used only for debug
 105 */
 106 void printQueue(NodeQueue * q){
 107         Node * n = q->first;
 108
 109         while(n!=NULL){
 110                 printf("(%d,%d)\n", n->x, n->y);
 111                 n=n->next;
 112         }
 113         printf("No more Nodes in the queue!\n");
 114
 115 }
 116
 117 /**
 118  * @brief Init queue values
 119 */
 120  NodeQueue * newQueue(void){
 121         NodeQueue * q;
 122         q = malloc(sizeof(NodeQueue));
 123         q->first=NULL;
 124         q->last=NULL;
 125         return q;
 126 }
 127 /**
 128  * @brief Free queue memory
 129  * @param q     Queue
 130  * @return 1 if memory was succesfully free 0 if the queue is not empty
 131 */
 132 int delQueue(NodeQueue * q){
 133         if (queueIsEmpty(q)) {
 134                 free(q);
 135                 return 1;
 136         } else return 0;
 137 }
 138
 139 /**
 140  * @brief Finds out if the node (X,Y) is in the queue
 141  * @param q     Queue
 142  * @param x     Coordonate X of a cell
 143  * @param y     Coordonate Y of a cell
 144  * @return 1 if (X,Y) was founded, 0 otherwise
 145 */
 146 int isInQueue(NodeQueue * q, int x, int y){
 147                 Node * tmp;
 148
 149                 tmp=q->first;
 150                 while (tmp!=NULL){
 151                         if((tmp->x==x)&&(tmp->y==y)) return 1;
 152                         tmp=tmp->next;
 153                 }
 154                 return 0;
 155 }
 156
 157 /**
 158  * @brief Free memory allocated in the queue
 159  * @param q     Queue
 160 */
 161 void drainQueue(NodeQueue * q){
 162     Node n;
 163         while(popNode(q, &n));
 164 }
 165
 166 /**@}*/
 167 /**
 168  * @}
 169 */
 170