POJ 3072 Robot (最短路）

题目连接：http://poj.org/problem?id=3072

题意：在平面上，给出R,N,意思有N个点，点与点之间的权值就是两点之间的距离，机器人每次移动的距离不超过R。开始时机器人面向第N个点，当机器人要移动到点X(i,j)是，必须面向该点，如果此时没有面向南点X(i,j),就需要转移角度去面向它，需要的时间就是转移的角度，输出从1到N的最短时间，如果到达不了，输出impossible。

参考代码：

#include<iostream>
#include<cstdio>
#include<cstring>
#include<cmath>
#include<queue>
using namespace std;

#define N 205  //顶点的个数
#define SIZE 20005 //边的个数
#define INF 1e-10
#define CLR(arr,v) memset(arr,v,sizeof(arr))
const double INS = 1 << 29 ;
const double P = 180.0/acos(-1.0) ;


struct Point{
	double x,y;
}po[N];

double ang[N][N],pre[N];  //pre[i]存的是从父节点到i点的角度

void slove(int r,int n);
double dist(Point ,Point);
double deg(double ,double);

class short_path{
public:
	short_path(){}
	void init(int m,int n){
		num_ed = m;num_v = n;
		CLR(des,0);CLR(next,0); CLR(h,0);
		for(int i = 0;i < N;++i)
			dis[i] = INS;
		while(!q.empty()) q.pop();
	}
	void add(int u,int v,double f){
		des[++pos] = v;
		val[pos] = f;
		next[pos] = h[u];
		h[u] = pos;
	}
	int Dijkstra(int start,int end){
		double degree;
		dis[start] = 0;
		mid.d = start;
		mid.v = 0;
		q.push(mid);
		while(!q.empty()){
			int s = q.top().d;
			double v = q.top().v;
			q.pop();
			for(int i = h[s]; i ;i = next[i]){
				degree = deg(pre[s],ang[s][ des[i] ]);  //夹角
				if(dis[s] + val[i] + degree < dis[ des[i] ]){
					dis[ des[i] ] = dis[s] + val[i] + degree;
					mid.d = des[i];
					mid.v = dis[ des[i] ];
					pre[ des[i] ] = ang[s][ des[i] ];
					q.push(mid);
				}
			}
		}
		return fabs(dis[end]  - INS) < INF ? -1 : (int)(dis[end] + 0.5);
	}
private:
	int h[N],des[SIZE],next[SIZE];
	double dis[N],val[SIZE];
	int pos,num_ed,num_v,res;
	struct Edge{
		int d;
		double v;
		bool operator<(const Edge &ed) const{
			return v > ed.v;
		}
	};
	struct Edge mid;
	priority_queue<Edge> q;
}sp;

double dist(Point a,Point b){  //计算距离
	return sqrt((b.y - a.y)*(b.y - a.y) + (b.x - a.x)*(b.x - a.x));
}

double deg(double s,double e){ //计算夹角
	double sum = fabs(s - e) * P;
	if(360.0 - sum < INF ) sum -= 360.0;
	if(180.0 - sum < INF ) sum = 360.0 - sum;
	return sum;
}

void init(){
	CLR(ang,0); CLR(po,0);
	CLR(pre,0); 
}

void slove(double r,int n){
	sp.init(n,n);
	for(int i = 1;i <= n;++i)
		scanf("%lf%lf",&po[i].x,&po[i].y);
	double d ;
	for(int i = 1;i <= n;++i)
		for(int j = i + 1;j <= n;++j){
			ang[i][j] = atan2(po[i].y - po[j].y,po[i].x - po[j].x);
			ang[j][i] = atan2(po[j].y - po[i].y,po[j].x - po[i].x);
			d = dist(po[i],po[j]) ;
			if(d - r < INF){  //判断是否可走
				sp.add(i,j,d);
				sp.add(j,i,d);
			}
		}
	int res = -1;
	pre[1] = ang[1][n];
	res = sp.Dijkstra(1,n);
	if(res == -1) 
		printf("impossible\n");
	else 
		printf("%d\n",res);
}

int main()
{
	int n;
	double r;
	while(~scanf("%lf%d",&r,&n),(r > -1 && n > -1)){
		slove(r,n);
	}
	return 0;
}