寻找电路布线最短路径(Queue)

bool FindPath(Position start, Position finish, int& PathLen, Position* &path)
{
 // 寻找从start 到finish 的最短路径

 if((start.row==finish.row) && (start.col==finish.col))
 {
  PathLen=0;
  return true;
 }

 // 初始化包围网格的围墙
 for(int i=0; i<=m+1;i++)
 {
  grid[0][i]=grid[m+1][i]=1;
  grid[i][0]=grid[i][m+1]=1;
 }

 // 初始化offset
 Position offset[4];
 offset[0].row=0; offset[0].col=1; // 右
 offset[1].row=1; offset[1].col=0; //下
 offset[2].row=0; offset[2].col=-1; //左
 offset[3].row=-1; offset[3].col=0; //上

 int NumofNbrs=4;
 Position here, nbr;
 here.row=start.row;
 here.col=start.col;
 grid[here.row][here.col]=2; // 封锁

 // 标记可到达的网格位置
 LinkedQueue<Position> Q;
 do
 {
  //标记相邻位置
  for(int i=0; i<NumofNbrs;i++)
  {
   nbr.row=here.row+offset[i].row;
   nbr.col=here.col+offset[i].col;
   if(grid[nbr.row][nbr.col]==0)
   {
    grid[nbr.row][nbr.col]=grid[here.row][here.col]+1;
    if((nbr.row==finish.row) && (nbr.col==finish.col))
     break;
    Q.Add(nbr);
   }
  }
  if((nbr.row==finish.row) && (nbr.col==finish.col))
   break;

  if(Q.IsEmpty())
   return false;
  Q.Delete(here);
 }while(1);

 // 构造路径
 PathLen=grid[finish.row][finish.col]-2;
 path=new Position [PathLen];

 // 回溯至finish;
 here=finish;
 for(int j=PathLen-1; j>=0; j--)
 {
  path[j]=here;
  // 寻找前一个位置
  for(int i=0; i<NumofNbrs;i++)
  {
   nbr.row=here.row+offset[i].row;
   nbr.col=here.col+offset[i].col;
   if(grid[nbr.row][nbr.col]==j+2)
    break;
  }
  // 移动到前一个位置
  here=nbr;
 }

 return true;
 
}

Ref:<<数据结构，算法与应用>>P201~204