算法学习——A*搜索算法
#include <>
#include <>
#include <iostream>
#include <vector>
using namespace std;
#define MAP_WIDTH 11 // 图宽 (1 pixel block)
#define MAP_HEIGHT 9 // 图高 (1 pixel block)
#define straight_cost 10 // 直线代价
#define diag_cost 14 // 斜线代价
typedef struct
{
int8_t row;
int8_t col;
uint16_t f;
uint16_t g;
uint16_t h;
}point_t;
//树的节点类型
struct treeNode // 结构体可以定义自身类型的指针（指针变量大小可以确定，而定义自身类型的成员则不行，因为编译时成员存储空间大小不确定）
{
point_t pos; // 当前点坐标
struct treeNode* pParent; // 存储当前点的父节点的指针变量
};
const uint8_t map[MAP_HEIGHT][MAP_WIDTH] = {
{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
{0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0,},
{0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0,},
{0, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0,},
{0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 0,},
{0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0,},
{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,},
{0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,},
};
// 2 辅助地图->标记已走过的点（封闭列表） false: 没有走过 true: 已走过
bool closeList[MAP_HEIGHT][MAP_WIDTH] = { false };
// 3 起点 终点
point_t start_pos = { 2, 2 };
point_t end_pos = { 4, 8 };
enum dircet
{
up,
down,
left,
right,
lup,
ldown,
rup,
rdown
};
// 计算预估代价H
uint16_t getH(point_t end_pos, point_t current_pos)
{
uint16_t x = abs(end_pos.col - current_pos.col);
uint16_t y = abs(end_pos.row - current_pos.row);
return (x + y) * straight_cost;
}
bool isAddOpenList(point_t pos, const uint8_t(*pMap)[MAP_WIDTH], bool(*pCloseList)[MAP_WIDTH])
{
if (pos.row >= MAP_HEIGHT || pos.row < 0 ||
pos.col >= MAP_WIDTH || pos.col < 0) // 越界
return false;
if (1 == pMap[pos.row][pos.col]) //! 障碍物
return false;
if (true == pCloseList[pos.row][pos.col]) //已走过
return false;
return true;
}
bool isFindEnd = false;
int main()
{
//4 标记起点走过
closeList[start_pos.row][start_pos.col] = true;
//5 创建一颗树，树的根节点是起点
treeNode* pRoot = new treeNode;
//新节点是起点
pRoot->pos = start_pos;
//6 用一个动态数组存储用来比较的节点（开放列表）
vector<treeNode*> openList;
//7 寻路
treeNode* pCurrent = pRoot;
treeNode* pChild = NULL;
vector<treeNode*>::iterator it;
vector<treeNode*>::iterator itMin;
while (1) {
//7.1 找到当前点周围能走的点
for (uint8_t i = 0; i < 8; i++)
{
pChild = new treeNode;
pChild->pos = pCurrent->pos;
switch (i)
{
case up:
pChild->pos.row--;
pChild->pos.g += straight_cost;
break;
case down:
pChild->pos.row++;
pChild->pos.g += straight_cost;
break;
case dircet::left:
pChild->pos.col--;
pChild->pos.g += straight_cost;
break;
case dircet::right:
pChild->pos.col++;
pChild->pos.g += straight_cost;
break;
case lup:
pChild->pos.row--;
pChild->pos.col--;
pChild->pos.g += diag_cost;
break;
case ldown:
pChild->pos.row++;
pChild->pos.col--;
pChild->pos.g += diag_cost;
break;
case rup:
pChild->pos.row--;
pChild->pos.col++;
pChild->pos.g += diag_cost;
break;
case rdown:
pChild->pos.row++;
pChild->pos.col++;
pChild->pos.g += diag_cost;
break;
default:
break;
}
//7.2 计算 g h f 值
pChild->pos.h = getH(end_pos, pChild->pos);
pChild->pos.f = pChild->pos.g + pChild->pos.h;
//7.3 入树，入开放列表，标记走过
if (isAddOpenList(pChild->pos, map, closeList)) // 判断是否满足入树条件
{
pChild->pParent = pCurrent; //父指针指向当前点
//入开放列表（open_list）
openList.push_back(pChild);
//标记已走过
closeList[pChild->pos.row][pChild->pos.col] = true;
}
else {
delete pChild;
}
}
if (openList.empty()) //! 如果开放列表为空，停止搜索（不会此条件遇到死路时，一直卡在循环不断开辟堆区空间造成内存溢出）
break;
//7.4 从开放列表中找到f值最小的节点
itMin = openList.begin();
for (it = openList.begin(); it != openList.end(); it++)
{
itMin = ((*itMin)->pos.f > (*it)->pos.f) ? it : itMin;
}
//7.5 然后变为当前点，再从开放列表中删掉
pCurrent = *itMin;
openList.erase(itMin);
//7.6 判断寻路结束
if (pCurrent->pos.col == end_pos.col && pCurrent->pos.row == end_pos.row)
{
isFindEnd = true;
break;
}
}
if (isFindEnd == true)
{
printf("找到终点了\n");
while (pCurrent)
{
printf("(%d,%d)", (int)pCurrent->pos.row, (int)pCurrent->pos.col);
if (pCurrent->pos.row == start_pos.row && pCurrent->pos.col == start_pos.col)
break;
pCurrent = pCurrent->pParent; // 通过父指针回溯根节点(起点父指针为空)
}
}
else
printf("没有找到终点\n");
delete pRoot;
system("pause");
return 0;
}