对于迷宫寻路的一般算法是用递归实现,或者用栈来实现,二者其实基本思路都一样:由起点开始,逐个点遍历上下左右,重复遍历率高,效率一般。还有就是A×算法,不过掌握起来难度较大。现介绍一种效率高,同时又较简单的寻路算法:等高线算法。
所谓等高线算法,主要原理是:由终点开始延伸,每个点的四个方向相临点属于同一个高度,将每个点到终点的最小距离记录到辅助数组的相应位置,如果有权值的话每走一步加的是权值,否则直接加加,这样所有位置到终点的距离都在辅助数组里面,最后只要到辅助数组里面去收集最短路径就可以了。
此算法同样适用于有障碍物的地图寻路,这里只给出了四联通的,此算法同样适合八个方向的情况。算法代码如下:
//
MapContour.cpp : 定义控制台应用程序的入口点。
//
算法原理: 从终点往外开始延伸, 直到起始点,
//
再从起始点选择最小的号码, 逐步按照路径行走, 回到终点,即算完成
#include
<
iostream
>
#include
<
queue
>
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
using
namespace
std;
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
#define
D_MapWidth 15
#define
D_MapHeight 13
#define
D_MapSize (D_MapWidth * D_MapHeight)
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
数组MapData记录地图中的原始数据;
//
1表示有路可走;
//
0表示没有路相通
int
MapData[D_MapHeight
*
D_MapWidth]
=
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,
1,1,1,1,0,0,0,0,1,1,1,1,1,1,1,
1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,
1,1,1,0,0,1,1,1,1,1,0,1,1,1,1,
1,1,0,0,0,1,1,1,0,0,0,1,1,1,1,
1,1,1,1,1,1,1,0,0,0,1,1,1,1,0,
0,1,1,1,1,1,1,0,0,1,1,1,1,0,0,
0,1,1,1,1,1,0,0,1,1,1,1,0,0,0,
0,0,1,1,1,0,0,1,1,1,0,0,0,0,0,
0,0,0,1,0,0,0,1,1,1,0,1,1,1,1,
0,0,0,0,0,1,1,1,1,1,0,1,1,1,0,
0,0,0,0,1,1,1,1,1,1,1,1,1,0,0,
0,0,0,0,1,1,1,1,1,1,1,0,0,0,0
}
;
//
计算用的地图数组
//
BackupMap数组记录地图上的各点到达起始点的最小步数;
int
BackupMap[D_MapWidth
*
D_MapHeight];
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
记录路径的数组
//
Path数组依次记录:结果路径的顶点;
int
Path[D_MapHeight
*
D_MapWidth];
//
为Path数组的容量;
int
CountOfPath
=
0
;
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
bool
SetpMaker(
int
Position,
int
StepNo,
int
CheckPosition);
void
QuickMapping(
int
Pos_Target,
int
Pos_Start);
void
PathRecorder(
int
StartPos,
int
TargetPos);
int
GetStep(
int
Position,
int
&
StepNo);
void
ShowPath(
void
);
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
由于STL的queue的函数都是正交的,
//
将Pop函数与front函数组合返回队列是非为空;
template
<
typename T
>
bool
IsNullAfterPop(queue
<
T
>
*
que, T
&
current)
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
if (que->empty())
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
return false;
}
current = que->front();
que->pop();
return true;
}
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
int
main()
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
//起点,终点;
int Pos_Start = (10 * D_MapWidth) + 12;
int Pos_Target = (1 * D_MapWidth) + 1;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
bool Found = false;
int Step = 1;
int Mapsize = D_MapSize;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
//初始化辅助数组;
memset(BackupMap, 0, sizeof(int)* Mapsize);
//将每个位置到终点的距离写到辅助数组BackupMap里面
QuickMapping(Pos_Target, Pos_Start);
//输出辅助数组内容:
for (int i = 0; i < D_MapHeight; i++)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
for (int j = 0; j < D_MapWidth; j++)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
printf(" %02d", BackupMap[i*D_MapWidth + j]);
}
cout << endl;
}
//筛选最小路径
PathRecorder(Pos_Start, Pos_Target);
//显式路径;
ShowPath();
return 0;
}
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
void
QuickMapping(
int
Pos_Target,
int
Pos_Start)
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
queue<int>QueA, QueB, *SrcPointer, *DesPointer;
int StepNo = 1,
Position,
MapSize = D_MapWidth*D_MapHeight,
CurrentPos;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
SrcPointer = &QueA;
DesPointer = &QueB;
SrcPointer->push(Pos_Target);
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
for (;;)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if ( !IsNullAfterPop(SrcPointer, CurrentPos) )
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (SrcPointer == &QueA)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
SrcPointer = &QueB;
DesPointer = &QueA;
StepNo ++;
}
else
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
SrcPointer = &QueA;
DesPointer = &QueB;
StepNo++;
}
//两者都空退出;
if ( !IsNullAfterPop(SrcPointer, CurrentPos) )
return;
}
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
BackupMap[CurrentPos] = StepNo;
//找到起点;结束;
if (CurrentPos == Pos_Start)
return;
//To UP
Position = CurrentPos - D_MapWidth;
if (Position >= 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (MapData[Position] != 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[Position] == 0 ||
BackupMap[Position] > StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
DesPointer->push(Position);
}
}
}
//To DOWN
Position = CurrentPos + D_MapWidth;
if (Position < MapSize)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (MapData[Position] != 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[Position] == 0 ||
BackupMap[Position] > StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
DesPointer->push(Position);
}
}
}
//To LEFT
Position = CurrentPos - 1;
if ( (CurrentPos%D_MapWidth) > 0 )
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (MapData[Position] != 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[Position] == 0 ||
BackupMap[Position] > StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
DesPointer->push(Position);
}
}
}
//To RIGHT
Position = CurrentPos + 1;
if ( (Position%D_MapWidth) < D_MapWidth )
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (MapData[Position] != 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[Position] == 0 ||
BackupMap[Position] > StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
DesPointer->push(Position);
}
}
}
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
}//for(;;)
}
//
QuickMapping()
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
将理想的点放进Path数组中;
void
PathRecorder(
int
StartPos,
int
TargetPos)
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
int Step = BackupMap[StartPos];
int index = 0;
int Pos = StartPos;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
Path[index++] = Pos;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
do
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
Pos = GetStep(Pos, Step);
if (Pos != -1)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
Path[index++] = Pos;
if (Pos == TargetPos)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
CountOfPath = index;
return ;
}
}
else
return;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
} while(true);
}
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
检查所在位置的四个方向; 找出最小的位置,
//
也就是最短路径的检测方法;
int
GetStep(
int
Position,
int
&
StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
int nUp = Position - D_MapWidth;
int nDown = Position + D_MapWidth;
int nLeft = Position - 1;
int nRight = Position + 1;
bool Found = false;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
if (nUp >= 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[nUp] != 0 && BackupMap[nUp] < StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
StepNo = BackupMap[nUp];
return nUp;
}
}
if (nDown < D_MapSize)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[nDown] != 0 && BackupMap[nDown] < StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
StepNo = BackupMap[nDown];
return nDown;
}
}
if ( (Position%D_MapWidth) > 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[nLeft] != 0 && BackupMap[nLeft] < StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
StepNo = BackupMap[nLeft];
return nLeft;
}
}
if ( (nRight%D_MapWidth) != 0)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
if (BackupMap[nRight] != 0 && BackupMap[nRight] < StepNo)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
StepNo = BackupMap[nRight];
return nRight;
}
}
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
return -1;
}
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
![](https://i-blog.csdnimg.cn/blog_migrate/6810355c2f78c12e91b7997a8e8c583a.gif)
//
先把BackuoMap数组清空;
//
再根据Path数组将路径的数据写入到BackupMap数组中,
//
最后显示BackupMap[]数组内容;
void
ShowPath(
void
)
![](https://i-blog.csdnimg.cn/blog_migrate/a41954a27d6ad96fa2c2cf816e677448.gif)
...
{
int Pos;
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
memset(BackupMap, 0, sizeof(int) * D_MapSize);
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
for (int loop = 0; loop < CountOfPath; loop++)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
Pos = Path[loop];
BackupMap[Pos] = 1;
}
![](https://i-blog.csdnimg.cn/blog_migrate/6a9c071a08f1dae2d3e1c512000eef41.gif)
cout << " 显示路径 ";
for (int loopy = 0; loopy < D_MapHeight; loopy++)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
for (int loopx = 0; loopx < D_MapWidth; loopx++)
![](https://i-blog.csdnimg.cn/blog_migrate/37c8bf68cdc3cc81759c34160776bc53.gif)
...{
printf(" %02d", BackupMap[loopy*D_MapWidth + loopx]);
}
cout << endl;
}
}
核心函数QuickMapping()的实现简介:使用两个队列来遍历,为了交换两个队列内容的需要,定义了两个队列指针。原理的话:当前遍历A点,找到其四个方向的点Aup,Adown, Aleft, Aright,此时A点是源点,而Aup,Adown, Aleft, Aright都是目标点。但是在下一次Src队列中的源点全部找完后,即Src为空,交换Src与Dest队列的内容,他们就分别都成为源点了,这样继续记录下去。
当来两个队列都为空,或者起点找到,就退出函数。