Find a path between 2 points in a maze with minimum turns - c++

The problem:
Given a 2D matrix consist of 0 and 1, you can only go in location with 1. Start at point (x, y), we can move to 4 adjacent points: up, down, left, right; which are: (x+1, y), (x-1, y), (x, y+1), (x, y-1).
Find a path from point (x, y) to point (s, t) so that it has the least number of turns.
My question:
I tried to solve this problem using dijsktra, it got most of the cases right, but in some cases, it didn't give the most optimal answer.
Here's my code:
pair<int,int> go[4] = {{-1,0}, {0,1}, {1,0}, {0,-1}};
bool minimize(int &x, const int &y){
if(x > y){
x = y;
return true;
}return false;
}
struct Node{
pair<int,int> point;
int turn, direc;
Node(pii _point, int _turn, int _direc){
point = _point;
turn = _turn;
direc = _direc;
}
bool operator < (const Node &x) const{
return turn > x.turn;
}
};
void dijkstra(){
memset(turns, 0x3f, sizeof turns);
turns[xHome][yHome] = -1;
priority_queue<Node> pq;
pq.push(Node({xHome, yHome}, -1, -1));
while(!pq.empty()){
while(!pq.empty() &&
pq.top().turn > turns[pq.top().point.first][pq.top().point.second])pq.pop();
if(pq.empty())break;
pii point = pq.top().point;
int direc = pq.top().direc;
pq.pop();
for(int i = 0; i < 4; i++){
int x = point.first + go[i].first ;
int y = point.second + go[i].second;
if(!x || x > row || !y || y > col)continue;
if(matrix[x][y])
if(minimize(turns[x][y], turns[point.first ][point.second] + (i != direc)))
pq.push(Node({x, y}, turns[x][y], i));
}
}
}
P/S: The main solving is in void dijkstra, the others are just to give some more information in case you guys need it.

I have found a way to solve this problem, storing directions and using BFS() to reduce the time complexity:
struct Node{
short row, col;
char dir;
Node(int _row = 0, int _col = 0, int _dir = 0){
row = _row; col = _col; dir = _dir;
}
};
void BFS(){
memset(turns, 0x3f, sizeof turns);
deque<pair<int, Node> > dq;
for(int i = 0; i < 4; i++){
Node s(xHome + dx[i], yHome + dy[i], i);
if(!matrix[s.row][s.col])continue;
turns[s.row][s.col][s.dir] = 0;
dq.push_back({0, s});
}
while(!dq.empty()){
int d = dq.front().fi;
Node u = dq.front().se;
dq.pop_front();
if(d != turns[u.row][u.col][u.dir])continue;
for(int i = 0; i < 4; i++){
Node v(u.row + dx[i], u.col + dy[i], i);
if(!matrix[v.row][v.col])continue;
if(minimize(turns[v.row][v.col][v.dir], turns[u.row][u.col][u.dir] + (i != u.dir))){
if(i == u.dir)dq.push_front({turns[v.row][v.col][v.dir], v});
else dq.push_back({turns[v.row][v.col][v.dir], v});
trace[v.row][v.col][v.dir] = u;
}
}
}
}

