CPPDay20 A星寻路算法

最新推荐文章于 2021-06-21 17:09:21 发布

'"<>{{7*7}}

最新推荐文章于 2021-06-21 17:09:21 发布

阅读量318

点赞数

分类专栏： # C++ 算法与数据结构

本文链接：https://blog.csdn.net/weixin_43415644/article/details/101147153

版权

C++ 同时被 2 个专栏收录

32 篇文章 1 订阅

订阅专栏

算法与数据结构

25 篇文章 1 订阅

订阅专栏

A星寻路算法是狄克斯特拉算法的优化。

0x00 A星寻路算法的思路：

广度寻路算法中，我们假设小人只能走上下左右四个方位。但是在A*寻路算法中，我们假设小人可以走如上图所示的八个方位。即小人可以走斜线，并且我们规定，走直线上下左右的代价为10，走斜线的代价为14。

第一步：首先计算起点到8个顶点的权重（代价）分别是多少，这个权重不仅仅是起点到该顶点的实际权重，而是起点到该点的实际权重与该点到终点的推测权重之和。计算完毕之后，选择出权重最小的点，更新小人的位置到该点。

第二步：计算从查找完毕的点出发到各个顶点的权重，并从中选择权重最小的一个顶点，并更新小人位置到该点。以此类推，直到到达终点。

总结：A星寻路循环，不断计算下一步走哪个格子所付出的代价最小。走代价最小的格子，直到到达终点。

特别注意：从当前点到某个格子的代价 = 实际代价 + 推测代价，即

f = g + h + w

f:当前点到某个格子的代价

g:实际代价

h:推测代价。

w:地形权重，如果寻路过程中还有地形的区别，比如说上坡，下坡，平地就需要加一个权重。下文中忽略地形权重。

我们知道两点之间直线距离最短，所以应用A星寻路算法，小人从起点开始一定是尽可能直端端的对准终点走的。

0x01那么如何计算推测代价呢？

首先，客观上要允许能够做到事先计算该点到终点的推测代价，如果没有办法计算该点到终点的推测代价，那么就没有办法使用A*寻路算法。

其次，一般而言，推测代价有三种计算方式：

例如平面上有两个点A(x1,y1) B(x2,y2)

1.曼哈顿距离 D = |x1 - x2|+|y1 - y2|

2.切比雪夫距离 D = max(|x1-x2|,|y1-y2|)

3.欧式几何平面直线距离：D=用勾股定理直接算两点之间的直线距离

这里我们采用曼哈顿距离，并且忽略障碍物。

注意：当推测距离与实际两点之间走过的距离相差甚大时，A*寻路算法的效率可能比狄克斯特拉算法的效率更差，甚至可能误导方向，从而造成无法得到最短路径的最终解。

0x02 代码版本一

思路：准备一个辅助地图，来标记一个点有没有走过；准备一个坐标类型，用来定义小人，起点，终点等。准备一个函数用来算总权重，然后直接开始寻路循环。一开始小人位置为起点位置，然后再循环中再嵌套一个循环，这个循环用来把小人周围的点（可以走的点：即没有走过并且是路不是墙的点）的权重都算一遍。于此同时选出权重最小的点。更新小人的位置为权重最小的点。标记该点已经走过。然后不断循环，直到到达终点。

这个思路有一个致命缺点：无法突破口袋阵，因为没有回退机制！

什么是口袋阵？

如下图所示，绿色部分圈起来的便是一个口袋阵，因为终点位于右下方，所以小人从起点开始一定会向右下方走，这样就到了(2,2)这个位置，并且这时标记起点(1,1)已经走过。但是(2,2)周围的八个点都是墙壁，所以这时，小人无路可走，无路可退。陷入一种困境之中。

如何突破口袋阵呢？像深度寻路算法设置一个栈，然后一步步退吗？不用这样，其实我们可以跳着退。具体实现方式请看代码版本2。

