＜图＞A* queue-CSDN博客

本文链接：https://blog.csdn.net/weixin_54498627/article/details/131722813

从起点开始先把每一个顶点关联的边(未被标记)计算出来(顺便更新路径)，
关联顶点(未被标记)入队列，如果这个顶点的所有边计算完毕，该顶点出队列
有向图加权：
           1
        B →→→→ D            
     5↗ ↓    ↗ ↓ ↘6
    A  2↓  /4  ↓  F
     1↘ ↓ ↗    ↓3
        C →→→→ E
            8

输入的图邻接矩阵G：
  A  B  C  D  E  F
A 0  5  1  0  0  0
B 0  0  2  1  0  0
C 0  0  0  4  8  0
D 0  0  0  0  3  6
E 0  0  0  0  0  0
F 0  0  0  0  0  0

初始化顶点点信息表T：
city_name  dist_2_A  last_city  visited
    A         0        -1        0
    B         ∞        -1        0
    C         ∞        -1        0
    D         ∞        -1        0
    E         ∞        -1        0
    F         ∞        -1        0
    G         ∞        -1        0
起点入队列：
    — — — — — — 
    A 
    — — — — — —

起点A出队列，标记A为访问过，表示找到其到达起点的最短距离了
    — — — — — —

    — — — — — —
    city_name  dist_2_A  last_city  visited
        A         0        -1        1
        B         ∞        -1        0
        C         ∞        -1        0
        D         ∞        -1        0
        E         ∞        -1        0
        F         ∞        -1        0
判断A为访问过，不更新：
判断B没有访问过，并且B的 T[A][B]=∞ > T[A][A] + G[A][B] = 0 + 5，更新：
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        0
    C         ∞        -1        0
    D         ∞        -1        0
    E         ∞        -1        0
    F         ∞        -1        0
B入队列
    — — — — — —
    B
    — — — — — —

判断C没有访问过，并且C的T[A][C]=∞ > T[A][A] + G[A][C] = 0 + 1，更新：
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        0
    C         1         A        0
    D         ∞        -1        0
    E         ∞        -1        0
    F         ∞        -1        0
C入队列
    — — — — — —
    B C
    — — — — — —
    — — — — — —
    C B
    — — — — — —

判断D没有访问过，并且D的T[A][D]=∞ !> T[A][A] + G[A][D] = 0 + ∞，不更新：
判断E没有访问过，并且E的T[A][E]=∞ !> T[A][A] + G[A][E] = 0 + ∞，不更新：
判断F没有访问过，并且F的T[A][F]=∞ !> T[A][A] + G[A][F] = 0 + ∞，不更新：

C出队列，标记C为访问过，表示找到其到达起点的最短距离了
    — — — — — —
      B
    — — — — — —
    city_name  dist_2_A  last_city  visited
        A         0        -1        1
        B         5         A        0
        C         1         A        1
        D         ∞        -1        0
        E         ∞        -1        0
        F         ∞        -1        0

判断A被访问过不更新
判断B没有被访问过，并且B的T[A][B]=5 > T[A][C] + G[C][B] = 1 + ∞，不更新
判断C被访问过不更新
判断D被没有访问过，并且D的T[A][D]=∞ !> T[A][C] + G[C][D] = 1 + 4，更新：
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        0
    C         1         A        1
    D         5         C        0
    E         ∞        -1        0
    F         ∞        -1        0

D入队列:
    — — — — — —
      B D
    — — — — — —

判断E没有被访问过，并且E的T[A][E]=∞ > T[A][C] + G[C][E] = 1 + 8，更新
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        0
    C         1         A        1
    D         5         C        0
    E         9         C        0
    F         ∞        -1        0
E入队列:
    — — — — — —
      B D E
    — — — — — —

判断F没有被访问过，并且E的T[A][F]=∞ !> T[A][C] + G[C][F] = 1 + ∞，不更新

