迷宫问题 BFS回溯保存路径

#include<iostream>
#include<queue>
#include<stdlib.h>
#include<string.h>
#include<stdlib.h>
using namespace std;
struct node
{
 int i;
 int j;
};
typedef struct node pp;
pp path[50];
struct node p1,p2;
int xx[4]={1,0,0,-1};
int yy[4]={0,1,-1,0};
int mark[10][10];
int judge(int x,int y)
{
 if(x<0||x>4||y<0||y>4)
 return 0;
 return 1;
}
int main( )
{
  int map[6][6],i,j,k,x1,y1,a,b,len=0;
  memset(mark,0,sizeof(mark));
  for(i=0;i<5;i++)
   for(j=0;j<5;j++)
     scanf("%d",&map[i][j]);
     queue<node>q;
     p1.i=0;
     p1.j=0;
     q.push(p1);
     mark[0][0]=1;
     while(!q.empty())
     {
       p1=q.front();
       q.pop();
       for(i=0;i<4;i++)
             {
              x1=p1.i+xx[i];
              y1=p1.j+yy[i];
              if(judge(x1,y1)&&map[x1][y1]!=1&&mark[x1][y1]==0)
              { 
               mark[x1][y1]=mark[p1.i][p1.j]+1;
               //printf("%d\n",len);
               if(x1==4&&y1==4)
               { 
                  len=mark[x1][y1];
                  break;
               }
                p2.i=x1;
                p2.j=y1;
                q.push(p2);
              }
            }
        }
        p1.i=4,p1.j=4;
        //printf("%d\n",len);
       for(i=len-1;i>=0;i--)
       {
        path[i].i=p1.i;
        path[i].j=p1.j;
        for(j=0;j<4;j++)
          {
            x1=p1.i+xx[j];
            y1=p1.j+yy[j];
           // printf("x1:%d\n y1:%d\n",x1,y1);
            if((mark[x1][y1]==mark[p1.i][p1.j]-1)&&judge(x1,y1))
            {
              p1.i=x1;
              p1.j=y1;
              break;
            }
          
          }
        } 
           for(i=0;i<len;i++)
       printf("(%d, %d)\n",path[i].i,path[i].j);    
        system("pause");
        return 0;
   }