A*启发式搜索模板

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

/*
ID: ljf_cnyali
PROG: 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, west

struct 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 9
1 1 1 1 1 1 1 1 1
1 0 0 0 0 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 0 0 1 0 0 0 1
1 0 1 0 1 0 0 0 1
1 0 0 0 0 0 0 0 1
1 1 1 1 1 1 1 1 1
4 2 1 7
*/