B出队列，B标记为访问过，表示找到其到达起点的最短距离了
    — — — — — —
        D E
    — — — — — —
    city_name  dist_2_A  last_city  visited
        A         0        -1        1
        B         5         A        1
        C         1         A        1
        D         5         C        0
        E         9         C        0
        F         ∞        -1        0

判断A为访问过，不更新：
判断B访问过，不更新：
判断B访问过，不更新：
判断D没有访问过，并且D的T[A][D]=5 !> T[A][B] + G[B][D] = 5 + 1，不更新：
判断E没有访问过，并且E的T[A][E]=9 !> T[A][B] + G[B][E] = 5 + ∞，不更新：
判断F没有访问过，并且F的T[A][F]=∞ !> T[A][B] + G[B][F] = 0 + ∞，不更新：

D出队列，标记D被访问过，表示找到其到达起点的最短距离了
    — — — — — —
          E
    — — — — — —
    city_name  dist_2_A  last_city  visited
        A         0        -1        1
        B         5         A        1
        C         1         A        1
        D         5         C        1
        E         9         C        0
        F         ∞        -1        0


判断A为访问过，不更新：
判断B访问过，不更新：
判断B访问过，不更新：
判断D访问过，不更新：
判断E没有访问过，并且E的T[A][E]=9 > T[A][D] + G[D][E] = 5 + 3，更新：
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        1
    C         1         A        1
    D         5         C        1
    E         8         D        0
    F         ∞        -1        0
E入再队列：
    — — — — — —
          E E
    — — — — — —

判断F没有访问过，并且F的T[A][F]=∞ !> T[A][D] + G[D][F] = 5 + 6，更新：

city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        1
    C         1         A        1
    D         5         C        1
    E         8         D        0
    F         11        D        0
F入队列
     — — — — — —
           E E F
     — — — — — —
E出队列
     — — — — — —
             E F
     — — — — — —

判断A为访问过，不更新：
判断B访问过，不更新：
判断B访问过，不更新：
判断D访问过，不更新：
判断E访问过，不更新：
判断F没有访问过，并且F的T[A][F]=11 !> T[A][E] + G[D][E] = 8 + ∞，不更新：
E出队列
     — — — — — —
               F
     — — — — — —
........
E被访问过了，continue跳过;

F出队列
标记F被访问过
city_name  dist_2_A  last_city  visited
    A         0        -1        1
    B         5         A        1
    C         1         A        1
    D         5         C        1
    E         8         D        1
    F         11        D        1
路径逆推：F->D->C->A

判断A访问过，不更新：
判断B访问过，不更新：
判断B访问过，不更新：
判断D访问过，不更新：
判断E访问过，不更新：
判断F访问过，不更新：
队列为空，退出循环

----------------
总结：
    step1 初始化并创建一个二维的顶点信息表，用于维护节点信息
    step2 起点入优先队列
    step3 判断优先队列是否为空，不为空继续往下访问，否则退出循环
    step4 头部指针指向的顶点出优先队列(出列队的过程 == 广度优先遍历的过程)，标记为访问过
    step5 遍历所有顶点，计算与当前出队列的点「邻接的」，并且「没有被访问过的」点的距离 + 当前出对立的点到起点的距离，更新顶点信息表
    step6 将符合上面两个条件的点入优先队列
    step7 返回step3.

出队列的顺序 == 广度优先遍历的顺序

//
// Created by wb on 2022/5/26.
//


#include "Astar.h"


// 测试 用例 START
void test(const char* testName, const vector<vector<int>>& arr, Point s, Point e)
{
    AStar as;
    // 路径存放在数组容器中，仿函数as，入参：二维矩阵地图，起点、终点
    vector<Point> result = as(arr, s, e);
    cout << "访问过的节点数量/生成路径的节点数量 = " << as.visitCount << " / " << result.size() << endl;
    
    // 打印从起点到终点的路径
    for (int i = 0; i < result.size(); ++i)
    {
        cout << "(" << result[i].x << "," << result[i].y << ")";
    }
    cout << endl;
}

