A星(A*,A star)寻路算法c++

A星(A*,A star)寻路算法

1. 整体思想是

对每个节点来说，有父节点,fcost,hcost,gcost,其中

• gcost是从起点到当前点的实际代价
• hcost是从当前点到终点的估计代价

gcost好理解，就是从起点到当前点的代价；hcost叫估计代价是因为在到底终点之前不知道实际代价是多少，我们只能估计，例如，不考虑影响到终点的阻碍，它会是多少。而$fcost=gcost+hcost$，整体寻路过程就是找到一条到终点的fcost最小的路径。

2. 整体流程是

1. 建一个空的容器open_list，保存还没访问的点；一个空的容器close_list,
   保存已经访问过的节点；起点进入open_list

2. 当open_list不为空，do
   2.1 找出当前open_list中$fcost$最小的节点(method)作为当前节点cur_grid
   2.2 cur_grid从open_list中移除
   2.3 cur_grid加入到close_list中
   2.4 找到cur_grid的所有邻近点(method,需要检查节点有效性<越界、已经在open_list中，已经在close_list中>)
   2.5 对cur_grid的所有邻近节点，更新他们的父节点、gcost、hcost、fcost(method)，并放入到open_list中；如果发现其中一个邻近的节点已经是终点，返回这个是终点的邻近节点，算法结束。

3. 否则open_list已经空，还没有找到终点，说明查找路径失败,算法结束

4. 如果程序从2结束，则从终点回溯父节点，显示路径。

3. 代码实现上

我觉得需要考虑的问题大概有：
3.1 如何表示节点
这个最基础，这个实现的好，那么后面节点的父节点、fcost、gcost、hcost的更新就会简单
3.2 如何表示open_list，close_list
3.3 如何组织整个框架，把各个模块联系起来

4.代码

下面就看看代码吧

AStar.h

#pragma once

#include
#include


class Grid 
{
public:
	// 坐标位置
	int x, y;
	// fcost,gcost,hcost
	double f, g, h;
	// 当前点的父节点
	std::shared_ptr<Grid> parent;
public:
	// 构造函数
	Grid(int x_, int y_);
	// 析构函数
	~Grid(){}

	// 更新grid
	void updateGrid(std::shared_ptr<Grid> parent, const std::shared_ptr<Grid> end);
};

class AStar
{
private:
	int mazeRow, mazeColum;
	std::vector<std::vector<int>> maze;

	void findMinGrid(std::vector<std::shared_ptr<Grid>>& openList,
					std::shared_ptr<Grid>& currentGrid);

	void findNeighbors(std::shared_ptr<Grid> grid,
		std::vector<std::shared_ptr<Grid>>& open_list,
		std::vector<std::shared_ptr<Grid>>& close_list,
		std::vector<std::shared_ptr<Grid>>& neighbors);

	bool isValidGrid(int x_, int y_,
		std::vector<std::shared_ptr<Grid>>& open_list,
		std::vector<std::shared_ptr<Grid>>& close_list);

	bool containGrid(const std::vector<std::shared_ptr<Grid>>& grids_, int x_, int y_);
public:
	AStar(const int maze_rows, const int maze_colums,
		const std::vector<std::vector<int>>& maze_) :mazeRow(maze_rows), mazeColum(maze_colums),maze(maze_) {}

	/// 

	/// A星寻路算法
	/// 
	/// 返回的结果格子
	/// 开始格子
	/// 终点格子
	void aStarSearch(std::shared_ptr<Grid>& re_end,
		std::shared_ptr<Grid> start,
		std::shared_ptr<Grid> end);

	/// 

	/// 由找到的终点回溯路径
	/// 
	/// 找到的终点
	/// 回溯到的路径
	void getPath(std::shared_ptr<Grid> re_grid, 
		std::vector<std::shared_ptr<Grid>>& path_);


	void printPath(const std::shared_ptr<Grid> start_,
				   const std::shared_ptr<Grid> end_,
				   const std::vector<std::shared_ptr<Grid>>& path_);
};

AStar.cpp

#include "AStar.h"
#include
#include

Grid::Grid(int x_, int y_):x(x_),y(y_)
{
	this->f = 0;
	this->h = 0;
	this->g = 0;
	this->parent = nullptr;
}

void Grid::updateGrid(std::shared_ptr<Grid> parent, const std::shared_ptr<Grid> end)
{
	this->parent = parent;
	this->g = parent->g + 1;
	this->h = std::abs(this->x - end->x) + std::abs(this->y - end->y);
	this->f = this->g + this->h;
}

void AStar::findMinGrid(
	std::vector<std::shared_ptr<Grid>>& openList, 
	std::shared_ptr<Grid>& currentGrid)
{
	currentGrid = openList[0];
	for (auto grid : openList) {
		if (grid->f < currentGrid->f) {
			currentGrid = grid;
		}
	}
}

