c++ - Breadth First Search in C gets Loop -

i'm trying implement breadth first search algorithm in c, search path first (first column) white (255) point in binary matrix until last 1 (last column).

i've created algorithm binarize image correctly based on histogram, when try find shortest path, i'm getting problems. maybe didn't understand how do.

i've grid, matrix containing binarizied values (0 or 255), 255 means can "walk", , 0 means it's "wall". , i've map, know can go, , went to. both have same sizes.

i paste code of bfs algorithm, , insert in gist code if wanna view, or further references someone.

obs: coordenada means coordinate.

coordenada bfs( unsigned char ** grid, coordenada local, queue queue, int8_t **map, unsigned long height, unsigned long width) {   /* insere coordenada atual na queue */   queue.push(&queue, local);    /* while queue not empty */   while ( queue.size != 0 )   {     /* retrieve point */     coordenada p = queue.pop(&queue);     printf("processando (%d, %d)\n", p.x, p.y);      /* check if it's last point */     if ( p.x == 33 - 1 && p.y == 16 - 1 )     {       printf("%s\n", "chegamos onde queríamos");       return p;     }      /* try move on */     if ( isfree(grid, map, p.y + 1, p.x, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y + 1;       next_point.x = p.x;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y - 1, p.x, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y - 1;       next_point.x = p.x;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y + 1, p.x + 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y + 1;       next_point.x = p.x + 1;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y - 1, p.x + 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y - 1;       next_point.x = p.x + 1;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y + 1, p.x - 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y + 1;       next_point.x = p.x - 1;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y - 1, p.x - 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y - 1;       next_point.x = p.x - 1;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y, p.x - 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y;       next_point.x = p.x - 1;       queue.push(&queue, next_point);     }      if ( isfree(grid, map, p.y, p.x + 1, height, width) )     {       map[p.y][p.x] = -1;       coordenada next_point;       next_point.y = p.y;       next_point.x = p.x + 1;       queue.push(&queue, next_point);     }    }    coordenada empty_coord;    /* otherwise */   return empty_coord; }  bool isfree( unsigned char ** grid, int8_t ** map, int y, int x, unsigned long height, unsigned long width ) {   if((y >= 0 && y < height) && (x >= 0 && y < width) && (map[y][x] == 0) && (grid[y][x] == 255))     return true;   return false; } 

and header:

/**  * point in matrix  *  * use data type represent point in our  * euclidean space matrix.  */ typedef struct {     int x;     int y; } coordenada;  /**  * node struct,  * contains item , pointer point next node.  *  * ref: http://ben-bai.blogspot.com.br/2012/04/simple-queue-data-structure-in-ansi-c.html  */ typedef struct node {     coordenada item;     struct node* next; } node;  /**  * queue struct, contains pointers  * point first node , last node, size of queue,  * , function pointers.  */ typedef struct queue {   node* head;   node* tail;    void (*push) (struct queue*, coordenada); // add item tail   // item head , remove queue   coordenada (*pop) (struct queue*);   // item head keep in queue   coordenada (*peek) (struct queue*);   // display element in queue   void (*display) (struct queue*);   // size of queue   int size; } queue; 

link in gist: gist containing sources

thanks reading (:

fore more reference, github repository.