poj 3072 Robot 最短路spfa

题意：

给n个点，每个点有坐标（x,y），有个robot要从1走到n,求最短路径长度，所求最短路中每段路径长度不能超过R，而且要考虑每次转向的时间，这个时间是每转1度需要1的路径长度。

思路：

spfa算法求最短路，这题描述距离起点的距离函数dis一维不够用，故升到二维,dis[n][m]表示到点n,上一个点是m的最短路径长度，相应地两点之间的距离g[x][y]升到3三维，g[x][y][m]表示x之前经过点是m，从x到y的的距离。

代码：

//poj 3072
//sepNINE
#include <iostream>
#include <cmath> 
#include <queue>
const double pi=acos(-1.0);
const int maxN=32;
const double inf=100000;
const double eps=1e-8;
using namespace std;

struct Point{
	double x,y;	
}pnt[maxN];

struct Node{
	int ids,dir;
};

int n;
double g[maxN][maxN][maxN];
double dis[maxN][maxN];
int inq[maxN][maxN];
int dblcmp(double x)
{
	if(fabs(x)<eps) return 0;
	return x>0?1:-1;
}

double dist(Point a, Point b)
{
	double sum=(a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y);
	return sqrt(sum);	
}

double rotate(int i, int k, int j)
{
	if(i==j)
		return 0;
	if(i==k||j==k)
		return inf;
		
	double x1=pnt[k].x-pnt[i].x;
	double y1=pnt[k].y-pnt[i].y;
	double x2=pnt[j].x-pnt[i].x;
	double y2=pnt[j].y-pnt[i].y;
	x1=-x1;y1=-y1;
	double deta=atan2(y2,x2)-atan2(y1,x1);
	if(deta<0) deta+=2*pi;
	return min(deta,2*pi-deta)*180/pi;	
}

void spfa()
{
	queue<Node> Q;
	inq[1][0]=1;
	dis[1][0]=0;
	Node a;
	a.ids=1;
	a.dir=0;
	Q.push(a);
	while(!Q.empty()){
		Node node=Q.front();
		Q.pop();
		int u=node.ids,dir=node.dir;
		for(int v=1; v<=n; ++v){
			if(dblcmp(g[u][v][dir]-inf)<0&&dis[v][u]>dis[u][dir]+g[u][v][dir]){
				dis[v][u]=dis[u][dir]+g[u][v][dir];
				if(inq[v][u]==0){
					Node p;
					p.ids=v;
					p.dir=u;
					inq[v][u]=1;
					Q.push(p);
				}
			}
		}	
	}
}
int main()
{
	double R;
	while(scanf("%lf%d", &R, &n)==2&&n!=-1){
		int i,j,k;
		for(i=1; i<=n; ++i)
			scanf("%lf%lf", &pnt[i].x, &pnt[i].y);				
		pnt[0].x=2*pnt[1].x-pnt[n].x;
		pnt[0].y=2*pnt[1].y-pnt[n].y;	
		memset(inq, 0, sizeof(inq));
		for(i=0; i<=n; ++i)
			for(j=0; j<=n; ++j)
				dis[i][j]=inf;
		for(i=1; i<=n; ++i)
			for(j=1; j<=n; ++j)
			{
				double d=dist(pnt[i], pnt[j]);
				for(k=0; k<=n; ++k){
					if(dblcmp(d-R)>0)
						g[i][j][k]=inf;
					else
						g[i][j][k]=d+rotate(i,k,j);	
				}
			}
		spfa();				
		double ans=inf;			
		for(int i=0;i<=n;++i)
			ans=min(ans,dis[n][i]);
		if(dblcmp(inf-ans)==0)
			printf("impossible\n");	
		else
			printf("%d\n",(int)(ans+0.5));
	}
}