一个小笔记(5)：A*算法
A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法
其实百科有
咳咳，直接上代码。各种注释也算是有助理解了，毕竟这还是抄的~
// A*寻路算法.cpp : 定义控制台应用程序的入口点。
// Win32控制台程序
#include <math.h>
#include <list>
using namespace std;
/*
把地图当成一个个的格子
公式：F=G+H
F：相对路径长度
G：从起点沿着产生的路径，移动到指定点的耗费（路径长度）
H：预估值，从指定的格子移动到终点格子的预计耗费
使用两个表来保存相关数据
启动列表：有可能将要经过的点存到启动列表
关闭列表：不会再被遍历的点
步骤
1、将起点格子加入启动列表中
2、在启动列表中查找权值（F值）最小的格子
3、查找它周围的能走的格子
4、把这些格子加入启动列表中，已经在启动或关闭列表中的格子不用加入
5、把这些加入启动列表的格子的"父格子"设为当前格子
6、再把当前格子从启动列表中删除，加入关闭列表中
7、如果终点在启动列表中，则找到路径，退出流程，不进行第9步
8、如果启动列表中没有格子了，说明没有找到路径，退出流程，不进行第9步
9、跳转第2步
*/
// 0：可行走的点
// 1：阻挡点
// 2：路径
// 3：起点
// 4：终点
int g_PathLattice[10][10] =
{
{ 0,0,0,0,0,0,0,0,0,0 },
{ 0,0,0,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 },
{ 0,0,3,0,1,0,4,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,0,0,0,0 },
{ 0,0,0,0,0,0,0,0,0,0 },
{ 0,0,0,0,0,0,0,0,0,0 },
};
struct Node
{
int row; // 行
int rank; // 列
int f;
int g;
int h;
Node * pParent; // 当前结点路径的前一个结点（父格子）
};
#define LatticeLen 10 // 格子边长
// 函数前向声明
int Distance(int row1, int rank1, int row2, int rank2);
bool IsNodeInList(Node * pNode, list<Node *> list);
Node * GetNearestNode(list<Node *> list, Node * Rec);
void GetNearNodeList(Node * pNode, list<Node *> & listNear,
list<Node *> listStart, list<Node *> listEnd, Node * pEndNode);
void EraseFromList(Node * pNode, list<Node *> & listStart);
void ClearList(list<Node *> nodeList);
int main()
{
// 起点
int rowStart;
int rankStart;
// 终点
int rowEnd;
int rankEnd;
// 查找起点和终点的位置
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 10; j++)
{
if (g_PathLattice[i][j] == 3)
{
rowStart = i;
rankStart = j;
}
if (g_PathLattice[i][j] == 4)
{
rowEnd = i;
rankEnd = j;
}
}
}
// 起点
Node * nodeStart = new Node;
nodeStart->row = rowStart;
nodeStart->rank = rankStart;
nodeStart->g = 0;
nodeStart->h = Distance(rowStart, rankStart, rowEnd, rankEnd);
nodeStart->f = nodeStart->h;
nodeStart->pParent = nullptr;
// 终点
Node * nodeEnd = new Node;
nodeEnd->row = rowEnd;
nodeEnd->rank = rankEnd;
// 定义启动列表和关闭列表
list<Node *> listStart;
list<Node *> listEnd;
// 把起点加入启动列表
listStart.push_back(nodeStart);
// 当前结点
Node * pNowNode = nullptr;
// 如果终点在启动列表中，则已经找到路径，退出循环
while (!IsNodeInList(nodeEnd, listStart))
{
Node * Rec = nullptr;
// 查找权值最小的格子作为当前点
pNowNode = GetNearestNode(listStart, Rec);
// 如果没有找到，则说明没有路径
if (pNowNode == nullptr)
{
break;
}
// 存放当前格子周围能加入启动列表的格子
list<Node *> listNear;
GetNearNodeList(pNowNode, listNear, listStart, listEnd, nodeEnd);
// 将当前结点加入关闭列表中
listEnd.push_back(pNowNode);
// 将当前结点从启动列表中删除
EraseFromList(pNowNode, listStart);
// 将周围点加入启动列表中
for (list<Node *>::iterator it = listNear.begin();
it != listNear.end(); it++)
{
listStart.push_back(*it);
}
}
if (pNowNode == nullptr)
{
printf("路径不存在\n");
ClearList(listStart);
ClearList(listEnd);
delete nodeEnd;
return 0;
}
// 在启动列表中找到终点
Node * pNodeFind = nullptr;
for (list<Node *>::iterator it = listStart.begin();
it != listStart.end(); it++)
{
if ((*it)->row == nodeEnd->row &&
(*it)->rank == nodeEnd->rank)
{
pNodeFind = (*it);
break;
}
}
while (pNodeFind)
{
g_PathLattice[pNodeFind->row][pNodeFind->rank] = 2;
pNodeFind = pNodeFind->pParent;
}
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 10; j++)
{
if (g_PathLattice[i][j] == 0)
{
printf("^ ");
}
else if (g_PathLattice[i][j] == 1)
{
printf("* ");
}
else if (g_PathLattice[i][j] == 2)
{
printf("# ");
}
}
printf("\n");
}
ClearList(listStart);
ClearList(listEnd);
delete nodeEnd;
return 0;
}
int Distance(int row1, int rank1, int row2, int rank2)
{
// 格子的中点坐标
int x1 = rank1 * LatticeLen + LatticeLen / 2;
int y1 = row1 * LatticeLen + LatticeLen / 2;
int x2 = rank2 * LatticeLen + LatticeLen / 2;
int y2 = row2 * LatticeLen + LatticeLen / 2;
return (int)sqrt((double)((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)));
}
bool IsNodeInList(Node * pNode, list<Node *> NodeList)
{
for (list<Node *>::iterator it = NodeList.begin();
it != NodeList.end(); it++)
{
if (pNode->row == (*it)->row && pNode->rank == (*it)->rank)
{
return true;
}
}
return false;
}
Node * GetNearestNode(list<Node *> NodeList, Node * Rec)
{
int tempF = 1000000;
for (list<Node *>::iterator it = NodeList.begin();
it != NodeList.end(); it++)
{
if ((*it)->f < tempF)
{
Rec = *it;
tempF = (*it)->f;
}
}
return Rec;
}
void GetNearNodeList(Node * pNode, list<Node *> & listNear,
list<Node *> listStart, list<Node *> listEnd, Node * pEndNode)
{
// 将结点旁边的8个点加入到listNear中
// 在启动或关闭列表中的点不能加入listNear
// 阻挡点不能加入listNear
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
if (i == 0 && j == 0)
{
// 自己格子
continue;
}
int rowTemp = pNode->row + i;
int rankTemp = pNode->rank + j;
if (rowTemp < 0 || rankTemp < 0 || rowTemp > 9 || rankTemp > 9)
{
// 越界
continue;
}
if (g_PathLattice[rowTemp][rankTemp] == 1)
{
// 阻挡点
continue;
}
Node node;
node.row = rowTemp;
node.rank = rankTemp;
if (IsNodeInList(&node, listStart))
{
// 在启动列表中
continue;
}
if (IsNodeInList(&node, listEnd))
{
// 在关闭列表中
continue;
}
Node * pNearNode = new Node;
pNearNode->g = pNode->g + Distance(pNode->row, pNode->rank, rowTemp, rankTemp);
pNearNode->h = Distance(rowTemp, rankTemp, pEndNode->row, pEndNode->rank);
pNearNode->f = pNearNode->g + pNearNode->h;
pNearNode->row = rowTemp;
pNearNode->rank = rankTemp;
pNearNode->pParent = pNode;
listNear.push_back(pNearNode);
}
}
}
void EraseFromList(Node * pNode, list<Node *> & listStart)
{
for (list<Node *>::iterator it = listStart.begin();
it != listStart.end(); it++)
{
if (pNode->row == (*it)->row && pNode->rank == (*it)->rank)
{
listStart.erase(it);
return;
}
}
}
void ClearList(list<Node *> nodeList)
{
for (list<Node *>::iterator it = nodeList.begin();
it != nodeList.end(); it++)
{
delete *it;
}
}