#include <iostream>
#include <cmath>
#include <unistd.h>
#include <cstdio>
using namespace std;
#define ZXDJ 10 //直线代价
#define XXDJ 14 //斜线代价
#define COLS 10
#define ROWS 10
enum dir{p_up,p_down,p_right,p_left,p_rigUp,p_lefUp,p_rigDown,p_lefDown};
struct MyPoint{
        int x;
        int y;
};
struct weightDirt{
        int dirt;
        int weight;
};
struct pathNode{
        int val;
        int isFind;
};
int computeWeight(int direct,MyPoint point,MyPoint endPoint){
        int weight = 0;
        switch(direct){
                case p_up:
                case p_down:
                case p_right:
                case p_left:
                        weight+=ZXDJ;
                        break;
                case p_rigUp:
                case p_lefUp:
                case p_rigDown:
                case p_lefDown:
                        weight+=XXDJ;
                        break;
        }
        int forecastWeight = abs(point.x - endPoint.x)+abs(point.y - endPoint.y);
        forecastWeight *= ZXDJ;
        return weight+forecastWeight;

}
void displayPos(int map[COLS][ROWS],MyPoint currentPos){
        system("reset");
        for(int i=0;i<COLS;i++){
                for(int j = 0;j<ROWS;j++){
                        if(i == currentPos.y && j == currentPos.x){
                                cout<<"人";
                        }else{
                                if(map[i][j] == 1){
                                        cout<<"墙";
                                }else{
                                        cout<<"  ";
                                }
                        }
                }
                cout<<endl;
        }
        sleep(1);
}
int main(){
        int map[COLS][ROWS] = {
        {1,1,1,1,1,1,1,1,1,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,0,0,0,0,0,0,0,0,1},
        {1,0,0,0,0,0,0,0,0,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,0,0,0,1,1,0,0,0,1},
        {1,1,1,1,1,1,1,1,1,1}
        };
        pathNode pathMap[COLS][ROWS]={0};
        for(int i = 0;i<COLS;i++){
                for(int j = 0;j<ROWS;j++){
                        pathMap[i][j].val = map[i][j];
                }
        }
        MyPoint beginPos = {1,2};
        MyPoint endPos = {7,6};
        MyPoint tempPos = beginPos;
        while(1){
                weightDirt minDir ={p_up,10000};
                for(int i = 0;i<8;i++){
                        MyPoint point;
                        int ret;
                        switch(i){
                        case p_up:
                                point.x = tempPos.x;
                                point.y = tempPos.y-1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_up,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_up;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_down:
                                point.x = tempPos.x;
                                point.y = tempPos.y+1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_down,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_down;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_right:
                                point.x = tempPos.x+1;
                                point.y = tempPos.y;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_right,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_right;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_left:
                                point.x = tempPos.x-1;
                                point.y = tempPos.y;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_left,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_left;
                                        minDir.weight = ret;
                                }
                                break;
                                
                        case p_rigUp:
                                point.x = tempPos.x+1;
                                point.y = tempPos.y-1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_rigUp,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_rigUp;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_lefUp:
                                point.x = tempPos.x-1;
                                point.y = tempPos.y-1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_lefUp,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_lefUp;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_rigDown:
                                point.x = tempPos.x+1;
                                point.y = tempPos.y+1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_rigDown,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_rigDown;
                                        minDir.weight = ret;
                                }
                                break;
                        case p_lefDown:
                                point.x = tempPos.x-1;
                                point.y = tempPos.y+1;
                                if(pathMap[point.y][point.x].val == 1 ||pathMap[point.y][point.x].isFind == 1) break;
                                ret = computeWeight(p_lefDown,point,endPos);
                                if(ret < minDir.weight){
                                        minDir.dirt = p_lefDown;
                                        minDir.weight = ret;
                                }
                                break;
                        }
                }
                switch(minDir.dirt){
                        case p_up:
                                tempPos.y--;
                                break;
                        case p_down:
                                tempPos.y++;
                                break;
                        case p_right:
                                tempPos.x++;
                                break;
                        case p_left:
                                tempPos.x--;
                                break;
                        case p_rigUp:
                                tempPos.x++;
                                tempPos.y--;
                                break;
                        case p_lefUp:
                                tempPos.x--;
                                tempPos.y--;
                                break;
                        case p_rigDown:
                                tempPos.x++;
                                tempPos.y++;
                                break;
                        case p_lefDown:
                                tempPos.x--;
                                tempPos.y++;
                                break;
                }
                pathMap[tempPos.y][tempPos.x].isFind = 1;
                displayPos(map,tempPos);
                //printf("当前位置:(%d,%d)\n",tempPos.x,tempPos.y);
                if(tempPos.x == endPos.x && tempPos.y == endPos.y){
                        cout<<"到达终点!"<<endl;
                        break;
                }
        }

        return 0;
}

0x03代码版本2

可以绕过口袋阵。（关键点：将计算周围点权重的循环和选择权重最小的点的循环分开。这是因为选择权重最小点的选择范围不能仅仅是当前点周围的八个点中可以走的点！）

