寻路算法（A*算法）实现（附C++代码）

–需求是求A到B的最短路径。这里的行走方案是上下左右，左上、左下、右上、右下八个方向（这个根据实际情况实际处理）。蓝色的块是障碍，不能行走。

步骤：
–维护待处理点的队列，初始只有起点即A。
–取出待处理点向各方向走。
如果到达点还没有点到达过，那么将处理点作为这个到达点的前点（用于找出路径），并设置到达点的距离为处理点+距离。
如果到达点已经有点到达过了，那么判断处理点的距离+两点距离是否小于到达点的距离，是的话将处理点设置为到达点的前点，并修改距离。
–结束条件：处理点队列内部没有数据点。

下面是C++代码

#include <iostream>
#include <vector>
#include <queue>

using namespace std;

struct Points
{
    int x;
    int y;
    Points(int xVal, int yVal):x(xVal), y(yVal){};
    string ToString()
    {
        return "(" + to_string(x) + "," + to_string(y) + ")";
    }
};

vector<Points> FindClosestPath(const vector<vector<bool>>& mat, const Points& start, const Points& end)
{
    queue<Points> inSerch;
    inSerch.push(start);
    
    ssize_t width = mat.size();
    ssize_t height = mat[0].size();
    vector<vector<pair<int, Points>>> datas;
    vector<pair<int, Points>> temp;
    for (int i = 0; i < height; i++)
    {
        temp.push_back(make_pair(-1, Points(0, 0)));
    }
    for (int i = 0; i < width; i++)
    {
        datas.push_back(temp);
    }
    
    datas[start.x][start.y].first = 0;
    
    auto bInMat = [width, height](Points point)->bool{
        return point.x >= 0 && point.y >= 0 && point.x < width && point.y < height;
    };
    
    // leftTop, left, leftBottom, Top, Bottom, RihgtTop, Right, RightBottom
    vector<Points> offsets = {Points(-1, -1), Points(-1, 0), Points(-1, 1), Points(0, -1), Points(0, 1), Points(1, -1), Points(1, 0), Points(1, 1)};
    vector<int> distances = {14, 10, 14, 10, 10, 14, 10, 14};
    
    while (inSerch.size() != 0) {
        ssize_t count = inSerch.size();
        for (int i = 0; i < count; i++)
        {
            Points point = inSerch.front();
            inSerch.pop();
            
            for (int j = 0; j < offsets.size(); j++)
            {
                Points dst = Points(point.x + offsets[j].x, point.y + offsets[j].y);
                if (bInMat(dst) && mat[dst.x][dst.y])
                {
                    if (datas[dst.x][dst.y].first == -1 || datas[dst.y][dst.y].first > datas[point.x][point.y].first + distances[j])
                    {
                        datas[dst.x][dst.y].first = datas[point.x][point.y].first + distances[j];
                        datas[dst.x][dst.y].second = point;
                        
                        if (dst.x != end.x || dst.y != end.y)
                        {
                            inSerch.push(dst);
                        }
                    }
                }
            }
        }
        
    }
    
    vector<Points> res;
    
    Points current = end;
    while (datas[current.x][current.y].first != -1)
    {
        res.push_back(current);
        current = datas[current.x][current.y].second;
        
        if (current.x == start.x && current.y == start.y)
        {
            res.push_back(current);
            break;
        }
    }
    
    reverse(res.begin(), res.end());
    return res;
}

int main(int argc, const char * argv[]) {
    
    vector<vector<bool>> mat = {
        {true, true, true, true, true},
        {true, true, true, true, true},
        {true, true, true, true, true},
        {true, false, false, false, false},
        {true, true, true, true, true},
        {true, true, true, true, true},
        {true, true, true, true, true}
    };
    
    vector<Points> res = FindClosestPath(mat, Points(1, 2), Points(4, 2));
    
    for (int i = 0; i < res.size(); i++)
    {
        cout << res[i].ToString();
        if (i != res.size() - 1)
        {
            cout << " -> ";
        }
    }
    
    if (res.empty())
    {
        cout << "No Way" << endl;
    }
    
    return 0;
}