A star.h

首先放完整版代码：

#include <vector>

#include <list>

using namespace std;

const int kCost1 = 10; //直移一格消耗

const int kCost2 = 14; //斜移一格消耗

struct Point

{

int x, y; //点坐标，这里为了方便按照C++的数组来计算，x代表横排，y代表竖列

int F, G, H; //F=G+H

Point *parent; //parent的坐标，这里没有用指针，从而简化代码

/*Point(int _x, int _y) :x(_x), y(_y), F(0), G(0), H(0), parent(NULL)

{

}*/

Point(int _x, int _y)//变量初始化

{

x = _x;

y = _y;

F = 0;

G = 0;

H = 0;

parent = NULL;

}

};

class Astar

{

public:

void InitAstar(vector<vector<int>> &_maze);

list<Point *> GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);

private:

Point *findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner);

vector<Point *> getSurroundPoints(const Point *point, bool isIgnoreCorner) const;

bool isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const; //判断某点是否可以用于下一步判断

Point *isInList(const list<Point *> &list, const Point *point) const; //判断开启/关闭列表中是否包含某点

Point *getLeastFpoint(); //从开启列表中返回F值最小的节点

//计算FGH值

int calcG(Point *temp_start, Point *point);

int calcH(Point *point, Point *end);

int calcF(Point *point);

private:

vector<std::vector<int>> maze;

list<Point *> openList; //开启列表

list<Point *> closeList; //关闭列表

};

void Astar::InitAstar(std::vector<std::vector<int>> &_maze)

{

maze = _maze;

}

int Astar::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 ? 0 : point->parent->G; //如果是初始节点，则其父节点是空

return parentG + extraG;

}

int Astar::calcH(Point *point, Point *end)

{

//用简单的欧几里得距离计算H，这个H的计算是关键，还有很多算法，没深入研究^_^

//return sqrt((double)(end->x - point->x)*(double)(end->x - point->x) + (double)(end->y - point->y)*(double)(end->y - point->y))*kCost1;

return ((end->x - point->x) + end->y - point->y)*kCost1;

}

int Astar::calcF(Point *point)

{

return point->G + point->H;

}

Point *Astar::getLeastFpoint()

{

if (!openList.empty())

{

auto resPoint = openList.front();

for (auto &point : openList)

if (point->F<resPoint->F)

resPoint = point;

return resPoint;

}

return NULL;

}

Point *Astar::findPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)

{

openList.push_back(new Point(startPoint.x, startPoint.y)); //置入起点,拷贝开辟一个节点，内外隔离

while (!openList.empty())

{

auto curPoint = getLeastFpoint(); //找到F值最小的点

openList.remove(curPoint); //从开启列表中删除

closeList.push_back(curPoint); //放到关闭列表

//1,找到当前周围八个格中可以通过的格子

auto surroundPoints = getSurroundPoints(curPoint, isIgnoreCorner);

for (auto &target : surroundPoints)

{

//2,对某一个格子，如果它不在开启列表中，加入到开启列表，设置当前格为其父节点，计算F G H

if (!isInList(openList, target))

{

target->parent = curPoint;

target->G = calcG(curPoint, target);

target->H = calcH(target, &endPoint);

target->F = calcF(target);

openList.push_back(target);

}

//3，对某一个格子，它在开启列表中，计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F

else

{

int tempG = calcG(curPoint, target);

if (tempG<target->G)

{

target->parent = curPoint;

target->G = tempG;

target->F = calcF(target);

}

}

Point *resPoint = isInList(openList, &endPoint);

if (resPoint)

return resPoint; //返回列表里的节点指针，不要用原来传入的endpoint指针，因为发生了深拷贝

}

}

return NULL;

}

std::list<Point *> Astar::GetPath(Point &startPoint, Point &endPoint, bool isIgnoreCorner)

{

Point *result = findPath(startPoint, endPoint, isIgnoreCorner);

list<Point *> path;

//返回路径，如果没找到路径，返回空链表

while (result)

{

path.push_front(result);

result = result->parent;

}

// 清空临时开闭列表，防止重复执行GetPath导致结果异常

openList.clear();

closeList.clear();

return path;

}

