寻找电路布线最短路径算法BFS

问题定义：
将布线区域划分成一格n*m的网格，网格内用-1来标识障碍点，求网格内一点到另一点之间的最短路径。
思想：
1、标记距离：
先用BFS的方法将网格做个标记，在经过每个的点的位置上记录一下该点到初始点start之间的距离，一直到终点end。
2、回走记录路径：
从终点往回走，每次只走到比当前位置与开始点start距离小1的位置,边走边记录路径。

开始时地图：（-1表示障碍不能走,0表示能走)

  -1  -1  -1  -1  -1  -1  -1  -1  -1
  -1   0   0  -1   0   0   0   0  -1
  -1   0   0  -1  -1   0   0   0  -1
  -1   0   0   0   0  -1   0   0  -1
  -1   0   0   0  -1  -1   0   0  -1
  -1  -1   0   0   0  -1   0   0  -1
  -1  -1  -1  -1   0   0   0   0  -1
  -1  -1  -1  -1   0   0   0   0  -1
  -1  -1  -1  -1  -1  -1  -1  -1  -1

代码：

#include<iostream>
#include <iomanip>
#include<queue> 
using namespace std;

const int M = 9;
const int MAXLEN = 30;

class Point
{
public:
	int x;  
	int y;
};

int dx[4] = {0,1,0,-1};   //x、y方向上的增量   上右下左
int dy[4] = {-1,0,1,0};

void findPath(int G[][M],Point start,Point finish,int& pathLen,Point* path)
{
	//寻找从start到finish的最短路径path及其长度pathLen。
	if(start.x == finish.x && start.y == finish.y)     //两点重合 
	{
		pathLen = 0;
		return ; 
	} 
	Point cur,next;
	G[start.x][start.y] = 1;    //封锁开始点 
	queue<int> qx,qy;    //坐标队列 
	qx.push(start.x);    qy.push(start.y);  //开始点进入队列 
	while(!qx.empty())
	{
		cur.x = qx.front(); qx.pop();      // cur为当前位置 
		cur.y = qy.front(); qy.pop();
		for(int i = 0;i < 4; i++)
		{
			next.x = cur.x + dx[i];  next.y = cur.y + dy[i];   // next为下一位置 
			if(!G[next.x][next.y]) 				//该位置未被标记 
			{
				G[next.x][next.y] = G[cur.x][cur.y] + 1; 
				if(next.x == finish.x && next.y == finish.y)    //已到达出口  
				{
						break;
				}   
				qx.push(next.x);
				qy.push(next.y); 
			} 
		}
		if(next.x == finish.x && next.y == finish.y) break;   //已到达出口 
	} 
	
	//构造路径 
	pathLen = G[finish.x][finish.y];
	cur = finish;
	for(int i = pathLen-1; i >= 0; i--)
	{
		path[i] = cur;      //记录当前位置
		for(int j = 0; j < 4; j++)			//寻找前一位置 
		{
			next.x = cur.x + dx[j];			
			next.y = cur.y + dy[j];
			if(G[next.x][next.y] > -1 && G[next.x][next.y] < G[cur.x][cur.y]) 
			{
				break;
			} 
		}
		cur = next;  //移动到当前位置 
	} 
} 

int main()
{
	int G[M][M];          //网格地图 
	Point start,finish;   // 开始点 
	int pathLen=0;          //最短路径 
	for(int i = 0; i < M; i++)  //初始化网外围墙为 -1 
	{
		G[0][i] = G[M-1][i] = G[i][0] = G[i][M-1] = -1; 
	}
	for(int i = 1; i < M-1; i++)    //初始化网格内区域为0 
	{
		for(int j = 1; j < M-1; j++)
			G[i][j] = 0; 
	}
	G[5][1] = -1; G[6][1] = -1; G[6][2] = -1; G[6][3] = -1; G[7][1] = -1;
	G[7][2] = -1; G[7][3] = -1; G[1][3] = -1; G[2][3] = -1; G[2][4] = -1;
	G[3][5] = -1; G[4][4] = -1; G[4][5] = -1; G[5][5] = -1;     // 内部设墙
	for(int i = 0; i < M; i++)
	{
		for(int j = 0; j < M; j++)
		{
			cout<<setw(4)<<G[i][j]; 
		}
		cout<<endl;
	} 
	start.x = 3; start.y = 2; finish.x = 4; finish.y = 6;
	Point *path = new Point[M*M]; 
	findPath(G,start,finish,pathLen,path);
	for(int i = 0; i < M; i++)
	{
		for(int j = 0; j < M; j++)
		{
			cout<<setw(4)<<G[i][j]; 
		}
		cout<<endl;
	}
	for(int i = 0; i < pathLen; i++)
	{
		cout<<"("<< path[i].x <<","<<path[i].y<<")"<<endl;
	} 
	for(int i = 0; i < M; i++)
	{
		for(int j = 0; j < M; j++)
		{
			cout<<setw(4)<<G[i][j]; 
		}
		cout<<endl;
	}
	return 0;
} 

输出：