c++實現A*算法

A星尋路算法的講解有很多，這裏不再論述，只給出實現程序。

AStar.h

#pragma once

#include <vector>
#include <map>
#include <queue>
// 二維地圖A*算法實現

const int OBLIQUE = 14;  // 斜線移動權重爲14
const int STEP = 10;
struct Point
{
	Point(int id, int reachable);
	~Point();

	void CalcF() { m_F = m_G + m_H; }
	bool IsReachable()
	{
		if (m_reachable == 0) return true;
		return false;
	}
	bool operator>(const Point* point) // 優先隊列小頂堆
	{
		if (m_F > point->m_F)
		{
			return true;
		}
		return false;
	}

	int m_F{ 0 };  // F = G + H
	int m_G{ 0 };  // G 表示從起點 A 移動到網格上指定方格的移動耗費 (可沿斜方向移動)
	int m_H{ 0 };  // H 表示從指定的方格移動到終點 B 的預計耗費 (H 有很多計算方法, 這裏我們設定只可以上下左右移動)
	int m_id; // 二維數組轉換成一維數組的位置編號 0, 1, 2,....
	int m_reachable;  // 0 可達 1 不可達
	Point* m_parent{nullptr};  // 保存父節點
	int m_close{0};  // 0 未加入closelist 1 加入closelist
	int m_x{0};
	int m_y{0};
};


//--------------
//|0 1 3 4 5
//|6 7 8 9 10
//|11 12 13 14 15
//|....
typedef std::vector<Point*> point_vec_type;
typedef std::map<int, Point*> point_map_type;
typedef std::priority_queue<Point*, point_vec_type> point_queue_type;
class AStarNav
{
public:
	AStarNav();
	~AStarNav();

	bool FindPath(int start_id, int end_id, std::vector<Point*>& find_path);
	bool CanReach(int start_id, int end_id);

	bool LoadMap(const char* path);
	int GetPointId(int i, int j);
private:
	void GetAroundPoints(int point_id, point_vec_type& point_set);
	void GetPos(int& i, int& j, int point_id);
	bool StopSearch(int target_id);
	Point* GetPoint(int point_id);
	Point* GetMinFInOpenList();
	template <typename T>
	bool IsInList(const T& t, Point* point);
	void InOpenList(Point* point);
	void OutOpenList(Point* point);
	int CalcG(const Point* start, const Point* point);
	int CalcH(const Point* end, const Point* point);
private:
	point_map_type m_close_list;  // 檢查完畢列表
	point_map_type m_open_list;   // 待檢查列表
	point_queue_type m_open_queue; // 查找最小值的輔助結構
	point_map_type m_points; // 地圖所以格子
	int m_map_width{ 0 };
	int m_map_hight{ 0 };
};

Astar.cpp

#include <fstream>
#include <string>
#include <algorithm>
#include "AStarNav.h"
#include "lexical_cast.hpp"
#include "StringUtil.h"


Point::Point(int id, int reachable):m_id(id),m_reachable(reachable)
{
}

Point::~Point()
{
}

AStarNav::AStarNav()
{

}

AStarNav::~AStarNav()
{
	for (auto it : m_points)
	{
		delete it.second;
		it.second = nullptr;
	}
	m_close_list.clear();
	m_open_list.clear();
	m_points.clear();
}

bool AStarNav::LoadMap(const char* path)
{
	std::ifstream ifs(path);
	if (!ifs) return false;

	std::string line;
	int point_id = 0;
	while (!ifs.eof())
	{
		ifs >> line;
		std::vector<std::string>  vec_nodes;
		StringUtil::SplitCpp(line, ",", vec_nodes);
		if (m_map_width == 0)
		{
			m_map_width = (int)vec_nodes.size();
		}
		for (auto& it : vec_nodes)
		{
			int k = lexical_cast<int>(it);
			Point* point = new Point(point_id, k);
			point->m_x = point_id % m_map_width;
			point->m_y = m_map_hight;
			m_points.emplace(point_id, point);
			point_id++;
		}
		m_map_hight++;
	}
	ifs.close();
	return true;
}