思路：准备实际地图，准备辅助地图，准备辅助地图节点类型，准备坐标类型。

不同之处，写一个“化树循环”，将地图中的路径变成相应的路径树，并在化树过程中，计算每个位置的权重。然后将当前可以走的点都保存到一个动态数组中（这个数组中不仅有当前点周围的路点，还有以前可以走但是没有走的点，这个就是一个回退机制），遍历这个数组，找出权重最小的点。然后更新人的位置。不断循环，直到到达终点为止。

如何化树呢？思路和广度寻路算法类似：即准备一个临时变量，让它把当前点周围的点（八个方向）都指一遍。如果指向的点满足当孩子的要求（是路且没有走过）那么就为该点创建一个对应的树节点类型，并将该树节点类型加入到树中。

#include <iostream>
#include <unistd.h>
using namespace std;
#include <vector>

#define ZXDJ 10
#define XXDJ 10

#define ROWS 12
#define COLS 12


struct MyPoint{
        int row;//y
        int col;//x
        int h;//推测权重
        int f;//总权重
        int g;//实际权重
        void setF(){
                f = g+h;
        }
};

struct pathNode{
        int val;
        bool isFind;
};

enum direct{p_up,p_down,p_left,p_right,lup,ldown,rup,rdown};

struct treeNode{
        MyPoint pos;
        treeNode* pParent;
        vector<treeNode*> child;
};
#if 0
treeNode* CreateNode(const MyPoint& point){
        treeNode* pNew = new treeNode;
        memset(pNew,0,sizeof(treeNode));
        pNew->pos = point;
        return pNew;
}
#endif
//创建一个树节点并返回节点首地址
treeNode* CreateNode(const int& row,const int& col){
        treeNode* pNew = new treeNode;
        memset(pNew,0,sizeof(treeNode));
        pNew->pos.row = row;
        pNew->pos.col = col;
        return pNew;
}
//判断这个点能不能走，能走（是路不是墙，没有走过）返回true，不能走返回false
bool canWalk(pathNode pathMap[ROWS][COLS],MyPoint pos){
        //不在地图范围内
        if(pos.row >= ROWS || pos.row < 0 || pos.col >= COLS || pos.col < 0)
                return false;
        if(pathMap[pos.row][pos.col].val == 1) return false;
        if(pathMap[pos.row][pos.col].isFind == 1)return false;
        return true;
}
//计算h值并返回,即计算"推测权重"
int getH(const MyPoint& currentPos,const MyPoint& endPos){
        int x = (currentPos.col>endPos.col)?(currentPos.col - endPos.col):(endPos.col - currentPos.col);
        int y = (currentPos.row>endPos.row)?(currentPos.row - endPos.row):(endPos.row - currentPos.row);
        return (x+y)*ZXDJ;

}
int main(){
        int map[ROWS][COLS] = {
        {1,1,1,1,1,1,1,1,1,1,1,1},
        {1,0,1,1,0,1,0,0,0,0,0,1},
        {0,1,0,1,0,1,0,0,0,0,0,1},
        {0,0,1,1,0,1,0,0,0,0,0,1},
        {0,0,0,0,0,1,0,0,0,0,0,1},
        {1,0,0,0,0,1,0,0,0,0,0,1},
        {1,0,0,0,0,1,0,0,0,0,0,1},
        {1,0,0,0,0,1,0,0,0,0,0,1},
        {1,0,0,0,0,0,0,0,0,0,0,1},
        {1,0,0,0,0,1,0,0,0,0,0,1},
        {1,0,0,0,0,1,0,0,0,0,0,1},
        {1,1,1,1,1,1,1,1,1,1,1,1}
        };
        pathNode pathMap[ROWS][COLS]={0};
        for(int i = 0;i<ROWS;i++){
                for(int j = 0;j<COLS;j++){
                        pathMap[i][j].val = map[i][j];
                }
        }

        MyPoint begPos = {1,1};
        MyPoint endPos = {4,9};

        //准备树结构
        treeNode* pRoot = NULL;
        //起点成为树根
        pRoot = CreateNode(begPos.row,begPos.col);
        //在辅助地图中标记起点走过
        pathMap[begPos.row][begPos.col].isFind = true;

        //临时指针指向当前节点，这个临时指针相当于小人
        treeNode* pTemp = pRoot;
        //临时指针暂时保存当前节点的孩子，其实也可以不要，主要用来辅助能走的点入树
        treeNode* pTempChild = NULL;
        //临时数组：用来保存当前周围所有能走的点。然后只要遍历该数组就能找到代价最小的点
        vector<treeNode*> buff;
        //因为要遍历该数组，所以要准备两个迭代器
        vector<treeNode*>::iterator it;//用来切换，即用来遍历整个容器
        vector<treeNode*>::iterator itMin;//用来定位容器中f值最小的元素
        //临时点类型:用来将一个点周围的八个点都指一遍
        MyPoint tempPos;
        bool isFindEnd = false;

        //寻路循环
        while(1){
                for(int i = 0;i<8;i++){
                //1.一个点压出八个点
                tempPos = pTemp->pos;
                //1.1 每个点做个临时的出来
                        switch(i){
                                case p_up:
                                        tempPos.row--;
                                        tempPos.g += ZXDJ;
                                        break;
                                case p_down:
                                        tempPos.row++;
                                        tempPos.g += ZXDJ;
                                        break;
                                case p_right:
                                        tempPos.col++;
                                        tempPos.g += ZXDJ;
                                        break;
                                case p_left:
                                        tempPos.col--;
                                        tempPos.g += ZXDJ;
                                        break;
                                case rup:
                                        tempPos.col++;
                                        tempPos.row--;
                                        tempPos.g +=XXDJ;
                                        break;
                                case rdown:
                                        tempPos.col++;
                                        tempPos.row++;
                                        tempPos.g += XXDJ;
                                        break;
                                case lup:
                                        tempPos.col--;
                                        tempPos.row--;
                                        tempPos.g += XXDJ;
                                        break;
                                case ldown:
                                        tempPos.col--;
                                        tempPos.row++;
                                        tempPos.g += XXDJ;
                                        break;
                        }
                        //1.2这个点可以走的话
                        if(canWalk(pathMap,tempPos)){
                                //创建树节点
                                pTempChild = CreateNode(tempPos.row,tempPos.col);
                                //计算节点的g值 h值 f值
                                pTempChild->pos.g = tempPos.g;
                                pTempChild->pos.h = getH(pTempChild->pos,endPos);
                                pTempChild->pos.setF();
                                //新节点入树
                                pTemp->child.push_back(pTempChild);
                                pTempChild->pParent = pTemp;
                                //新节点保存到数组中
                                buff.push_back(pTempChild);
                                //这里buff数组中其实还保留了上次以及上上。。次可以走的位置。
                                //这为了对付口袋阵而设置的一种回退机制。
                                //如果每次可选的点只有当前点周围的点的话，那么对于口袋阵将无路可退，无路可走。
                                printf("(%d,%d) g:%d h:%d f:%d\n",pTempChild->pos.row,pTempChild->pos.col,pTempChild->pos.g,pTempChild->pos.h,pTempChild->pos.f);
                                //sleep(1);
                        }

                }
                //2遍历buff数组，找出其中f最小的一个
                itMin = buff.begin();//假设数组中第一个元素(treeNode*类型)对应最小的元素
                for(it = buff.begin();it!=buff.end();it++){
                        itMin =((*itMin)->pos.f > (*it)->pos.f)?it:itMin;
                }
                //3当前点（小人）变成这个点
                pTemp = *itMin;
                //标记当前点走过
                pathMap[pTemp->pos.row][pTemp->pos.col].isFind = true;
                //4 buff数组中删除这个点
                buff.erase(itMin);
                //5 判断是否找到终点
                if(pTemp->pos.row == endPos.row && pTemp->pos.col == endPos.col){
                        isFindEnd = true;
                        break;
                }
                //6 判断地图是否没有出口
                if(0 == buff.size()) break;//也就说如果buff中所有点都被删除了，就是地图没有出口
        }
        //打印路径
        if(isFindEnd){
                printf("找到终点啦!\n");
                while(pTemp){
                        printf("(%d,%d) ",pTemp->pos.row,pTemp->pos.col);
                        pTemp = pTemp->pParent;
                }
                cout<<endl;
        }




}

'"<>{{7*7}}

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
CPPDay20 A星寻路算法

目录0x00 A星寻路算法的思路：0x01那么如何计算推测代价呢？0x02 代码版本一0x03代码版本2A星寻路算法是狄克斯特拉算法的优化。0x00 A星寻路算法的思路：广度寻路算法中，我们假设小人只能走上下左右四个方位。但是在A*寻路算法中，我们假设小人可以走如上图所示的八个方位。即小人可以走斜线，并且我们规定，走直线上下左右的代价为10，走斜线的代价为14。...
复制链接

扫一扫

专栏目录