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