A*启发式搜索 模板

博主本身很想提高一下自己的水平，于是打算写一下以前一直觉得很难的A*启发式搜索，努力工作了一个多小时，终于把这么一个恶心的程序调试完成了（不要问我启发式搜索是什么，网上有的是，博主只是给大家提供一下参考代码），这个代码都有注释，而且测试通过了（最末端有博主的测试数据之一），希望大家可以因此有所提高！

/*ID: ljf_cnyaliPROG: A*LANG: C++*/#include<bits/stdc++.h>using namespace std;#define REP(i, a, b) for(int i = (a), _end_ = (b);i <= _end_; ++ i)#define mem(a) memset((a), 0, sizeof(a))#define str(a) strlen(a)const int maxn = 1010;int Row, Col;//地图大小 struct block {    int f, g, h, Fx, Fy, Mx, My;//F = G + H, Fx, Fy是父亲的坐标, Mx, My是现在的坐标 };int dt[4][2] = {{0, -1}, {-1, 0}, {1, 0}, {0, 1}};//east, south, north, weststruct block Open[maxn], Close[maxn];//队列int Startx, Starty, Endx, Endy;//起点终点坐标int Map[maxn][maxn];//地图, 1代表墙 int CountOpen, CountClose;//队列坐标个数 void INIT() {    scanf("%d%d", &Row, &Col);    REP(i, 0, Row - 1)        REP(j, 0, Col - 1)            scanf("%d", &Map[i][j]);    scanf("%d%d%d%d", &Startx, &Starty, &Endx, &Endy);} int getG(int x, int y) {    return abs(x - Startx) + abs(y - Starty); } int getH(int x, int y) {    return abs(x - Endx) + abs(y - Endy);}void InOpen(int Fx, int Fy, int Mx, int My) {    Open[CountOpen].g = getG(Mx, My);    Open[CountOpen].h = getH(Mx, My);    Open[CountOpen].f = Open[CountOpen].g + Open[CountOpen].h;    Open[CountOpen].Fx = Fx;    Open[CountOpen].Fy = Fy;    Open[CountOpen].Mx = Mx;    Open[CountOpen].My = My;    ++CountOpen;        }void DeleteOpen(int index) {    REP(i, index, CountOpen - 1)        Open[i] = Open[i + 1];    --CountOpen;}void InClose(int index) {    Close[CountClose] = Open[index];    ++CountClose;       DeleteOpen(index);//将index从Open中删去 }void PrintfPath() {    int ans = 0;    //目的地的父亲坐标     int Fx = Close[CountClose - 1].Fx;    int Fy = Close[CountClose - 1].Fy;    //找目的地的父亲，向上推    for(int i = CountClose - 1;i >= 0; -- i)         if(Close[i].Mx == Fx && Close[i].My == Fy) {            printf("Result: %d, %d\n", Fx, Fy);            Fx = Close[i].Fx;            Fy = Close[i].Fy;            ++ans;        }    printf("%d\n", ans);//输出距离 }int SearchRound(int x, int y) {    bool flag;    REP(i, 0, 3) {        int dx = dt[i][0] + x;        int dy = dt[i][1] + y;        flag = true;        //即将进入Open里的检查：Close数组检查、Open数组检查、墙、能进Open        //Close或Open数组里已经包含了则不能进入        REP(j, 0, CountClose - 1)            if(dx == Close[j].Mx && dy == Close[j].My) {                flag = false;                break;            }        REP(j, 0, CountOpen - 1)            if(dx == Open[j].Mx && dy == Open[j].My) {                flag = false;                break;              }        if(Map[dx][dy] == 1)            flag = false;        //如果flag为真，则加入Open        if(flag)            InOpen(x, y, dx, dy);           //是否到达终点        if(dx == Endx && dy == Endy)            return 1;       }    return 0;}void ShorestPath() {    int x = Startx, y = Starty, k;    //设置起点    Open[CountOpen].g = getG(x, y);//查找G(x, y)的值     Open[CountOpen].h = getH(x, y);//查找H(x, y)的值    Open[CountOpen].f = Open[CountOpen].g + Open[CountOpen].h;//计算F(x, y)的值    Open[CountOpen].Fx = -1;    Open[CountOpen].Fy = -1;    Open[CountOpen].Mx = x;    Open[CountOpen].My = y;    ++CountOpen;//队列继续    //选出Open中F值最小的一个     while(1) {        int min = 1000000000, index = 0;        REP(i, 0, CountOpen - 1)             if(Open[i].f < min) {                min = Open[i].f;                index = i;            }        //把F最小的放入Close中         InClose(index);        int flag = SearchRound(Close[CountClose - 1].Mx, Close[CountClose - 1].My);//寻找可以走的路径         if(flag == 1) {            InClose(CountOpen - 1);//把终点放入Close中             PrintfPath();//输出路径,注意是从后往前输出的             break;          }    }}int main() {//  freopen("A*.in", "r", stdin);//  freopen("A*.out", "w", stdout);    INIT();//读入地图    ShorestPath();//主代码     return 0;}/*8 91 1 1 1 1 1 1 1 11 0 0 0 0 0 0 0 11 0 0 0 1 0 0 0 11 0 0 0 1 0 0 0 11 0 0 0 1 0 0 0 11 0 1 0 1 0 0 0 11 0 0 0 0 0 0 0 11 1 1 1 1 1 1 1 14 2 1 7*/