【数据结构与算法】A*算法——自动寻路

大家好，欢迎来到IT知识分享网。

一.为什么用A*算法

二.A*算法的实现原理

三.A*算法的实现

头文件,我们先来看看需要做哪些事?

1.初始化地图

2.格子初始化

3.两个列表

4.起点到终点的路径

5.起点到终点的最佳路径★

如果越界了不可以,因为是一个二维数组,如果是障碍物不可以,这里我们在二维数组中设置的值为1的是障碍物.,如果在closeList的也不可以,因为我们已经走过了.
同时我们只需要上下左右就可以了.

判断某个格子是否在某个列表.

这个时候我们就得到了周围可走通格子的容器了,我们进行遍历计算其F值.

对于周围每个可以走通的格子我们进行判断是否在openList里面,如果没有就加入,有的话,就判断需不需要进行更新.判断的依据就是G是否更小,因为H都是一样的.

距离起点的步数计算.就是当前各个到下一步的步数+其父亲节点的G步数.

H(点到终点的距离)计算用了勾股定理.
如2~16的距离.

F的计算自不必多说.

6.资源的释放

四.完整代码

1.Astar.h

#pragma once #include <list> const int kCost1 = 10; const int kCost2 = 14; typedef struct _Point { 
    int x,y;//二维数组下标 int F, G, H;//F=G+H struct _Point* parent;//parent的坐标 }Point; //分配一个节点(格子) Point* AllocPoint(int x, int y); //初始化地图 void initAstarMaze(int* _maze, int _lines, int _colims); //通过A*算法寻找路径 std::list<Point*>GetPath(Point* statPoint, Point* endPoint); //清理资源,结束后必须调用 void ClearAstarMaze(); 

2.Astar.cpp