// 测试用例
void Test1()
{
    vector<vector<int>> arr =
            {
                {0,0},
            };

    Point s(0, 0);
    Point e(1, 0);
    test("Test1()", arr, s, e);
}

void Test2()
{
    vector<vector<int>> arr =
            {
                {0},
                {0}
            };

    Point s(0, 0);
    Point e(0, 1);

    test("Test2()", arr, s, e);
}

void Test3()
{
    vector<vector<int>> arr =
            {
                {0,0,},
                {0,0,},
            };

    Point s(0, 0);
    Point e(1, 1);

    test("Test3()", arr, s, e);
}

void Test4()
{
    vector<vector<int>> arr =
            {
                {0,0,0,},
                {0,0,0,},
            };

    Point s(0, 0);
    Point e(2, 1);

    test("Test4()", arr, s, e);
}

void Test5()
{
    vector<vector<int>> arr =
            {
                {0,1,0,},
                {0,1,0,},
                {0,0,0,},
            };

    Point s(0, 0);
    Point e(2, 0);

    test("Test5()", arr, s, e);
}

void Test6()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0},
            };

    Point s(2, 2);
    Point e(6, 2);

    test("Test6()", arr, s, e);
}

void Test7()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
                {0,0,0,0,1,0,0,0},
            };

    Point s(2, 2);
    Point e(6, 2);

    test("Test7()", arr, s, e);
}

void Test8()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,0,0,0,0},
                {0,0,0,0,1,1,1,1,0},
                {0,0,0,0,1,0,0,1,0},
                {0,0,0,0,1,0,0,1,0},
                {0,0,0,0,1,0,1,1,0},
                {0,0,0,0,1,0,0,0,0},
            };

    Point s(2, 2);
    Point e(6, 2);

    test("Test8()", arr, s, e);
}

void Test9()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
            };

    Point s(2, 2);
    Point e(6, 3);

    test("Test9()", arr, s, e);
}

void Test10()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,1,0,0,0,0,0},
                {0,0,0,0,1,0,0,0,0,0},
                {0,0,0,0,1,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0},
            };

    Point s(2, 2);
    Point e(6, 3);

    test("Test10()", arr, s, e);
}

void Test11()
{
    vector<vector<int>> arr =
            {
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0},
                {0,0,0,0,0,0,0,0,0,0,0,0},
                {0,0,0,0,0,1,0,0,0,0,0,0}
            };

    Point s(1, 1);
    Point e(10, 5);// (x,y) (列,行)

    test("Test11()", arr, s, e);
}




int main()
{
    /*
    Test1();
    Test2();
    Test3();
    Test4();
    Test5();
    Test6();
    Test7();
    Test8();
    */
    Test9();
    Test10();
    Test11();
}