void AStar::findNeighbors(std::shared_ptr<Grid> grid, 
						std::vector<std::shared_ptr<Grid>>& open_list, 
						std::vector<std::shared_ptr<Grid>>& close_list, 
						std::vector<std::shared_ptr<Grid>>& neighbors)
{
	if (this->isValidGrid(grid->x, grid->y - 1, open_list, close_list)) {
		neighbors.push_back(std::make_shared<Grid>(grid->x, grid->y - 1));
	}
	if (this->isValidGrid(grid->x, grid->y + 1, open_list, close_list)) {
		neighbors.push_back(std::make_shared<Grid>(grid->x, grid->y + 1));
	}
	if (this->isValidGrid(grid->x - 1, grid->y, open_list, close_list)) {
		neighbors.push_back(std::make_shared<Grid>(grid->x - 1, grid->y));
	}
	if (this->isValidGrid(grid->x + 1, grid->y, open_list, close_list)) {
		neighbors.push_back(std::make_shared<Grid>(grid->x + 1, grid->y));
	}
}

bool AStar::isValidGrid(int x_, int y_, 
					std::vector<std::shared_ptr<Grid>>& open_list, 
					std::vector<std::shared_ptr<Grid>>& close_list)
{
	// 是否越界
	if (x_ < 0 || x_ >= this->mazeRow || y_ < 0 || y_ >= this->mazeColum) {
		return false;
	}
	// 是否有障碍物
	if (this->maze[x_][y_] == 1) { return false; }
	// 是否已经在openList中
	if (this->containGrid(open_list, x_, y_)) { return false; }
	// 是否在closeList中
	if (this->containGrid(close_list, x_, y_)) { return false; }
	return true;
}

bool AStar::containGrid(const std::vector<std::shared_ptr<Grid>>& grids_, int x_, int y_)
{
	for (auto grid : grids_) {
		if (grid->x == x_ && grid->y == y_) {
			return true;
		}
	}
	return false;
}

void AStar::aStarSearch(std::shared_ptr<Grid>& re_end, std::shared_ptr<Grid> start, std::shared_ptr<Grid> end)
{
	// 待访问的格子
	std::vector<std::shared_ptr<Grid>> openList;
	// 已访问的格子
	std::vector<std::shared_ptr<Grid>> closeList;
	// 把起点加入到openList
	openList.push_back(start);

	/*
	主循环，每一轮检查一个当前方格节点
	*/
	while (openList.empty() == false) {
		// 在openList中查找f值最小的节点作为当前方格节点
		std::shared_ptr<Grid> currentGrid;
		this->findMinGrid(openList, currentGrid);
		auto it = std::find(openList.begin(), openList.end(), currentGrid);
		// 当前方格点从openList中移除
		it = openList.erase(it);
		// 当前方格点进入closeList
		closeList.push_back(currentGrid);
		// 找到当前方格点的所有邻近节点
		std::vector<std::shared_ptr<Grid>> neighbors;
		this->findNeighbors(currentGrid, openList, closeList, neighbors);
		for (auto grid : neighbors) {
			// 更新节点。标记父亲，更新g,h,f,并放入openList中
			grid->updateGrid(currentGrid, end);
			openList.push_back(grid);
			// 如果终点在openList中,直接返回终点格子
			if (grid->x == end->x && grid->y == end->y) {
				re_end = grid;
			}
		}
	}
}

void AStar::getPath(std::shared_ptr<Grid> re_grid, 
					std::vector<std::shared_ptr<Grid>>& path_)
{
	while (!(re_grid==nullptr)) {
		path_.push_back(std::make_shared<Grid>(re_grid->x, re_grid->y));
		re_grid = re_grid->parent;
	}
}

void AStar::printPath(const std::shared_ptr<Grid> start_,
	const std::shared_ptr<Grid> end_,
	const std::vector<std::shared_ptr<Grid>>& path_)
{
	for (int i = 0; i < this->maze.size(); i++) { // 行
		for (int j = 0; j < this->maze[0].size(); j++) { // 列
			if (this->containGrid(path_, i, j)) {
				if ((i == start_->x && j == start_->y) || (i == end_->x && j == end_->y))
				{
					std::cout << "$ ";
				}
				else { // 起点和终点用o表示
					std::cout << "* ";
				}
			}
			else {
				std::cout << this->maze[i][j] << " ";
			}
		}
		std::cout << std::endl;
	}
}

main.cpp

#include"AStar.h"
#include
using namespace std;

int main()
{
	const size_t rows = 6;
	const size_t colums = 7;
	vector<vector<int>> maze = {
		{0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 1, 0, 0, 0},
		{0, 0, 0, 0, 0, 0, 0}
	};
	/*for (auto& ar : maze) {
		for (auto item : ar) {
			cout << item << " ";
		}
		cout << endl;
	}*/
	// 起点
	shared_ptr<Grid> startGrid(new Grid(3,0));
	// 终点
	shared_ptr<Grid> endGrid(new Grid(3, 6));

	AStar astar(rows, colums, maze);
	shared_ptr<Grid> reGrid = nullptr;
	astar.aStarSearch(reGrid, startGrid, endGrid);
	
	// 如果找到了终点,返回迷宫路径
	if (!(reGrid == nullptr)) {
		vector<std::shared_ptr<Grid>> path;
		astar.getPath(reGrid, path);
		/*for (auto grid : path) {
			cout << grid->x << ", " << grid->y << endl;
		}*/
		cout << "steps: "<<path.size() << endl;
		// 显示迷宫路径，路径用*表示
		astar.printPath(startGrid,endGrid,path);
	}
	return 0;
}