An obvious error in you algorithm is that to detect the length of the path start->x->y, you should store all directions to x that can form a shortest path from start to x.
For example, suppose start=(0,0),x=(1,1),y=(1,2) and there are two paths from start to x: start->(0,1)->x, start->(1,0)->x, both have the shortest length. However, start->(0,1)->x->y has two turns while start->(1,0)->x->y has only one turn. So you need to store all the directions for each node (in this case, you should store both the directions (0,1)->x and (1,0)->x in x.

Related

Finding heaviest path (biggest sum of weights) of an undirected weighted graph? Bellman Ford --

There's a matrix, each of its cell contains an integer value (both positive and negative). You're given an initial position in the matrix, now you have to find a path that the sum of all the cells you've crossed is the biggest. You can go up, down, right, left and only cross a cell once.
My solution is using Bellman Ford algorithm: Let's replace all the values by their opposite number, now we've just got a new matrix. Then, I create an undirected graph from the new matrix, each cell is a node, stepping on a cell costs that cell's value - it's the weight. So, I just need to find the shortest path of the graph using Bellman-Ford algorithm. That path will be the longest path of our initial matrix.
Well, there's a problem. The graph contains negative cycles, also has too many nodes and edges. The result, therefore, isn't correct.
This is my code:
Knowing that xd and yd is the initial coordinate of the robot.
void MatrixToEdgelist()
{
int k = 0;
for (int i=1;i<=n;i++)
for (int j=1;j<=n;j++)
{
int x = (i - 1) * n + j;
int y = x + 1;
int z = x + n;
if (j<n)
{
edges.push_back(make_tuple(x, y, a[i][j+1]));
}
if (i<n)
{
edges.push_back(make_tuple(x, z, a[i+1][j]));
}
}
}
void BellmanFord(Robot r){
int x = r.getXd();
int y = r.getYd();
int z = (x-1)*n + y;
int l = n*n;
int distance[100];
int previous[100]{};
int trace[100];
trace[1] = z;
for (int i = 1; i <= l; i++) {
distance[i] = INF;
}
distance[z] = a[x][y];
for (int i = 1; i <= l-1; i++) {
for (auto e : edges) {
int a, b, w;
tie(a, b, w) = e;
//distance[b] = min(distance[b], distance[a]+w);
if (distance[b] < distance[a] + w)// && previous[a] != b)
{
distance[b] = distance[a] + w;
previous[b] = a;
}
}
}
//print result
int Max=INF;
int node;
for (int i=2;i<=l;i++)
{
if (Max < distance[i])
{
Max = distance[i];
node = i;
}
}
if (Max<0)cout << Max << "\n";
else cout << Max << "\n";
vector<int> ans;
int i = node;
ans.push_back(i);
while (i != z)
{
i = previous[i];
ans.push_back(i);
}
for (int i=ans.size()-1;i>=0;i--)
{
int x, y;
if (ans[i] % n == 0)
{
x = ans[i] / n;
y = n;
}
else{
x = ans[i] / n + 1;
y = ans[i] - (( x - 1 ) * n);
}
cout << x << " " << y << "\n";
}
}
Example matrix
The result
Clearly that the distance should have continued to update, but it doesn't. It stops at the final node.
"Let's replace all the values by their opposite number"
Not sure what you mean by an opposite number. Anyway, that is incorrect.
If you have negative weights, then the usual solution is to add the absolute value of the most negative weight to EVERY weight.
Why Bellman-Ford? Dijkstra should be sufficient for this problem. ( By default Dijkstra finds the cheapest path. You find the most expensive by assigning the absolute value of ( the original weight minus the greatest ) to every link. )

From recursive algorithm to an iterative one

I have to convert this recursive algorithm into an iterative one:
int alg(int A[], int x, int y, int k){
int val = 0;
if (x <= y){
int z = (x+y)/2;
if(A[z] == k){
val = 1;
}
int a = alg(A,x,z-1,k);
int b;
if(a > val){
b = alg(A,z+1,y,k);
}else{
b = a + val;
}
val += a + b;
}
return val;
}
I tried with a while loop, but I can't figure out how I can calculate "a" and "b" variables, so I did this:
int algIterative(int A[], int x, int y, int k){
int val = 0;
while(x <= y){
int z = (x+y)/2;
if(A[z] == k){
val = 1;
}
y = z-1;
}
}
But actually I couldn't figure out what this algorithm does.
My questions are:
What does this algorithm do?
How can I convert it to iterative?
Do I need to use stacks?
Any help will be appreciated.
I am not sure that alg computes anything useful.
It processes the part of the array A between the indexes x and y, and computes a kind of counter.
If the interval is empty, the returned value (val) is 0. Otherwise, if the middle element of this subarray equals k, val is set to 1. Then the values for the left and right subarrays are added and the total is returned. So in a way, it counts the number of k's in the array.
But, if the count on the left side is found to be not larger than val, i.e. 0 if val = 0 or 0 or 1 if val = 1, the value on the right is evaluated as the value on the left + val.
Derecursivation might be possible without a stack. If you look at the sequence of subintervals that are traversed, you can reconstruct it from the binary representation of N. Then the result of the function is the accumulation of partials results collected along a postorder process.
If the postorder can be turned to inorder, this will reduce to a single linear pass over A. This is a little technical.
A simple way could be smt like this with the aid of a two dimensional array:
int n = A.length;
int[][]dp = new int[n][n];
for(int i = n - 1;i >= 0; i--){
for(int j = i; j < n; j++){
// This part is almost similar to the recursive part.
int z = (i+j)/2;
int val = 0;
if(A[z] == k){
val = 1;
}
int a = z > i ? dp[i][z - 1] : 0;
int b;
if(a > val){
b = (z + 1 <= j) ? dp[z + 1][j] : 0;
}else{
b = a + val;
}
val += a + b;
dp[i][j] = val;
}
}
return dp[0][n - 1];
Explanation:
Notice that for i, it is decreasing, and j, it is increasing, so, when calculate dp[x][y], you need dp[x][z - 1] (with z - 1 < j) and dp[z + 1][j] (with z >= i), and those values should already be populated.

to detect path in bfs

i am doing a standard problem to calculate min moves to reach target by a knight but i also want to keep track of path but its showing error.it dispalys
prog.cpp: In function
'int minStepToReachTarget(int*, int*, int)':
prog.cpp:76:42: error: no match for 'operator[]' (operand types are
'std::vector<cell>' and 'cell')
{q.push(cell(x, y, t.dis + 1));parent[cell(x, y, t.dis + 1)]=t;}
I have commented down the line 76 in my code.
struct cell {
int x, y;
int dis;
cell() {}
cell(int x, int y, int dis): x(x), y(y), dis(dis) {}
};
//cell parent[10000];
typedef cell c;
vector<c> parent(10000);
bool isInside(int x, int y, int N) {
if (x >= 1 && x <= N && y >= 1 && y <= N)
return true;
return false;
}
int minStepToReachTarget(int knightPos[], int targetPos[], int N) {
// x and y direction, where a knight can move
int dx[] = {-2, -1, 1, 2, -2, -1, 1, 2};
int dy[] = {-1, -2, -2, -1, 1, 2, 2, 1};
// queue for storing states of knight in board
queue<cell> q;
// push starting position of knight with 0 distance
q.push(cell(knightPos[0], knightPos[1], 0));
cell t;
int x, y;
bool visit[N + 1][N + 1];
// make all cell unvisited
for (int i = 1; i <= N; i++)
for (int j = 1; j <= N; j++)
visit[i][j] = false;
visit[knightPos[0]][knightPos[1]] = true;
// parent[cell(knightPos[0], knightPos[1], 0)]=t;
// loop untill we have one element in queue
while (!q.empty()) {
t = q.front();
//parent[t]=t;
q.pop();
visit[t.x][t.y] = true;
// if current cell is equal to target cell,
// return its distance
if (t.x == targetPos[0] && t.y == targetPos[1])
return t.dis;
// loop for all reahable states
for (int i = 0; i < 8; i++) {
x = t.x + dx[i];
y = t.y + dy[i];
// If rechable state is not yet visited and
// inside board, push that state into queue
if (isInside(x, y, N) && !visit[x][y]) {
q.push(cell(x, y, t.dis + 1));
//76 ERROR: parent[cell(x, y, t.dis + 1)]=t;}
}
}
}
int main() {
// size of square board
int N = 6;
int knightPos[] = {4, 5};
int targetPos[] = {1, 1};
int m= minStepToReachTarget(knightPos, targetPos, N);
cout<<m<<endl;
return 0;
}
#Code
int dx[8] = {-1,-2, 1 ,2 ,-1, -2, 1, 2};
int dy[8] = {-2,-1, -2,-1, 2, 1, 2, 1};
q.push(ppi(pi(kx, ky), 0));
while(!q.empty()){
pi cur = q.front().first; int d = q.front().second; q.pop();
int x = cur.first;
int y = cur.second;
// printf("%d %d %d\n", x,y, visited[x][y]);
if(visited[x][y]) continue;
visited[x][y] = true;
if (x == tx && y == ty){
printf("%d", d);
return 0;
}
for(int i = 0; i<8; i++){
int nx = x+dx[i];
int ny = y+dy[i];
if(nx <= 0 || nx > n || ny <= 0 || ny > n) continue;
q.push(ppi(pi(nx, ny), d+1));
}
}
printf("-1");
Explanation
Here, this is the bfs which I implemented. I am storing the grid as a boolean value, I can keep track of which squares I have traveled too. If I am unable to reach the square, I output -1. I would also recommend not using function calls unless really necessary, as this is a simple BFS question.
Extension
Just in case you want to find out more, I took the following chunk from my code for a harder problem, where there exists a grid with forbidden squares where the knight cannot travel. In that case, forbidden squares are initially marked as 'true' instead of 'false' to signify that is traveled so I will not go on it.
I hope my above solution helps.

Really slow when getting maximum distance between m nodes (Backtracking) (c++)

I've been asked to make an exercise using Backtracking, or Backtracking + Branch and Bound, where the imput data are n, m and a matrix(n x n). The n, represents a number of people, and the m, some people from the n. In the matrix, there are the distances among them, and the distances between i and j is different from the j and the i.
I am trying to get the maximum distance i can get from m nodes, that distance is the sum of the distance of all of them. For example, if i choose the node 1, 2 and 4, the result is the sums: distance(1, 2) + distance(2,1) + distance(2, 4) + distance(4, 2) + distance(1, 4) + distance(4, 1).
I have used Backtracking with Branch and Bound (iterative, not recursive), storing the nodes (structs where i store the current value and nodes used) that may get me to a solution. This nodes stores de lower and upper bound, i mean, the lower and higher solution I can obtain if i keep on using this node and his sons. From a node x, I generate all the possible nodes (nodes that are not used), and I check if this node may get me to a solution, if not, this node is discarded and erased.
The code i have implemented to make this, works, but it is really slowly. With low values of n and m, it is quick, but if i use higher numbers it is really slowly.
This is the main function and the others functions that are used:
void backtracking(int **matrix, int n, int m){
/////////////////////////////////////////////////////
/*
Part where I try to get the minimum/maximum that I can get from the beginning of the problem
*/
// Lists where I store the values from the matrix, sort from the minimum to the maximum, and from
// the maximum to the minimum. The values are the distances, I mean, the sum of matrix[i][j] and
// matrix[j][i].
list<int> listMinSums; // list of minimum sums
list<int> listMaxSums; // list of maximum sums
int nMinimumSums = floor((m*m - m)/2); // rounding down
int nMaximumSums = ceil((m*m - m)/2); // rounding up
/*
* m*m - m = Given m nodes, there are m*m - m sums.
*
* I count matrix[i][j] + matrix[j][i] as one, so there
* are (m*m - m)/2 sums.
*/
for (int i = 0; i < n; i++){
for (int j = 0; j < i; j++){
int x = (matrix[i][j] + matrix[j][i]);
// to differentiate from the minimum and maximum sums, I use false and true
aLista(listMinSums, x, nMinimumSums, false);
aLista(listMaxSums, x, nMaximumSums, true);
}
}
int min = 0;
int max = 0;
int contador = 0; // counting in every iteration to not surpassing the minimum/maximum sums
list<int>::iterator it = listMinSums.begin();
while (it != listMinSums.end() && contador < nMinimumSums){
min += *it;
it++;
contador++;
}
contador = 0;
list<int>::iterator it2 = listMaxSums.begin();
while (it2 != listMaxSums.end() && contador < nMaximumSums){
max += *it2;
it2++;
contador++;
}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////
// LLV = List of Live Nodes. Where I store the nodes that are going to
// guide me to the solution
list<nodo*> llv;
// I do not store the root node, i store the first n nodes, of the first level.
for (int i = 0; i < n; i++){
nodo *nod = new nodo(n);
nod ->level = 0;
//lower bound. It's the lower solution i can get from this node
nod ->ci = min;
// upper bound. The higher solution i can get from this node.
nod ->cs = max;
// estimated benefit. If i have to choose from one node or another, i choose the one with higher
nod ->be = (min+max)/2;
// The node i is used
nod ->used[i] = true;
// Inserting this node to the list of live nodes.
insert(llv, nod);
}
int solution = 0; // Initial solution
int c = min; // c = variable used to not use a node and his "sons", anymore
while (!empty(llv)){
nodo *x = erase(llv, n); // erasing the node with the highest estimated benefit from the llv.
if (x ->cs > c){
for (int i = 0; i < n; i++){ // Creating every son of the x node...
if (!(x ->used[i])){ // ... that has not being used yet
nodo *y = new nodo(n);
y ->level = x ->level + 1;
for (int j = 0; j < n; j++){
y ->used[j] = x ->used[j];
}
y ->used[i] = true;
// Adding the values. For example, if node 1 and 2 were inserted, and this is the node 4,
// adding matrix[1][4]+matrix[4][1]+matrix[2][4] + matrix[4][2]
int acum = 0;
for (int k = 0; k < n; k++){
if (k != i && consult(x ->used, k))
acum += matrix[i][k] + matrix[k][i];
}
y ->bact = x ->bact + acum;
// Getting the lower and upper bound of this node y.
cotas(y, x, i, y ->level, n, matrix);
y ->be = (y ->ci + y ->cs)/2;
// Node where i can get the solution
if (y ->level == (m-1) && y ->bact > solution){
solution = y ->bact;
if (y ->bact > c)
c = y ->bact;
}
// Checking if i can update c
else if (y ->level != (m-1) && y ->cs > c){
insert(llv, y);
if (y ->ci > c)
c = y ->ci;
}
else{
// i cannot use this node anymore, so i delete it.
delete y;
}
}
}
}
}
cout << solution << endl;
liberacionMemoria(matrix, n); // freeing the memory used in the matrix
}
void liberacionMemoria(int **matriz, int n){
for (int i = 0; i < n; i++)
delete[] matriz[i];
delete[] matriz;
}
void insert(list<nodo*> &t, nodo *x){
list<nodo*>::iterator it= t.begin();
t.insert(it, x);
}
/*
* Getting the node with hightest estimated benefit from the list of live nodes
* */
nodo* erase (list<nodo*> &t, int n){
nodo* erased = new nodo(n);
erased ->level = -1;
erased ->be = -1;
list<nodo*>::iterator it= t.begin();
list<nodo*>::iterator it2;
while (it != t.end()){
nodo* aux = *it;
if (aux ->be > erased ->be){
it2 = it;
erased = aux;
}
else if (aux ->be == erased ->be && aux ->level > erased ->level){
it2 = it;
erased = aux;
}
it++;
}
t.erase(it2);
return erased;
}
/*
* Checking if in the array of nodes used, the node in the x position it's used
* */
bool consult(bool *nodesUsed, int x){
if (nodesUsed[x])
return true;
return false;
}
bool empty(list<nodo*> &t){
list<nodo*>::iterator it= t.begin();
return (it==t.end());
}
bool aLista(list<int> &t, int x, int m, bool MayorAMenor){
list<int>::iterator it = t.begin();
int contador = 0;
while (it != t.end() && contador < m){
if (!MayorAMenor){ // lower to upper
if (*it > x){
t.insert(it, x);
return true;
}
}
else{
if (*it < x){
t.insert(it, x);
return true;
}
}
contador++;
it++;
}
if (it == t.end() && contador < m){
t.insert(it, x);
return true;
}
return false;
}
void cotas(nodo *sonNode, nodo *fatherNode, int elegido, int m, int n, int **matriz){
int max = 0;
int min = 999;
// Getting the sums from the chosen node with the already used
for (int i = 0; i < n; i++){
if (consult(sonNode ->used, i)){
if (elegido != i){
int x = matriz[i][elegido] + matriz[elegido][i];
if (x > max)
max = x;
if (x < min)
min = x;
}
}
}
min *= (m-1);
max *= (m-1);
min += fatherNode -> bact;
max += fatherNode -> bact;
sonNode -> ci = fatherNode ->ci - min;
sonNode -> cs = fatherNode ->cs - max;
}
I think, that the reason of going really slow with n and m a bit high, it is really slowly, it is because of the upper and lower bounds of the nodes, that are not accurate, but i don't know how to make it better.
I've been many days thinking how to do it, and trying to, but nothing works.
Here there are some examples:
Given an n = 4 and m = 2 and the following matrix:
0 3 2 4
2 0 4 5
2 1 0 4
2 3 2 0
the result is 8. This works and it is quickly.
But with n = 40 and m = 10 it never ends...
I hope someone may help me. Thanks.
****EDIT******
I may not have explained well. My doubt is, how can i know, from a node x, the less and the maximum I can get.
Because, the length of the solution nodes depends on m, but the solution changes if i choose some nodes or others, and I don't know how to be sure, of obtaining the less and the maximum from a node, but being accurate, to be able to cut the others branchs that do not guide me to a solution

I'm trying to change my Maze traversal recursive coding part into a while loop

Here's my code.
#include <iostream>
using namespace std;
enum Direction { EAST, NORTH, WEST, SOUTH };
const int size = 12;
int xStart = 2; int yStart = 0;
char *maze2[ ] = {
"############",
"#...#......#",
"..#.#.####.#",
"###.#....#.#",
"#....###.#..",
"####.#.#.#.#",
"#..#.#.#.#.#",
"##.#.#.#.#.#",
"#........#.#",
"######.###.#",
"#......#...#",
"############",
};
void printMaze ( char maze[][ size ] );
void mazeTraverse( char maze[][ size ], int x, int y, int direction );
int main()
{
char maze[ size ][ size ];
for (int x = 0; x < size; x++ )
for (int y = 0; y < size; y++)
maze[ x ][ y ] = maze2[ x ][ y ];
printMaze( maze );
mazeTraverse( maze, xStart, yStart, EAST);
}
void printMaze ( char maze[][ size ] )
{
for ( int x = 0; x < size; x++)
{
for ( int y = 0; y < size; y++)
cout << maze[ x ][ y ];
cout << endl;
}
cout << endl;
cout << "\nHit return to see next move\n";
cin.get();
}
bool validMove( char maze[][ size ], int x, int y )
{
return x >= 0 && x < size && y >= 0 && y < size && maze[x][y] != '#';
}
bool coordsAreEdge( int x, int y )
{
return x== 0 || x== size - 1 || y == 0 || y== size - 1;
}
void mazeTraverse( char maze[][ size ], int x, int y, int direction )
{
maze[ x ][ y ] = 'x';
printMaze( maze );
if (coordsAreEdge(x, y) && (x != xStart || y!= yStart ))
{
cout <<"\nMaze successfully exited!\n\n";
return;
}else{
for ( int move = direction, count = 0; count < 4;
count++, move++, move %=4 )
{
int nextX; int nextY;
switch ( move )
{
case SOUTH: nextX = x + 1; nextY = y; break;
case EAST: nextX = x; nextY = y + 1; break;
case NORTH: nextX = x - 1; nextY = y; break;
case WEST: nextX = x; nextY = y - 1; break;
default: ;
}
if (validMove( maze, nextX, nextY ))
{
//Recursion move part 1
//mazeTraverse ( maze, nextX , nextY, (move + 3)%4 );
return;
}
}
}
}
I'm trying to make my void mazeTraverse function a while loop, instead of the recursion and I'm stuck.
Create a struct to hold X, Y and direction (the three things that change between calls). We'll call that struct State;
Create a std::stack<State> object. Push the current values of X,Y, direction onto the stack before you change them, pop them after you do your work.
hence
while(.....)
{
push state
Do work of mazeTraverse
pop state
}
It would've been nice if you described how the traversal works. If I'm not reading the code wrong, you are basically moving south/east/north/west on any position that doesn't contain a # and is within the bounds of the matrix.
You can do this iteratively by using a BF search: http://en.wikipedia.org/wiki/Breadth-first_search or, applied to a matrix, the Lee algorithm: http://en.wikipedia.org/wiki/Lee_algorithm which can be efficiently implemented using a FIFO queue, which I describe how to do here: Change FloodFill-Algorithm to get Voronoi Territory for two data points?
Your validMove function will stay the same: you add a position to the queue only if that position is valid. Basically all checks stay the same, just that you use a FIFO queue to hold your states instead of an implicit stack.
You could use a breadth-first search instead using a standard queue and while loop.
typedef pair<int, int> Point;
queue<Point> path;
Point start(xStart, yStart);
path.push(start);
const int move_x[] = {-1, 0, 1, 0};
const int move_y[] = {0, -1, 0, 1};
while (!path.empty())
{
Point p = path.front();
int x = p.first, y = p.second;
maze[x][y] = 'x';
path.pop();
if (coordsAreEdge(x,y) && p != start)
{
// Finished
break;
}
for (int i = 0; i < 4; ++i)
{
int newx = x + move_x[i];
int newy = y + move_y[i];
if (validMove(maze, newx, newy))
path.push(Point(newx, newy));
}
}
That should do the trick. Note that it's untested though.
You can improve its performance by using A* instead, but that's a little more complex. Let me know if you need to find the shortest path from this code as well.
EDIT: Note that if you change the queue to a stack (and change path.front() to path.top()) then you'll get a depth-first search (DFS) instead, which is what your code does. The DFS, however, doesn't find the shortest path (if that is necessary).