/*
(2,2), (3,3), (4,4), (5,3), (6,3)
BSF：
    BFS以起点A为圆心，先搜索A周围的所有点，形成一个类似圆的搜索区域，
    再扩大搜索半径，进一步搜索其它没搜索到的区域，直到终点B进入搜索区域内被找到。
DFS：
    深度优先搜索不是所有路径都搜索而是沿着终点方向搜索
    DFS是让搜索的区域离起点尽量远，离终点尽量近
对比：
    比起BFS，DFS因为尽量靠近终点的原则，其实是用终点相对与当前点的方向为导向，所以有一个大致的方向，就不用盲目地去找了
    这样，就能比BFS能快地找出来最短路径，但是这种快速寻找默认起点A终点B之间没有任何障碍物
    ，地形的权值也都差不多。如果起点终点之间有障碍物，那么DFS就会出现绕弯的情况。
    在DFS遇到障碍物时，其实没有办法找到一条最优的路径，只能保证DFS会提供其中的一条路径

    BFS保证的是从起点到达路线上的任意点花费的代价最小（但是不考虑这个过程是否要搜索很多格子）；
    DFS保证的是通过不断矫正行走方向和终点的方向的关系，使发现终点要搜索的格子更少（但是不考虑这个过程是否绕远）。
A*思想：
    A*算法的设计同时融合了BFS和DFS的优势，既考虑到了从起点通过当前路线的代价（保证了不会绕
    ，又不断的计算当前路线方向是否更趋近终点的方向（保证了不会搜索很多图块）
    是一种静态路网中最有效的直接搜索算法。

    运用的思想：先进行一次DFS搜索再进行一次BFS搜索，循环这个过程直到找到目标点。
    过程1，运用DFS思想，尽量找离终点近的点（也就是代价值最小的点）。
    过程2，运用BFS思想，以点K为圆心，搜索A周围的所有还未搜索的点。
代价值计算：
    F = G + H
        G = 从起点移动到当前点的移动代价
        H = 从当前点移动到终点的移动代价
A*算法查找过程总结：
    1.创建一个辅助地图(和原地图的尺寸一样大小)，用来存放所有访问过的节点
    2.起点入队列，存入辅助地图
    3.判断是否找到终点或者队列为空，若是则退出循环
    4.优先队列头指针指向的节点出队列，标记为已访问，作为当前节点
    5.遍历当前节点的未访问过的邻居节点(BFS思想)
        5.1.计算邻居节点的f值，当做最小f值，标记为访问过
        5.2.把当前邻居节点的中心节点当做是父节点
        5.3.以当前邻居节点为中心，遍历其周围访问过的节点，假设为父节点
        5.4.重新计算g值，g值最小且最后访问过的节点为当前邻居节点的父节点
            g越小，f越小(h不变)，距离终点就越进，DFS思想
        5.5.将当前邻居节点信息添加到辅助地图中和优先队列中
        5.6.如果当前邻居节点是终点就退出循环
    6.返回5，直到把当前节点的未访问过的节点入队列标记为访问过
    7.返回3

*/

Astar.cpp

#include "Astar.h"
#include <iostream>
using namespace std;

