基于栅格地图的A*算法

#include<iostream>
#include<vector>
using namespace std;
struct Node2D {
    int x;
    int y;
    float g;
    float h;
    float f;
    int idx;
    int preidx;
};

Node2D popNode2D(std::vector<Node2D> &O)
{
    float value = 10000;
    int index = 0;
    for (int i = 0; i < O.size(); i++)
    {
        float f = O[i].g + O[i].h;
        if (f < value) {
            value = f;
            index = i;
        }
    }
    Node2D output = O[index];
    O.erase(O.begin() + index);
    return output;
}

bool Astar(int *map, int height, int width, int startx, int starty, int endx, int endy, vector<int> &pathx, vector<int> &pathy)
{
    //map二维数组，1为障碍物，0为非障碍物
    if (startx < 0 || startx >= width || starty < 0 || starty >= height || endx < 0 || endx >= width || endy < 0 || endy >= height)
        return false;
    if (map[starty*width + startx] == 1 || map[endy*width + endx] == 1)//起点或终点是障碍物
        return false;


    int directions[8][2] = { { -1, 1 },{ 0, 1 },{ 1, 1 },{ -1, 0 },{ 1, 0 },{ -1, -1 },{ 0, -1 },{ 1, -1 } };//八邻域搜索方向
    short *isVisited = new short[height*width];//表示节点状态，0:未访问，1：open,2:close
    for (int i = 0; i < height*width; i++)
        isVisited[i] = 0;

    Node2D* nodes2d = new Node2D[width*height];//存储所有节点

    Node2D startNode, endNode;//初始化起点和终点
    startNode.x = startx;
    startNode.y = starty;
    startNode.g = 0;
    startNode.h = abs(startNode.x - endx) + abs(startNode.y - endy);//使用曼哈顿距离
    startNode.f = startNode.g + startNode.h;
    startNode.idx = starty*width + startx;
    startNode.preidx = -1;//起点没有父节点

    endNode.x = endx;
    endNode.y = endy;


    Node2D nPred;//当前节点
    Node2D nSucc;//子节点
    int iPred, iSucc;//节点id

    vector<Node2D> O;//开列表
    vector<Node2D> C;//闭列表
    O.push_back(startNode);
    isVisited[startNode.idx] = 1;
    nodes2d[startNode.idx] = startNode;

    while (!O.empty())
    {
        nPred = popNode2D(O);
        iPred = nPred.y * width + nPred.x;

        if (nPred.x == endNode.x && nPred.y == endNode.y)
        {
            Node2D tempNode = nPred;
            while (tempNode.preidx > -1)
            {
                pathx.push_back(tempNode.x);
                pathy.push_back(tempNode.y);
                tempNode = nodes2d[tempNode.preidx];
            }
            pathx.push_back(tempNode.x);
            pathy.push_back(tempNode.y);

            int start = 0;
            int end = pathx.size()-1;
            while (start < end)//反转一下顺序
            {
                swap(pathx[start], pathx[end]);
                swap(pathy[start], pathy[end]);
                start++;
                end--;
            }
            delete[]isVisited;
            delete[]nodes2d;
            return true;
        }

        isVisited[iPred] = 2;//更新为close状态
        C.push_back(nodes2d[iPred]);


        for (char i = 0; i < 8; i++)
        {
            int xSucc = nPred.x + directions[i][0];
            int ySucc = nPred.y + directions[i][1];
            if (xSucc >= 0 && xSucc < width && ySucc >= 0 && ySucc < height && (map[ySucc*width + xSucc] != 1 && isVisited[ySucc*width + xSucc] != 2))
                //边界检测、非障碍物、不在closed链表中
            {
                nSucc.x = xSucc;
                nSucc.y = ySucc;
                nSucc.idx = ySucc*width + xSucc;
                nSucc.preidx = nPred.idx;
                nSucc.g = nPred.g + sqrt((xSucc - nPred.x) * (xSucc - nPred.x) + (ySucc - nPred.y) * (ySucc - nPred.y));
                nSucc.h = abs(nSucc.x - endNode.x) + abs(nSucc.y - endNode.y);
                nSucc.f = nSucc.g + nSucc.h;
                iSucc = nSucc.y * width + nSucc.x;

                if (isVisited[iSucc] == 0)//第一次访问
                {
                    isVisited[iSucc] = 1;//更新状态
                    nodes2d[iSucc] = nSucc;
                    O.push_back(nSucc);//插入Open
                }
                else if (isVisited[iSucc] == 1 && nSucc.f + 0.00001 < nodes2d[iSucc].f)//处于open表中
                {
                    nodes2d[iSucc] = nSucc;
                    for (int i = 0; i < O.size(); i++) //查找Open表中的该点并替换
                    {
                        if (O[i].x == nSucc.x && O[i].y == nSucc.y) {
                            O[i] = nSucc;//更新Open
                            break;
                        }
                    }
                }
            }
        }

    }
    delete[]isVisited;
    delete[]nodes2d;
    return false;
}

int main()
{


    int map[5][5] = {
        0,0,1,0,1,
        0,1,1,1,0,
        1,0,1,0,1,
        0,1,0,1,0,
        0,0,0,0,0
    };
    for (auto i = 0; i < 5; i++) {
        {
            for (auto j = 0; j < 5; j++)
                cout << map[i][j] << " ";
            cout << endl;
        }
    }

    vector<int>x;
    vector<int>y;
    bool result = Astar(&map[0][0], 5, 5, 0, 0, 4, 4, x, y);
    if (result == true)
    {
        cout << "Find Path:" << endl;
        for (int i = 0; i < x.size(); i++)
            cout << x[i] << " " << y[i] << endl;
    }
    else
        cout << "No Path" << endl;
    system("pause");
    return 0;
}