基于队列的最短路径搜索算法(A*算法)实现

       今天总算实现了一个基于我开发的快速链表的队列,并使用基于队列的最短路径搜索算法做测试,现在把核心函数贴在这里,这个函数参考了《数据结构与算法C++语言描述》书上的FindPath函数，原函数只能朝四个方向搜索，我改成了8个方向，现在把代码贴在这里，与大家分享，有问题请跟贴。

#pragma pack(push)
#pragma pack(1)

struct ST_GRS_POINT
{
 SHORT m_nX;
 SHORT m_nY;
public:
 ST_GRS_POINT(SHORT x,SHORT y)
 {
  m_nX = x;
  m_nY = y;
 }
 
 ST_GRS_POINT(const ST_GRS_POINT&pt)
 {
  m_nX = pt.m_nX;
  m_nY = pt.m_nY;
 }

 ST_GRS_POINT()
  :m_nX(0),
  m_nY(0)
 {
 }
 ~ST_GRS_POINT()
 {
 }
public:
 ST_GRS_POINT& operator = (SHORT n)
 {
  m_nX = n;
  m_nY = n;
  return *this;
 }

 ST_GRS_POINT& operator = (const ST_GRS_POINT& pt)
 {
  m_nX = pt.m_nX;
  m_nY = pt.m_nY;
  return *this;
 }
 

};
#pragma pack(pop)

static UINT g_NumOfNbrs = 8; // 一个网格位置的相邻位置数
static ST_GRS_POINT g_Offset[8];
static int g_M = 256;

SHORT g_Grid[256 + 2][256 + 2];

bool FindPath(ST_GRS_POINT ptStart,
     ST_GRS_POINT ptFinish,
     USHORT& PathLen,
     ST_GRS_POINT*& path)
{ //寻找从ptStart到ptFinish的路径
 // 如果成功，则返回true，否则返回false
 // 如果空间不足，则引发异常NoMem
 if ((ptStart.m_nY == ptFinish.m_nY) &&
  (ptStart.m_nX == ptFinish.m_nX))
 {// ptStart = ptFinish
  PathLen = 0;
  return true;
 }
 
 // 初始化包围网格的“围墙”
 for (int i = 0; i <= g_M + 1; i++)
 {
  g_Grid[0][i] = g_Grid[g_M + 1][i] = 1; // 底和顶
  g_Grid[i][0] = g_Grid[i][g_M + 1] = 1; // 左和右
 }

 // 初始化offset
 g_Offset[0].m_nY = 0; g_Offset[0].m_nX = 1; // 右
 g_Offset[1].m_nY = 1; g_Offset[1].m_nX = 0; // 下
 g_Offset[2].m_nY = 0; g_Offset[2].m_nX = -1; // 左
 g_Offset[3].m_nY = -1; g_Offset[3].m_nX = 0; // 上

 g_Offset[4].m_nY = 1; g_Offset[4].m_nX = 1; // 右下
 g_Offset[5].m_nY = 1; g_Offset[5].m_nX = -1; // 左下
 g_Offset[6].m_nY = -1; g_Offset[6].m_nX = 1; // 右上
 g_Offset[7].m_nY = -1; g_Offset[7].m_nX = -1; // 左上
 
 ST_GRS_POINT here, nbr;
 here.m_nY = ptStart.m_nY;
 here.m_nX = ptStart.m_nX;
 g_Grid[ptStart.m_nY][ptStart.m_nX] = 2; // 封锁

 // 标记可到达的网格位置
 CGRSQueue<ST_GRS_POINT> Q;
 do
 {//标记相邻位置
  for (int i = 0; i < g_NumOfNbrs; i++)
  {
   nbr.m_nY = here.m_nY + g_Offset[i].m_nY;
   nbr.m_nX = here.m_nX + g_Offset[i].m_nX;

   if (g_Grid[nbr.m_nY][nbr.m_nX] == 0)
   { // 没有标记 nbr, 打上标记
    g_Grid[nbr.m_nY][nbr.m_nX] = g_Grid[here.m_nY][here.m_nX] + 1;
    if ((nbr.m_nY == ptFinish.m_nY) && (nbr.m_nX == ptFinish.m_nX))
    {
     break; // 完成
    }
    Q.Add(nbr);
   } // if 结束
  } // for 结束

  //已到达ptFinish吗?
  if ((nbr.m_nY == ptFinish.m_nY) &&
   ( nbr.m_nX == ptFinish.m_nX))
  {
   break; // 完成
  }

  //未到达ptFinish，可移动到nbr吗?
  if (Q.IsEmpty())
  {
   return false; // 没有路径
  }

  Q.Delete(here); // 到下一位置
 } while(true);

 // 构造路径
 PathLen = g_Grid[ptFinish.m_nY][ptFinish.m_nX] - 2;
 path = new ST_GRS_POINT[PathLen];

 // 回溯至ptFinish
 here = ptFinish;
 for (int j = PathLen-1; j >= 0; j--)
 {
  path[j] = here;
  //寻找前一个位置
  for (int i = 0; i < g_NumOfNbrs; i++)
  {
   nbr.m_nY = here.m_nY + g_Offset[i].m_nY;
   nbr.m_nX = here.m_nX + g_Offset[i].m_nX;
   if (g_Grid[nbr.m_nY][nbr.m_nX] == j + 2)
   {
    break;
   }
  }
  here = nbr; // 移动到前一个位置
 }
 return true;
}