C++ A*算法实现

参考:https://blog.csdn.net/zgwangbo/article/details/52078338

AStar.h

#ifndef __AStar__
#define __AStar__

#include <algorithm>
#include <vector>
#include <string>

struct Grid
{
	Grid() {}
	Grid(int x, int y) {
		this->x = x;
		this->y = y;
	}
	int x = 0;
	int y = 0;

	double distance(const Grid& grid) {
		double dis = sqrt(pow(grid.x - x, 2) + pow(grid.y - y, 2));
		return dis;
	}

	bool operator==(const Grid& grid) const{
		return this->x == grid.x && this->y == grid.y;
	}
};

class AStar
{
public:
	AStar();
	~AStar();

	struct MapData
	{
		Grid grid;
		bool visited = false;
		bool isSpace = false;
		std::string symbolStr;
		void reset() {
			grid.x = 0;
			grid.y = 0;
			visited = false;
			isSpace = false;
			symbolStr.clear();
		}
	};

	void findPath(const Grid & startPos,const Grid &endPos, std::vector<std::vector<MapData>> &arr);
};

#endif /* defined(__AStar__) */

AStar.cpp


#include "AStar.h"
#include <iostream>
#include <map>

struct Data
{
	Data() {}
	Data(Grid pos, double h, double g, Data* parent) {
		this->grid = pos;
		this->h = h;
		this->g = g;
		this->parent = parent;
	}

	Grid grid = Grid(0, 0);
	double h = 0;
	double g = 0;
	Data* parent = nullptr;

	double f() {
		return this->g + this->h;
	}
};

struct MinHeap
{
	std::vector<Data*> m_vec;
	std::map<std::string, Data*> m_map;
	int index = 0;

	Data* getMinAndRemove() {
		if (isEmpty()) {
			return nullptr;
		}
		sort();
		auto first = m_vec.at(0);
		auto last = m_vec.at(index - 1);
		m_vec[0] = last;

		--index;
		return first;
	}

	bool isEmpty() {
		return index <= 0;
	}

	std::string getKey(const Grid& grid) {
		char buff[32] = { 0 };
		sprintf(buff, "%d-%d", grid.x, grid.y);
		return buff;
	}

	Data* find(const Grid& grid) {
		auto it = m_map.find(getKey(grid));
		if (it != m_map.end())
		{
			return it->second;
		}
		return nullptr;
	}

	void add(Data* data) {
		if (index < m_vec.size())
		{
			m_vec[index] = data;
		}
		else
		{
			m_vec.push_back(data);
		}
		index = m_vec.size();
		m_map[getKey(data->grid)] = data;
	}

	void sort() {
		std::sort(m_vec.begin(), m_vec.end(), [](Data* a, Data* b) {return a->f() < b->f(); });
	}

	void release() {
		for (auto it = m_map.begin(); it != m_map.end();)
		{
			Data* tmp = it->second;
			it = m_map.erase(it);
			delete tmp;
		}
	}
};



AStar::AStar()
{}

AStar::~AStar()
{
}

void AStar::findPath(const Grid& startPos, const Grid& endPos, std::vector<std::vector<MapData>>& arr)
{
	//------可以拓展的四个方向
	int directs[][2] = { { 0, 1 }, { 1, 0 }, { 0, -1 }, { -1, 0 } };

	MinHeap heap;
	heap.add(new Data(startPos, 0, 0, nullptr));
	bool finish = false;
	Data* lastData = nullptr;// 记录最后一个点的数据,用来反推路径 若为空则无路径可到达

	int max_l = 0;
	while (!finish && !heap.isEmpty())
	{
		Data* data = heap.getMinAndRemove();// 取出f值最小的点
		if (arr[data->grid.x][data->grid.y].isSpace)
		{
			arr[data->grid.x][data->grid.y].visited = true;// 将取出的点标识为已访问点
			arr[data->grid.x][data->grid.y].symbolStr = "_";
		}

		max_l = arr[data->grid.x].size();
		for (auto dir : directs)// 遍历四个方向的点
		{
			Grid pos = Grid(data->grid.x + dir[0], data->grid.y + dir[1]);
			if (pos.x > 0 && pos.x < arr.size()
				&& pos.y>0 && pos.y < max_l)
			{
				auto &c = arr[pos.x][pos.y];
				if (endPos==pos)// 如果是终点,则跳出循环,不用再找
				{
					finish = true;
					lastData = data;
					break;
				}
				if (!c.isSpace)// 如果不是空地,就不需要再扩展
				{
					continue;
				}

				auto nn = heap.find(pos);
				if (nn)
				{
					if (nn->g > data->g + 1)
					{
						nn->g = data->g + 1;
						nn->parent = data;
					}
				}
				else
				{
					heap.add(new Data(pos, pos.distance(endPos), data->g + 1, data));
				}
			}
		}
	}

	if (lastData)
	{
		// 反向找出路径
		while (lastData->parent)
		{
			arr[lastData->grid.x][lastData->grid.y].symbolStr = 'O';
			lastData = lastData->parent;
		}

		for (int i = 0; i < arr.size(); ++i)
		{
			for (int j = 0; j < max_l; ++j)
			{
				std::cout << arr[i][j].symbolStr;
			}
			std::cout << std::endl;
		}
	}
	else
	{
		std::cout << "no path" << std::endl;
	}
	heap.release();
}

 测试代码:

#include "stdafx.h"
#include <iostream>
#include <vector>
#include <string>
#include "AStar.h"

int main()
{
	const char START = 'S';
	const char END = 'E';
	const char VISITED = '-';
	const char SPACE = '.';
	Grid startPos;
	Grid endPos;

	//地图字符串
	const std::string s_arr[10] = {
		"....................",
		"....................",
		"...II.......IIIII...",
		".....I......I....I..",
		".....I......I..E....",
		".....I......I....I..",
		"...S.I......IIIII...",
		"...II...............",
		"....................",
		"....................",
	};

	std::vector<std::vector<AStar::MapData>> vec;
	std::vector<AStar::MapData> childVec;
	AStar::MapData data;
	for (int i = 0; i < 10; ++i)
	{
		std::string str = s_arr[i];
		childVec.clear();
		for (int j = 0; j < str.size(); ++j)
		{
			char c = str.at(j);
			if (c == START)
			{
				startPos = Grid(i, j);
			}
			else if (c == END)
			{
				endPos = Grid(i, j);
			}

			data.grid.x = i;
			data.grid.y = j;
			data.isSpace = c == SPACE;
			data.symbolStr = c;
			childVec.push_back(data);
		}
		vec.push_back(childVec);
	}
	
	AStar star;
	star.findPath(startPos, endPos, vec);

	getchar();
	return 0;
}

运行结果:

 

 

评论 9
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值