// 仿函数as，入参：二维矩阵地图，起点、终点
vector<Point> AStar::operator()(const vector<vector<int>>& arr, Point s, Point e)
{
    if (arr.empty() || s == e)
        return {};
    // 地图高 行数，大容器中小容器的个数
    int lenY = (int)arr.size() - 1;
    cout << "地图的高 = " << lenY << endl;
    // 地图宽 列数，小容器中元素的个数
    int lenX = (int)arr[0].size() - 1;
    cout << "地图的宽 = " << lenX << endl;
    // 如果起点或终点 超出地图边界
    if (s.x > lenX || s.y > lenY || e.x > lenX || e.y > lenY)
        return {};
    // 如果起点或终点 位于障碍物上，y值代表行，x值代表列
    if (arr[s.y][s.x] != 0 || arr[e.y][e.x] != 0)
        return {};

    for (int i = 0; i < lenY; ++i)
        assert(lenX == (int)arr[i].size() - 1);

    // 该二维矩阵存放所有访问过的节点的节点信息，根据该节点坐标来索引该节点
    // 二维矩阵中的每一个元素都是一个结构体
    vector<vector<Point_Cell>> pArr(lenY + 1, vector<Point_Cell>(lenX + 1));
    
    /*
    优先队列，优先级高的排在前面(先出队列)
    priority_queue<Type, Container, Functional>
    Type:数据类型、 Container：用数组实现的容器、Functional：比较的方式
    */
    priority_queue<Point_Cell*, vector<Point_Cell*>, comp> openList;

    // 重载- ， h = (起点坐标 - 终点坐标)*一个栅格的长度
    int g = 0, h = (s - e) * 10, f = g + h;

    // 初始化起点，起点的父节点是自己
    Point_Cell pt(f, g, h, s.x, s.y);

    // 起点信息存入空地图，petential_array??
    pArr[s.y][s.x] = pt;

    // 把起点信息的地址放入优先队列
    openList.push(&pArr[s.y][s.x]);
    // 标记为访问过 TODO
    // pt.visited = true;

    // 找到终点
    bool seek = true;
    /*

         |
    -----o------>x
         |
         ↓ y
    父节点搜索顺序是：从右边开始顺时针搜索
    */

    //                        y x
    const int dirs[8][3] = { {0,1,10},// 右
                             {1,1,14},// 右下
                             {1,0,10},// 下
                             {1,-1,14},// 左下
                             {0,-1,10},// 左
                             {-1,-1,14},// 左上
                             {-1,0,10},// 上
                             {-1,1,14} };// 右上
    cout <<"sizeof(dirs) = " << sizeof(dirs)<< endl;
    cout <<"sizeof(dirs[0]) = " << sizeof(dirs[0])<< endl;

    // 找到终点或者队列为空才退出
    while (seek && !openList.empty())
    {
        // 取出优先队列头部数据，p是别名
        Point_Cell& p_curr = *openList.top();
        // 优先队列队列头部出队列，是下一步要走的点，
        // notice: 出队列的过程 == 起点走到终点的过程 
        openList.pop();
        // 标记为已访问
        p_curr.visited = true;

        // notice: BFS思想：遍历当前节点的未访问过的邻居节点
        for (int i = 0; i < GET_ARRAY_LEN(dirs) && seek; ++i)
        {

            // 当前邻居节点坐标，结构体对象
            Point neigh_point(p_curr.x + dirs[i][1], p_curr.y + dirs[i][0]);

            // notice: 需要未被访问
            if (neigh_point.x < 0 || neigh_point.x > lenX || neigh_point.y < 0 || neigh_point.y > lenY
                || arr[neigh_point.y][neigh_point.x] == 1 || pArr[neigh_point.y][neigh_point.x].visited)
                continue;

            // 起点的父节点是自己，只有找到节点对应的父节点，才能够算出最小的g
            // 当前邻居节点的g值 = 父节点的g + 当前节点到父节点的g
            g = p_curr.g + dirs[i][2];
            // h = 当前邻居节点坐标 - 终点坐标
            h = (neigh_point - e) * 10;
            f = g + h;
            // 把当前节点的代价值认为是最小值
            int minf = f;

            // 创建当前邻居节点，入参：当前节点的邻居节点的代价值和坐标
            Point_Cell newPoint(f, g, h, neigh_point.x, neigh_point.y);
            // 标记为访问过
            newPoint.visited = true;

            // 父节点要满足：F值最小且最后访问
            // 把当前邻居节点的中心节点默认设置为父节点，但其实不一定是其父节点
            newPoint.parentNode.x = p_curr.x;
            newPoint.parentNode.y = p_curr.y;

            // notice: DFS思想：把当前邻居节点看作是中心节点，遍历其所有邻居，
            // 直到找到其父节点(就找到了最小g值，即最小F)，即距离终点进的节点
            for (int j = 0; j < GET_ARRAY_LEN(dirs); ++j)
            {
                // 假设为父节点，中心节点
                Point p_parent(neigh_point.x + dirs[j][1], neigh_point.y + dirs[j][0]);

                // 父节点pp：辅助地图中已经被访问过
                // 如果当前节点不越界，不在障碍物上，在队列中没有被访问过，则跳过
                if (p_parent.x < 0 || p_parent.x > lenX || p_parent.y < 0 || p_parent.y > lenY
                    || arr[p_parent.y][p_parent.x] == 1 || !pArr[p_parent.y][p_parent.x].visited)
                    continue;

                // 当前节点的g = 当前节点的父节点的g + 当前节点到其父节点的距离
                g = pArr[p_parent.y][p_parent.x].g + dirs[j][2];
                f = g + h;

                // notice: 如果当前的，不可以把等号去掉，因为虽然代价值相同，但最后一个访问的点才是父节点 
                if (f <= minf)
                {
                    minf = f;
                    f = g + h;
                    // 重新设置代价值
                    newPoint.SetFGH(f, g, h);
                    // g值最小最后访问的点为父节点
                    newPoint.parentNode = p_parent;
                }

            }

            // 将当前邻居节点信息添加到地图中
            pArr[neigh_point.y][neigh_point.x] = newPoint;
            // 将当前邻居节点添加到优先队列中
            openList.push(&pArr[neigh_point.y][neigh_point.x]);
            // 如果当前邻居节点是终点就退出循环
            if (neigh_point == e)
                seek = false;
        }

    }

    if (!pArr[e.y][e.x].visited)
    {
        cout << "无法到达" << endl;
        return {};
    }
    else
    {
        // 从终点开始不断添加当前节点的父节点到数组容器中，就组成了从终点到起点的路径
        vector<Point> path;
        // 往路径中放入终点
        path.push_back(e);

        // 当前点(终点)的父节点，现在开始把终点看做是当前节点
        Point p = pArr[e.y][e.x].parentNode;
        // ----------------回溯---------------------
        while (true)
        {
            if (!pArr[p.y][p.x].visited)
            {
                cout << "无法到达" << endl;
                return {};
            }
            // 存入当前节点的父节点
            path.push_back(p);
            // 如果当前点的父节点 == 起点，则退出
            if (p == s)
                break;
            // 更新p(父节点的父节点)
            p = pArr[p.y][p.x].parentNode;
        }
        // 把数组容器中的节点逆置，就变成了从起点到终点的路径
        reverse(path.begin(), path.end());
        SetVisitedCount(pArr);	// 辅助测试，记录访问的结点数

        return path;
    }
}

