参考: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;
}
运行结果: