A*练习..

A*练习

#include <bits/stdc++.h>

#define D 1000
#define N 10
#define INF 0x3f3f3f3f
#define D2 D*sqrt(2)
using namespace std;
void show();

struct Node{// 节点结构体
    int x,y;
    double f,h,g;// 启发函数值。
    void setH(Node e){
        int dx = abs(x - e.x);
        int dy = abs(y - e.y);
        h =  D * (dx + dy) + (D2 - 2 * D) * min(dx, dy);
    }
    void setG(Node e){
        int dx = abs(x - e.x);
        int dy = abs(y - e.y);
        g =  D * (dx + dy) + (D2 - 2 * D) * min(dx, dy);
    }
    void setF(){
        f = g + h;
    }
}start_p ,end_p;// 起点和终点
vector<Node> open,close;// openlist和closelist
map<pair<int,int>,pair<int,int> > mp;// 当前节点到父亲节点的一个映射
int Map[N][N]={// 地图
 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
 { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 0, 1, 3, 0, 0, 0 },
 { 0, 0, 2, 0, 0, 1, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
 { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } };
int d[8][2] = {{0,1},{0,-1},{1,0},{-1,0},{1,1},{-1,1},{1,-1},{-1,-1}};// 偏移量函数

bool check(int x,int y){// 判断是否跳过的检测函数
    bool isin = 0;
    for(int i=0;i<close.size();i++){// 已经在close 中
        if(close[i].x == x && close[i].y == y) isin = true;
    }
    return (isin||(x<0||y<0||x>=N|y>=N||Map[x][y] == 1));// 或者这个点不合法
}

int inOpen(Node &node){// 查找节点是否在open表中
    for(int i=0;i<open.size();i++){
        if(open[i].x == node.x && open[i].y == node.y)
            return i;
    }
    return INF;
}

bool cmpNode(Node a,Node b){
    return a.f < b.f;
}
int dis=0;
void find_way(Node node){// 回溯搜索路径
    int x = node.x,y = node.y;
    pair<int,int>fa;
    fa = mp[{x,y}];
    while(fa.first != -1 && fa.second != -1){
        dis++;
        Map[fa.first][fa.second ] = '*';
        fa = mp[{fa.first,fa.second}];
    }
}
void A_star(){// A*核心代码
    start_p.f = start_p.g = start_p.h = 0;
    open.push_back(start_p);
    while(open.size()){
        Node u = open[0];
        open.erase(open.begin());// 每次取出第一个并加入close
        close.push_back(u);
        if(u.x == end_p.x && u.y == end_p.y){// 终止条件
            
            find_way(u);
            show();
            break;
        }

        for(int i=0;i<8;i++){// 向八个方向搜索
            int nx = u.x + d[i][0];
            int ny = u.y + d[i][1];
            if(check(nx,ny)) continue;
            Node v = u;
            v.x = nx;
            v.y = ny;
            v.setG(end_p);
            v.setH(start_p);
            v.setF();

            int idx = inOpen(v);
            if(idx != INF){// 更新open表
                if(open[idx].f > v.f){
                    open[idx].g = v.g;
                    open[idx].h = v.h;
                    open[idx].f = v.f;
                    mp[{nx,ny}] = {u.x,u.y};
                }
            }else{// 加入open表
                mp[{nx,ny}] = {u.x,u.y};
                open.push_back(v);
            }
        }
        std::sort(open.begin(),open.end(),cmpNode);// 按启发函数值从小到大排序
    }
}

void show(){// 打印地图和路径结果
    printf("distance = %d\n\n",dis);   
    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            if(i == end_p.x && j == end_p.y) cout << "X" << ' ';
            else if(i == start_p.x && j == start_p.y) cout << "S"<<' ';
            else if(Map[i][j] == '*') cout <<'*'<<' ';
            else if(Map[i][j]) cout << "# ";
            else cout << ". ";
        }
        cout << endl;
    }

}
void init(){// 初始化函数
    start_p.x = 5;
    start_p.y = 2;
    mp[{5,2}] = {-1,-1};
    end_p.x = 4;
    end_p.y = 6;
}
int main(){
    init();
    A_star();
    cout << "Please press any to exit it";
    getchar();
}