簡單的bfs(最短路徑)c++

#include<iostream>
#include<queue>
#include<vector>
using namespace std;
const int height = 5;
const int width = 5;
class Node
{
public:
    int x;
    int y;
    Node *prenode;
    Node(int x1, int y1, Node *prenode1=NULL) :x(x1), y(y1), prenode(prenode1){};
};
int matrix[height][width] = {
    0, 0, 0, 0, 1,
    0, 1, 0, 1, 0,
    0, 1, 0, 0, 0,
    0, 1, 0, 1, 0,
    0, 0, 0, 1, 0,
};
vector<Node*> vnode;     //存放節點是否訪問過的
queue<Node*> qnode;      
//訪問
void visit(int x, int y, Node* p)
{
    Node* pr = new Node(x,y,p);
    matrix[x][y] = 2;               
    qnode.push(pr);
}

void bfs()
{
    int dist[4][2] = { {0,-1}, {0,1}, {-1,0}, {1,0} };   //上下左右
    Node* p = NULL;
    Node* head = new Node(0,0,p);
    qnode.push(head);
    while (!qnode.empty())
    {
        Node *p = qnode.front();
        vnode.push_back(p);
        qnode.pop();
        if (p->x == height - 1 && p->y == width - 1)
            break;
        for (int i = 0; i < 4; i++)
        {
            
            if (p->x+dist[i][0]>=0 &&p->x+dist[i][0]<height&&
                p->y+dist[i][1] >= 0&&p->y+dist[i][1]<width && matrix[p->x+dist[i][0]][p->y+dist[i][1]] == 0)
                visit(p->x+dist[i][0], p->y+dist[i][1], p);
        }
        
    }

}
void printPath()  //打印路徑
 {
        
         Node *p = vnode[vnode.size() - 1];

        while (p != NULL)
             {
                 cout << "(" << p->x << "," << p->y << ")";
                 if (p->prenode != NULL)
                     cout << "<-";
                 p = p->prenode;
                 
            }
         cout<<endl;
         }
void destroy()  //銷燬節點
{
    for (int i = 0; i < vnode.size(); i++)
    {
        delete vnode[i];
    }
}
int main()
{
    bfs();
    printPath();
    destroy();
    system("pause");
    return 0;
}