Point *Astar::isInList(const std::list<Point *> &list, const Point *point) const

{

//判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标

for (auto p : list)

if (p->x == point->x&&p->y == point->y)

return p;

return NULL;

}

bool Astar::isCanreach(const Point *point, const Point *target, bool isIgnoreCorner) const

{

if (target->x<0 || target->x>maze.size() - 1

|| target->y<0 || target->y>maze[0].size() - 1

|| maze[target->x][target->y] == 1

|| target->x == point->x&&target->y == point->y

|| isInList(closeList, target)) //如果点与当前节点重合、超出地图、是障碍物、或者在关闭列表中，返回false

return false;

else

{

if (abs(point->x - target->x) + abs(point->y - target->y) == 1) //非斜角可以

return true;

else

{

//斜对角要判断是否绊住

if (maze[point->x][target->y] == 0 && maze[target->x][point->y] == 0)

return true;

else

return isIgnoreCorner;

}

}

}

vector<Point *> Astar::getSurroundPoints(const Point *point, bool isIgnoreCorner) const

{

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++)

if (isCanreach(point, new Point(x, y), isIgnoreCorner))

surroundPoints.push_back(new Point(x, y));

return surroundPoints;

}

测试 main.cpp

#include <math.h>

#include<iostream>

#include "A star .h"

using namespace std;

int main()

{

//初始化地图，用二维矩阵代表地图，1表示障碍物，0表示可通

vector<vector<int>> maze = {

{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },

{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1 },

{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1 },

{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1 },

{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1 },

{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 },

{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1 },

{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }

};

Astar astar;

astar.InitAstar(maze);

//设置起始和结束点

Point start(1, 1);

Point end(6, 10);

//A*算法找寻路径

list<Point *> path = astar.GetPath(start, end, false);

//打印

for (auto &p : path)

cout << '(' << p->x << ',' << p->y << ')' << endl;

system("pause");

return 0;

}

测试结果

检验

附：

http://qiao.github.io/PathFinding.js/visual​qiao.github.io

讲解

首先定义一个结构体：

这个结构体就表示一个栅格，其中包括坐标（x，y），每一个栅格都有的FGH，G表示从起点到当前节点的路程，H表示当前节点到终点的预估。还包括父节点，也是Point类型的。在结构体里面对其初始化。

接下来定义一个类：

类中声明了10种函数，以及3个变量，意义见注释。

第一种函数InitAstar：

这个函数把地形矩阵传给Astar类中的maze二维向量，用于导航。

第二种函数用来计算从temp_start作为父节点这条路走过来的G值：

第三种函数计算H，这里我用的是曼哈顿距离，也可以用欧式距离：

第四种计算节点的F：

接下来的这个函数用于搜索openlist中F值最小的节点：

第六种函数用来判断某一个节点是否处于openlist或者closelist中，返会list中的这个重复的节点，没有重复的就返回空节点：

第七种函数用来看目标节点是否可以作为下一个搜索的中心点：

首先，目标节点若是不在地图中，或是目标节点是一个障碍物，或是目标点与当前节点重合，亦或是目标点已经在close链表中（考察过了），都表示这个点不在考察范围内。非上述情况，看目标点与当前点是否是斜角，如果不是，是否考虑corner都可以考察。这里说一下考虑corner的意义，如果考虑corner，那么斜着走时候两边不能有障碍物。如果两边都是0（没有障碍物），那么就可以考察，否则就看是否考虑corner，考虑就不能走这条路。

第八种函数用来把周围可选的点提取出来：

原理很简单：遍历周围8个周围点加上自身，把可考察的点放入一个向量中返回。

接下来就进行寻路函数：

流程就是：先把起点放入openlist中，然后每次在open list中找最小F值，把它从openlist拿到closelist中，表示这个节点已经考察过了，然后对这个点周围的点进行考察，不在openlist，就先计算FGH值后加进去；如果已经在了，就更新FGH，更新父节点。一直更新下去，直到终点出现在openlist中就找到了。这时返回找到的点，否则返回空节点。

接下来把找到的路径存在一个链表中：

从终点开始，每次把它的父节点插入链表头，直到父节点为空，这样我们就得到了路径坐标。