A*搜索算法

A*搜索算法（A* search algorithm）是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。

A*不是贪心算法。

A*很好用，不难学，简单易懂。

通常用于游戏NPC寻路。

（啊？

该算法综合了最良优先搜索和Dijkstra算法的优点：在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径。

A*搜索算法很像隔壁的广度优先，区别在于广度有限是无脑向4个方向拓展，而A*只向着代价(cost)最低的方向拓展。

代价为当前步数+曼哈顿距离。

（为什么请找高数dalao，我也不会www）

据说步数也可以是当前步数+正常的距离（sqrt(x^2-y^2))

#include 
#include 
#include 
using namespace std;
int dx[4] = {0, 1, 0, -1};
int dy[4] = {1, 0, -1, 0};
int m[100][100], v[100][100];
int cost[5];
struct road{
    int x;
    int y;
    int step;
    int cost;
};
queue ma;
int Manhaton(int x1, int y1, int x2, int y2){
    return abs(x1 - x2) + abs(y1 - y2);
}
int main(){
    int a, b;
    cin >> a >> b;
    int nu;
    for (int i = 1; i <= a; i++)
        for (int j = 1; j <= b; j++){
            cin >> nu;
            m[i][j] = nu + 1;
        }
    int startx, starty, endx, endy;
    cin >> startx >> starty >> endx >> endy;
    road start;
    start.x = startx;
    start.y = starty;
    start.step = 0;
    ma.push(start);
    v[startx][starty] = 1;
    bool flag = 0;
    while (!ma.empty()){
        road next[4];
        start = ma.front();
        if (start.x == endx && start.y == endy){
            cout << start.step << endl;
            flag = 1;
            break;
        }
        for (int k = 0; k < 4; k++){
            next[k].x = start.x + dx[k];
            next[k].y = start.y + dy[k];
            next[k].step = start.step + 1;
            next[k].cost = next[k].step + Manhaton(next[k].x, next[k].y, endx, endy);
        }
        int mincost = 999999;
        int mink = 9;
        for (int ff = 0; ff < 4; ff++){
            if (m[next[ff].x][next[ff].y] == 1 && v[next[ff].x][next[ff].y] == 0)
            {
                mincost = min(mincost, next[ff].cost);
                if (mincost == next[ff].cost)
                    mink = ff;
                v[next[ff].x][next[ff].y] = 1;
            }
        }
        if (mincost != 999999 || mink != 9)
            ma.push(next[mink]);
        ma.pop();
    }
    if (flag == 0)
        cout << "Not Found" << endl;
    return 0;
}