http://poj.org/problem?id=1376

題意：一個直徑機器人在一個m*n格子的木板上走，按照go或者turn命令走，
go是指每秒走1-3米，
turn是指轉90度，花一秒，

問從起點到終點要花多少秒

知道原來的方向

#include <iostream>
#include <queue>
using namespace std;
int n,m,a[55][55],b[55][55],sx,sy,ex,ey,d[4][2]={{-1,0},{0,1},{1,0},{0,-1}};
char c[10];
struct point {
	int x,y,i,time;
}p;
void bfs()
{
	int i,j,r,k;
	point t;
	p.x=sx;p.y=sy;p.time=0; b[sx][sy]=0;
	queue <point> q;
	q.push(p);
	while(!q.empty())
	{
		p=q.front(); q.pop();
		for(i=0;i<4;i++)
		{
			switch(i)
			{
				case 0: break; //原方向 
				case 1: p.i=(p.i-1+4)%4; p.time++;break;//向左 
				case 2: p.i=(p.i+2)%4; break;//向右 
				case 3: p.i=(p.i+1)%4;p.time++;//相反  
			}
			for(j=1;j<4;j++)
			{
			r=p.x+j*d[p.i][0];k=p.y+j*d[p.i][1]; t.time=p.time+1;
			if(r>=1&&r<n&&k>=1&&k<m&&!a[r][k]&&!a[r][k+1]&&!a[r+1][k]&&!a[r+1][k+1])
			{
			if(b[r][k]>t.time) b[r][k]=t.time,t.x=r,t.y=k,t.i=p.i,q.push(t);//如果小於當前時間則壓入 
			}
			else break;
		    }
	     }
	}
}
int main(int argc, char *argv[])
{
	int i,j,k;
	while(cin>>n>>m&&(n||m))
	{
		for(i=1;i<=n;i++)
		for(j=1;j<=m;j++)
		{
			cin>>a[i][j]; b[i][j]=1000000;
		}
		cin>>sx>>sy>>ex>>ey>>c;
		if(c[0]=='n') p.i=0;
		if(c[0]=='e') p.i=1;
		if(c[0]=='s') p.i=2;
		if(c[0]=='w') p.i=3;
		bfs();
		if(b[ex][ey]==1000000) cout<<-1<<endl; else cout<<b[ex][ey]<<endl;
	}
	return 0;
}

#include <iostream>
#include <queue>
using namespace std;
int n,m,a[55][55],b[55][55],sx,sy,ex,ey,d[4][2]={{-1,0},{0,1},{1,0},{0,-1}};
char c[10];
struct point {
	int x,y,i,time;
}p;
void bfs()
{
	int i,j,r,k;
	point t;
	p.x=sx;p.y=sy;p.time=0; b[sx][sy]=0;
	queue <point> q;
	q.push(p);
	while(!q.empty())
	{
		p=q.front(); q.pop();
		for(i=0;i<4;i++)
		{
			switch(i)
			{
				case 0: break; //原方向 
				case 1: p.i=(p.i-1+4)%4; p.time++;break;//向左 
				case 2: p.i=(p.i+2)%4; break;//向右 
				case 3: p.i=(p.i+1)%4;p.time++;//相反  
			}
			for(j=1;j<4;j++)
			{
			r=p.x+j*d[p.i][0];k=p.y+j*d[p.i][1]; t.time=p.time+1;
			if(r>=1&&r<n&&k>=1&&k<m&&!a[r][k]&&!a[r][k+1]&&!a[r+1][k]&&!a[r+1][k+1])
			{
			if(b[r][k]>t.time) b[r][k]=t.time,t.x=r,t.y=k,t.i=p.i,q.push(t);//如果小於當前時間則壓入 
			}
			else break;
		    }
	     }
	}
}
int main(int argc, char *argv[])
{
	int i,j,k;
	while(cin>>n>>m&&(n||m))
	{
		for(i=1;i<=n;i++)
		for(j=1;j<=m;j++)
		{
			cin>>a[i][j]; b[i][j]=1000000;
		}
		cin>>sx>>sy>>ex>>ey>>c;
		if(c[0]=='n') p.i=0;
		if(c[0]=='e') p.i=1;
		if(c[0]=='s') p.i=2;
		if(c[0]=='w') p.i=3;
		bfs();
		if(b[ex][ey]==1000000) cout<<-1<<endl; else cout<<b[ex][ey]<<endl;
	}
	return 0;
}