A* 算法 C++

#include <iostream>
#include <cstdio>
#include<cstring>
#include <queue>
#include <algorithm>
#include <cmath>
#include <stack>
// #include <array>

using namespace std;

const int N = 10;

int map[N][N];

class node{

    public:
        // 节点有无被初始化,用以判空
        bool hasInited = false;
        // 节点x坐标
        int m_nx = 0;
        // 节点y坐标
        int m_ny = 0;
        // 节点离起点的最近距离值,bfs搜索中不断更新
        int cost = 0;
        // 节点离终点的耗费,(用曼哈顿距离计算)
        int rest = 0;

    public:
        node(){};
        node(int x,int y,int cost,int rest){
            this->hasInited=true;
            this->m_nx=x;
            this->m_ny=y;
            this->cost=cost;
            this->rest=rest;
        };
        bool operator < (const node &b) const {
            int fa = this->cost + this->rest;
            int fb = b.cost + b.rest;
            return fa > fb;
        };
        bool operator == (const node &b) const {
            return this->m_nx == b.m_nx && this->m_ny == b.m_ny;
        }
};

int dir[8][2] = {
    {0,1},
    {1,1},
    {1,0},
    {1,-1},
    {0,1},
    {-1,-1},
    {-1,0},
    {-1,1},
};

bool vis[N][N];
bool in_queue[N][N];

stack<node> st;

//存放父节点
node pre[N][N];

bool ok(int x,int y){
    return x>=0 && y>=0 && x<N && y<N && map[x][y] > 0 && !vis[x][y];
}

void print_path(){
    printf("打印路径.....\n");
    node now = st.top();
    st.pop();
    printf("(%d,%d)",now.m_nx,now.m_ny);
    while(st.size()){
        node now = st.top();
        st.pop();
        printf("-->(%d,%d)",now.m_nx,now.m_ny);
    }
    printf("\n");
}

bool bfs_astar(node start, node end){
    priority_queue<node> q;
    q.push(start);
    in_queue[start.m_nx][start.m_ny] = true;
    vis[start.m_nx][start.m_ny] = true;
    node now;
    int x,y,nx,ny;
    while(q.size()){
        now = q.top();
        q.pop();
        x = now.m_nx;
        y = now.m_ny;
        vis[x][y] = true;
        in_queue[x][y] = false;
        printf("当前位置%d %d\n",x,y);
        // 判断是否是终点
        if(now == end){
            printf("找到终点\n");
            st.push(now);
            while(pre[now.m_nx][now.m_ny].hasInited){
                now = pre[now.m_nx][now.m_ny];
                st.push(now);
            }
            print_path();
            return true;
        }
        for(int i=0;i < 8;i++){
            nx = x + dir[i][0];
            ny = y + dir[i][1];
            if(ok(nx,ny)){
                //计算cost rest值
                int cost = now.cost + map[nx][ny];
                int rest = abs(end.m_nx - nx) + abs(end.m_ny - ny);
                if(in_queue[nx][ny]){
                    priority_queue<node> qq;
                    while(q.size()){
                        node temp = q.top();
                        q.pop();
                        if(temp.m_nx == nx && temp.m_ny == ny){
                            if(temp.cost > cost){
                                temp.cost = cost;
                                pre[nx][ny] = now;
                            }
                        }
                        qq.push(temp);
                    }
                    q = qq;
                }
                pre[nx][ny] = now;
                q.push(node(nx,ny,cost,rest));
                in_queue[nx][ny] = true;
            }
        }
    }
    return false;
}   
bool bfs(node start, node end){
    queue<node> q;
    q.push(start);
    vis[start.m_nx][start.m_ny] = true;
    node now;
    int x,y,nx,ny;
    while(q.size()){
        now = q.front();
        q.pop();
        x = now.m_nx;
        y = now.m_ny;
        vis[x][y] = true;
        printf("当前位置%d %d\n",x,y);
        // 判断是否是终点
        if(x == end.m_nx && y == end.m_ny){
            printf("找到终点\n");
            return true;
        }
        for(int i=0;i<8;i++){
            nx = x + dir[i][0];
            ny = y + dir[i][1];
            if(ok(nx,ny)){
                q.push(node(nx,ny,0,0));
            }
        }
    }
    return false;
}   


int main(){
    int sx,sy,ex,ey;
    int N;
    while(true){
        scanf("%d ",&N);
        memset(vis,0,sizeof vis);
        memset(pre,0,sizeof pre);
        memset(in_queue,0,sizeof in_queue);
        scanf("%d %d %d %d",&sx,&sy,&ex,&ey);
        for(int i=0;i<N;i++){
            for(int j=0;j<N;j++){
                scanf("%d",&map[i][j]);
            }
        }

        for(int i=0;i<N;i++){
            for(int j=0;j<N;j++){
            printf("%d ",map[i][j]);
            }
            printf("\n");
        }
        bfs_astar(node(sx,sy,0,abs(ex-sx)+abs(ey-sy)),node(ex,ey,map[ex][ey],0));
        printf("\n");

    }
    system("pause");
    return 0;
}

/*
0 0 3 0
1 1 -1 1 1
0 1 -1 1 1
0 1 -1 0 0
1 0  0 1 1
0 0  0 1 1

6
0 0 3 5
1 1 -1 1 1 0
0 1 -1 1 1 0
0 1 -1 0 1 0
1 0  0 1 0 1
2 1  1 0 -1 0
0 0 0 0 0 0
*/

A* 算法本质是BFS+启发函数,相比普通BFS快了大约两倍多。启发函数一般使用曼哈顿距离估算剩余耗费rest(2D)

本例中使用了优先级队列来自动排序结点,但优先级队列不可遍历,故增加了一层O(N)的手动遍历的办法 来修改节点

A* 多用于寻路

  • 1
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值