搜到的一个寻路算法

前几天在图书馆借了一本《游戏程序设计概论》，发现这本书还不错，对游戏有个大概的介绍。学了里面的四方向等高线寻路算法后，把它改成了八方向。大概原理是，从目的点开始向周围一步一步扩展，知道遇到起始点为止。用到了两个队列，一个作为源，一个作为目的。先把目的点放入源队列，在将目的点向八个方向扩展，并放入目的队列，然后交换源与目的，再将源队列中的点向周围扩展，并放入目的队列，如此循环，知道遇到起始点。标记好后，从起始点开始寻找最短路径。
程序代码：

[复制到剪贴板]

CODE:

#include <windows.H>
#include <stdio.h>
#include "Queue.h"
#define D_MapWidth 15
#define D_MapHeight 13
#define D_MapSize (D_MapWidth*D_MapHeight)
// 地图数据 15 X 13
// 0 : 不可移动的区域, 1 : 可移动区域.
// 起始坐标 [12,10], 目的坐标 [1,1]
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 };
int BackupMap[D_MapHeight*D_MapWidth];
int Path[D_MapHeight*D_MapWidth];
int CountOfPath = 0;
void QuickMapping ( int Posi_Target, int Posi_Start );
void PathRecorder( int StartPosi, int TargetPosi );
int GetStep  ( int Position, int &StepNo );
void ShowPath ( void );
///////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
int  Posi_Start = (10*D_MapWidth) + 12;
int  Posi_Target = (1*D_MapWidth) + 1;
bool Found = false;
int  Step = 1;
int  MapSize = D_MapSize;
memset ( BackupMap, 0, sizeof(int) * MapSize );
// 由目的点开始绘制"等高线"
QuickMapping ( Posi_Target, Posi_Start );