Astar.h

//
// Created by wb on 2022/5/26.
//

#ifndef ASTAR_QUEUE_ASTAR_H
#define ASTAR_QUEUE_ASTAR_H


#include <cassert>
#include <vector>
#include <list>
#include <iostream>
#include <queue>
#include <algorithm>
using namespace std;


// 数组长度
#define GET_ARRAY_LEN(array) (sizeof(array)/sizeof(array[0]))

struct Point
{
    int x;		// 宽
    int y;		// 高
    // 构造函数
    Point(int tx = 0, int ty = 0) : x(tx), y(ty) {}

    // 两个坐标距离：横坐标距离 + 纵坐标距离
    int operator - (const Point& p) const
    {
        return abs(x - p.x) + abs(y - p.y);
    }

    bool operator == (const Point& p) const
    {
        return x == p.x && y == p.y;
    }
};


// 栅格中一个解点的全部信息
struct Point_Cell : public Point
{
    // f = g + h
    int f, g, h;
    // 是否被访问过，0：未被访问，1已经被访问
    bool visited;	
    // 父节点坐标
    Point parentNode;
    // 构造函数初始化列表，要创建子类对象，必须先创建父类对象
    Point_Cell(int tf = 0, int tg = 0, int th = 0, int tx = 0, int ny = 0) :
        Point(tx, ny), f(tf), g(tg), h(th), visited(false), parentNode() {};

    void SetFGH(int tf, int tg, int th)
    {
        f = tf; g = tg, h = th;
    }
};

// 重写仿函数, 优先队列元素大小比较
struct comp //重写仿函数
{
    bool operator() (Point_Cell* a, Point_Cell* b)
    {
        return a->f > b->f; //小顶堆，小的排在前面
    }
};

// A* 算法
class AStar
{
public:
    // arr 是一个二维数组
    // s 起点; e 终点
    vector<Point> operator()(const vector<vector<int>>& arr, Point s, Point e);

    // 辅助测试，用于获取访问的结点数
    void SetVisitedCount(const vector<vector<Point_Cell>>& pArr)
    {
        visitCount = 0;
        for (int i = 0; i < pArr.size(); ++i)
        {
            for (int j = 0; j < pArr[i].size(); ++j)
            {
                if (pArr[i][j].visited)
                    ++visitCount;
            }
        }
    }

    int visitCount;
};


#endif //ASTAR_QUEUE_ASTAR_H