#include <math.h> #include "Astar.h" #include <iostream> #include <vector> static int* maze;//迷宫对应的二维数组,使用一级指针表示 static int cols;//二维数组对于的列数 static int lines;//二维数组对于的行数 static std::list<Point*>openList;//开放列表 static std::list<Point*>closeList;//关闭列表 //搜索从起点到终点的最佳路径 static Point* findPath(Point* startPoint, Point* endPoint); //从开启列表中返回F值最小的节点 static Point* getLeasFpoint(); //获取当前点周围可达的节点 static std::vector<Point*>getSurroundPoints(const Point* point); //判断某点是否可以用于下一步判断 static bool isCanreach(const Point* point, const Point* target); //判断开放/关闭列表中是否包含某点 static Point* isInList(const std::list<Point*>& list, const Point* point); //计算FGH值 static int calcG(Point* temp_start, Point* point); static int calcH(Point* point, Point* end); static int calcF(Point* point); //分配一个节点(格子) Point* AllocPoint(int x, int y) { 
    Point* temp = new Point; memset(temp, 0, sizeof(Point));//初始值清零 temp->x = x; temp->y = y; return temp; } //初始化地图 void initAstarMaze(int* _maze, int _lines, int _colums) { 
    maze = _maze; lines = _lines; cols = _colums; } //通过A*算法寻找路径 std::list<Point*>GetPath(Point* startPoint, Point* endPoint) { 
    Point* result = findPath(startPoint, endPoint); std::list<Point*>path; //返回路径,如果没有找到路径,返回空链表 while (result) { 
    path.push_front(result); result = result->parent; } return path; } //搜索从起点到终点的最佳路径 static Point* findPath(Point* startPoint, Point* endPoint) { 
    openList.push_back(AllocPoint(startPoint->x,startPoint->y)); while (!openList.empty()) { 
    //第一步,从开放列表中取最小的值 Point* curPoint = getLeasFpoint();//找到F值最小的点 //第二步,把当前节点放到关闭列表中 openList.remove(curPoint); closeList.push_back(curPoint); //第三步,找到当前节点周围可达的节点 std::vector<Point*>surroundPoints = getSurroundPoints(curPoint); std::vector<Point*>::const_iterator iter; for (iter = surroundPoints.begin(); iter != surroundPoints.end(); iter++) { 
    Point* target = *iter; //对某个格子,如果不在开放列表中,就加入,设置当前格为其父节点. Point* exist = isInList(openList, target); if (!exist) { 
    target->parent = curPoint; target->G = calcG(curPoint, target); target->H = calcH(target, endPoint); target->F = calcF(target); openList.push_back(target); } else { 
    int tempG = calcG(curPoint, target); if (tempG < target->G) { 
    exist->parent = curPoint; exist->G = tempG; exist->F = calcF(target); } delete target;//如果已经存在了就不用了释放. } } surroundPoints.clear(); Point* resPoint = isInList(openList, endPoint); if (resPoint) { 
    return resPoint; } } return NULL; } //从开启列表中返回F值最小的节点 static Point* getLeasFpoint() { 
    if (!openList.empty()) { 
    Point* resPoint = openList.front(); std::list<Point*>::const_iterator itor; for (itor = openList.begin(); itor != openList.end(); itor++) { 
    if ((*itor)->F < resPoint->F) { 
    resPoint = *itor; } } return resPoint; } return NULL; } //获取当前点周围可达的节点 static std::vector<Point*>getSurroundPoints(const Point* point) { 
    std::vector<Point*>surroundPoints; for (int x = point->x - 1; x <= point->x + 1; x++) { 
    for (int y = point->y - 1; y <= point->y + 1; y++) { 
    Point* temp = AllocPoint(x, y); if (isCanreach(point, temp)) { 
    surroundPoints.push_back(temp); } else { 
    delete temp; } } } return surroundPoints; } static bool isCanreach(const Point* point, const Point* target) { 
    if (target->x<0 || target->x>(lines - 1) || target->y<0 || target->y>(cols - 1) || maze[target->x * cols + target->y] == 1 || (target->x == point->x && target->y == point->y) || isInList(closeList, target)) { 
    return false; } if (abs(point->x - target->x) + abs(point->y - target->y) == 1)//abs绝对值.上下左右 { 
    return true; } else { 
    return false; } } //判断开启/关闭列表中是否包含某点 static Point* isInList(const std::list<Point*>& list, const Point* point) { 
    std::list<Point*>::const_iterator itor; for (itor = list.begin(); itor != list.end(); itor++) { 
    if ((*itor)->x == point->x && (*itor)->y == point->y) { 
    return *itor; } } return NULL; } static int calcG(Point* temp_start, Point* point) { 
    int extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2; int parentG = (point->parent == NULL ? NULL : point->parent->G); return extraG + parentG; } static int calcH(Point* point, Point* end) { 
    return (int)sqrt((double)(end->x - point->x) * (double)(end->x - point->x) + (double)(end->y - point->y) * (double)(end->y - point->y)); } static int calcF(Point* point) { 
    return point->G + point->H; } //清理资源,结束后必须调用 void ClearAstarMaze() { 
    maze = NULL; lines = 0; cols = 0; std::list <Point*>::iterator itor; for (itor = openList.begin(); itor != openList.end();) { 
    delete* itor; itor = openList.erase(itor);//获取到下一个节点 } for (itor = closeList.begin(); itor != closeList.end();) { 
    delete* itor; itor = closeList.erase(itor);//获取到下一个节点 } } 

3.main.cpp

#include "Astar.h" #include <list> #include <iostream> #include <Windows.h> using namespace std; int map[5][8] = { 
    { 
   1,1,1,0,0,0,1,1}, { 
   0,0,0,0,1,0,0,0}, { 
   0,0,0,0,1,0,0,0}, { 
   0,0,0,0,1,0,0,0}, { 
   1,1,1,0,0,0,1,1}, }; void AStarTest() { 
    initAstarMaze(&map[0][0], 5, 8); //设置起始和结束点 Point* start = AllocPoint(2, 1); Point* end = AllocPoint(2, 6); //通过A*算法寻找路径 list<Point*>path=GetPath(start, end); cout << "寻路结果:" << endl; list<Point*>::const_iterator iter; for (iter = path.begin(); iter != path.end();iter++) { 
    Point* cur = *iter; cout << '(' << cur->x << ',' << cur->y << ')' << endl; Sleep(800); } ClearAstarMaze(); } int main() { 
    AStarTest(); system("pause"); return 0; } 

4.运行结果

就是走的这条路.