void AStarNav::GetPos(int& i, int& j, int point_id)
{
	i = point_id % m_map_width;
	j = point_id / m_map_hight;
}

int AStarNav::GetPointId(int i, int j)
{
	int point_id = j * m_map_width + i;
	return point_id;
}

void AStarNav::GetAroundPoints(int point_id, point_vec_type& point_vec)
{
	int i = 0, j = 0;
	GetPos(i, j, point_id);
	Point* point = nullptr;

	for(int x = i-1; x<=i+1; x++)
		for (int y = j - 1; y <= j + 1; y++)
		{
			if (x >= m_map_width || x < 0) continue;
			if (y >= m_map_hight || y < 0) continue;
			Point* point = GetPoint(GetPointId(x, y));
			if (point && point->IsReachable() && point->m_id != point_id)
			{
				point_vec.emplace_back(point);
			}
		}
}

bool AStarNav::FindPath(int start_id, int end_id, std::vector<Point*>& find_path)
{
	if (CanReach(start_id, end_id))
	{
		Point* start = GetPoint(start_id);
		Point* end = GetPoint(end_id);
		Point* point = end;
		while (point->m_parent)
		{
			find_path.emplace_back(point);
			point = point->m_parent;
		}
		find_path.emplace_back(start);  // 路徑包含了開始結束位置
		return true;
	}
	return false;
}

bool AStarNav::CanReach(int start_id, int end_id)
{
	m_open_list.clear();
	m_close_list.clear();
	Point* start = GetPoint(start_id);
	Point* end = GetPoint(end_id);
	if (start == nullptr || end == nullptr)
		return false;
	InOpenList(start);
	point_vec_type around_points;
	while (m_open_list.size() > 0)
	{
		Point* tmp_start = GetMinFInOpenList();
		m_close_list.emplace(tmp_start->m_id, tmp_start);
		OutOpenList(tmp_start);

		around_points.clear();
		GetAroundPoints(tmp_start->m_id, around_points);
		for (auto point : around_points)
		{
			if (!point->IsReachable() || IsInList<point_map_type>(m_close_list, point))
				continue;
			if (!IsInList<point_map_type>(m_open_list, point))
			{
				point->m_parent = tmp_start;
				point->m_G = CalcG(tmp_start, point);
				point->m_H = CalcH(end, point);
				m_open_list.emplace(point->m_id,point);
				m_open_queue.push(point);
			}
			else
			{
				int G = CalcG(tmp_start, point);
				if (G < point->m_G)
				{
					point->m_parent = tmp_start;
					point->m_G = G;
					point->CalcF();
				}
			}
		}
		if (IsInList<point_map_type>(m_open_list, end))
		{
			return true;
		}
	}
	return false;
}


Point* AStarNav::GetMinFInOpenList()
{
	return m_open_queue.top();
}

// 目標格已經在 "開啓列表", 這時候路徑被找到
bool AStarNav::StopSearch(int target_id)
{
	auto it = m_open_list.find(target_id);
	if (it != m_open_list.end())
	{
		return true;
	}
	return false;
}
void AStarNav::InOpenList(Point* point)
{
	m_open_list.emplace(point->m_id, point);
	m_open_queue.push(point);
}
void AStarNav::OutOpenList(Point* point)
{
	m_open_queue.pop();
	m_open_list.erase(point->m_id);
}

Point* AStarNav::GetPoint(int point_id)
{
	auto it = m_points.find(point_id);
	if (it == m_points.end())
	{
		return nullptr;
	}
	return it->second;
}

template <typename T>
bool AStarNav::IsInList(const T& t, Point* point)
{
	auto it = t.find(point->m_id);
	if (it != t.end())
		return true;
	return false;
}

int AStarNav::CalcG(const Point* start, const Point* point)
{ 
	int G = (abs(point->m_x - start->m_x) + abs(point->m_y - start->m_y)) == 2 ? STEP : OBLIQUE;
	int parentG = point->m_parent != nullptr ? point->m_parent->m_G : 0;
	return G + parentG;
}

int AStarNav::CalcH(const Point* end, const Point* point)
{
	int step = abs(point->m_x - end->m_x) + abs(point->m_y - end->m_y);
	return step * STEP;
}