// 显示结果
for ( int loopy=0 ; loopy<D_MapHeight ; loopy++ )
{
  for ( int loopx=0 ; loopx<D_MapWidth ; loopx++ )
  {
  printf( " %02d", BackupMap[(loopy*D_MapWidth)+loopx] );
  }
  printf(" ");
}
// 计算路径
PathRecorder( Posi_Start, Posi_Target );
// 显示路径
ShowPath();
getchar();
return 0;
}
///////////////////////////////////////////////////////////////////////////////
void QuickMapping ( int Posi_Target, int Posi_Start )
{
clQueue RecA, RecB, *SrcPointer, *DesPointer;
int  StepNo = 1,
  Position,
  MapSize = D_MapWidth * D_MapHeight,
  CurrentPosi,
  QueueSize;
// 计算队列所需的容量, 并重新定义队列的大小.
QueueSize = (D_MapWidth+D_MapHeight)*2;
RecA.Resize ( QueueSize );
RecB.Resize ( QueueSize );
// 初始队列的功能.
SrcPointer = &RecA;
DesPointer = &RecB;
// 先存入第一点的位置.
SrcPointer->Push ( Posi_Target );
BackupMap[Posi_Target]=StepNo++;
for (;;)
{
  // 当 SrcPointer 是空的时候.
  // 将 SrcPointer 与 DesPointer 对调.
  if ( !SrcPointer->Pop( CurrentPosi ) )//失败后就交换源与目的
  {
  if ( SrcPointer == &RecA ){
    SrcPointer = &RecB;
    DesPointer = &RecA;
  }
  else {
    SrcPointer = &RecA;
    DesPointer = &RecB;
  }
  StepNo++;
  if ( !SrcPointer->Pop( CurrentPosi ) ){
    return;      //如果仍失败则返回
  }
  }
  // 判断是否已经结束.
  if ( CurrentPosi == Posi_Start ) {
  BackupMap [ Posi_Start ] = --StepNo;//标记步数
  return;
  }
  /// 向上展开.
  Position = CurrentPosi - D_MapWidth;
  if ( Position >= 0 )  //是否越界
  if ( MapData[ Position ] != 0 )  //是否可行走
    if ( BackupMap[ Position ] == 0 ||BackupMap[ Position ] > StepNo ) //如果等于零，或步数比当前步数更大   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  /// 向下展开.
  Position = CurrentPosi + D_MapWidth;
  if ( Position < MapSize )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  /// 向左展开.
  Position = CurrentPosi - 1;
  if ( (CurrentPosi%D_MapWidth) > 0 )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  /// 向右展开.
  Position = CurrentPosi+1;
  if ( (Position%D_MapWidth) < D_MapWidth )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  ///向左上
  Position = CurrentPosi-1-D_MapWidth;
  if ( (CurrentPosi%D_MapWidth) > 0&&Position >= 0 )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  ///向右上
  Position = CurrentPosi+1-D_MapWidth;
  if ( (Position%D_MapWidth) < D_MapWidth > 0&&Position >= 0 )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  ///向右下
  Position = CurrentPosi+1+D_MapWidth;
  if ( (Position%D_MapWidth) < D_MapWidth > 0&&Position < MapSize )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
  ///向左下
  Position = CurrentPosi-1+D_MapWidth;
  if ( (CurrentPosi%D_MapWidth) >0 > 0&&Position < MapSize )
  if ( MapData[ Position ] != 0 )
    if ( BackupMap[ Position ] == 0 || BackupMap[ Position ] > StepNo )   
    {
    DesPointer->Push ( Position ); // 将展开点的位置存入目的队列.
    BackupMap [ Position ] = StepNo;//标记步数
    }
   
}
}
///////////////////////////////////////////////////////////////////////////////
//
// 记录最短路径
//
void PathRecorder ( int StartPosi, int TargetPosi )
{
int Step = BackupMap[ StartPosi ];
int Index = 0;
int Posi = StartPosi;
Path[ Index++ ] = Posi;
do {
  Posi = GetStep( Posi, Step );
  if ( Posi != -1 ){
  Path[ Index++ ] = Posi;
  if (  Posi == TargetPosi ){
    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;
int nUpLeft=Position - D_MapWidth-1;
int nUpRight=Position - D_MapWidth+1;
int nDownLeft=Position + D_MapWidth-1;
int nDownRight=Position + D_MapWidth+1;
bool Found = false;
// up.
if ( nUp >= 0 ){
  if ( BackupMap[ nUp ] != 0 && BackupMap[ nUp ] < StepNo ) 
  {
  StepNo = BackupMap[ nUp ];
  return nUp;
  }
}
// down.
if ( nDown < D_MapSize ){
  if ( BackupMap[ nDown ] != 0 && BackupMap[ nDown ] < StepNo ) 
  {
  StepNo = BackupMap[ nDown ];
  return nDown;
  }
}
// left.
if ( (Position%D_MapWidth) > 0 ) {
  if ( BackupMap[ nLeft ] != 0 && BackupMap[ nLeft ] < StepNo ) 
  {
  StepNo = BackupMap[ nLeft ];
  return nLeft;
  }
}
// right.
if ( (nRight%D_MapWidth) != 0 ) {
  if ( BackupMap[ nRight ] != 0 && BackupMap[ nRight ] < StepNo ) 
  {
  StepNo = BackupMap[ nRight ];
  return nRight;
  }
}
// UpRight.
if ( (nUpRight%D_MapWidth) != 0&&nUpRight>= 0 ) {
  if ( BackupMap[ nUpRight ] != 0 && BackupMap[ nUpRight ] < StepNo ) 
  {
  StepNo = BackupMap[ nUpRight ];
  return nUpRight;
  }
}
// UpLeft.
if ( (Position%D_MapWidth) > 0 &&nUpLeft>= 0 ) {
  if ( BackupMap[ nUpLeft ] != 0 && BackupMap[ nUpLeft ] < StepNo ) 
  {
  StepNo = BackupMap[ nUpLeft ];
  return nUpLeft;
  }
}
// DownLeft.
if ( nDownLeft< D_MapSize &&nDownLeft>= 0 ) {
  if ( BackupMap[ nDownLeft ] != 0 && BackupMap[ nDownLeft ] < StepNo ) 
  {
  StepNo = BackupMap[ nDownLeft ];
  return nDownLeft;
  }
}
// DownRight.
if ( (nDownRight%D_MapWidth) != 0 &&nDownRight < D_MapSize ) {
  if ( BackupMap[ nDownRight ] != 0 && BackupMap[ nDownRight ] < StepNo ) 
  {
  StepNo = BackupMap[ nDownRight ];
  return nDownRight;
  }
}
return -1;
}
///////////////////////////////////////////////////////////////////////////////
///
/// 显示路径
///
void ShowPath ( void )
{
int Posi;
memset( BackupMap, 0, sizeof(int) * D_MapSize );
for ( int loop=0 ; loop<CountOfPath ; loop++ )
{
  Posi = Path[ loop ];
  BackupMap[ Posi ] = 1;
}
printf(" 显示路径 ");
for ( int loopy=0 ; loopy<D_MapHeight ; loopy++ )
{
  for ( int loopx=0 ; loopx<D_MapWidth ; loopx++ )
  {
  printf( " %02d", BackupMap[(loopy*D_MapWidth)+loopx] );
  }
  printf(" ");
}
}

用到的队列头文件：

[复制到剪贴板]

CODE:

class clQueue
{
private:
unsigned int mCount;
unsigned int mBufSize;
          int *mBuffer;
unsigned int PushIndex;    // Push 数据存入端
    unsigned int  PopIndex;  // Pop 数据取出端
public:
clQueue() : mCount(0), mBufSize(10), mBuffer(NULL), PushIndex(0), PopIndex(0)
{
  mBuffer = new int[ mBufSize ];
}
~clQueue()
{
  if ( mBuffer != NULL ){
  delete [] mBuffer;
  mBuffer = NULL;
  }
}
bool Resize ( int NewSize ){
  if ( mBuffer != NULL ){
  delete [] mBuffer;
  mBuffer = NULL;
  }
  mBufSize = NewSize;
  if ( mBufSize == 0 ){ return true; }
  mBuffer = new int [ mBufSize ];
  return ((mBuffer==NULL) ? false : true);
}
bool Push ( int nNum )    // 存入数据.
{
  if ( mCount >= mBufSize ){
  return false;
  }
  mBuffer[ PushIndex ] = nNum;
  PushIndex++;
  if ( PushIndex >= mBufSize ) {
  PushIndex = 0 ;
  }
  mCount++;
  return true;
}
bool Pop ( int &nNum )    // 取出数据
{
  if ( mCount == 0 ){
  return false;
  }
  nNum = mBuffer[ PopIndex ];
  PopIndex ++;
  if ( PopIndex >= mBufSize ){
  PopIndex = 0;
  }
  mCount--;
  return true;
}
};

程序运行结果：
02 02 02 03 00 00 00 00 00 12 13 14 15 16 17
02 01 02 03 00 00 00 00 11 12 13 14 15 16 17
02 02 02 03 00 00 10 10 11 12 13 14 15 16 17
03 03 03 00 00 09 09 10 11 12 00 14 15 16 17
04 04 00 00 00 08 09 10 00 00 00 15 15 16 17
05 05 05 06 07 08 09 00 00 00 16 16 16 16 00
00 06 06 06 07 08 09 00 00 17 17 17 17 00 00
00 07 07 07 07 08 00 00 18 18 18 18 00 00 00
00 00 08 08 08 00 00 19 19 19 00 00 00 00 00
00 00 00 09 00 00 00 20 20 20 00 24 24 00 00
00 00 00 00 00 22 21 21 21 21 00 23 24 25 00
00 00 00 00 23 22 22 22 22 22 22 23 24 00 00
00 00 00 00 23 23 23 23 23 23 23 00 00 00 00

显示路径
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
00 01 00 00 00 00 00 00 00 00 00 00 00 00 00
00 01 00 00 00 00 00 01 01 01 01 00 00 00 00
00 01 00 00 00 00 01 00 00 00 00 01 00 00 00
00 01 00 00 00 01 00 00 00 00 00 01 00 00 00
00 00 01 01 01 00 00 00 00 00 01 00 00 00 00
00 00 00 00 00 00 00 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 01 00 00 00 00 00
00 00 00 00 00 00 00 00 00 01 00 01 01 00 00
00 00 00 00 00 00 00 00 00 00 01 00 00 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
原理与书上四方向是一样的，只是多了对左上，左下，右上，右下四个方向的处理。