搜到的一个寻路算法

#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("\n");
}
// 计算路径
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("\n\n显示路径\n");
for ( int loopy=0 ; loopy<D_MapHeight ; loopy++ )
{
  for ( int loopx=0 ; loopx<D_MapWidth ; loopx++ )
  {
  printf( " %02d", BackupMap[(loopy*D_MapWidth)+loopx] );
  }
  printf("\n");
}
}

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;
}
};