[POJ][3072][Robot]

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

题意：平面上有n个点，从一点到另一点需要转向的话就需要时间，求从第一点到第n点的最短时间。

注意：

1、转向角度<=180度。

2、计算转向角：

     例如：由P1经过P2到P3 

   　　 angle=fabs(atan2(p2.y-p1.y,p2.x-p1.x)【P2-P1向量的方向角】- atan2(p3.y-p2.y,p3.x-p2.x)【P3-P2向量的方向角】)

3、初始时，robot是面向第n点的。

#include<stdio.h>
#include<string.h>
#include<math.h>
//#include<conio.h>

const double PI=3.14159265358;
const double INF= 99999999999.9;
double r, x[31], y[31], cost[31][31], t;

double angtime(int i, int j, int u)
{
    double t;
    t=atan2(y[u]-y[i], x[u]-x[i])-atan2(y[j]-y[u], x[j]-x[u]);
    t = t<0?-180/PI*t:t/PI*180;
    return t<180?t:360-t;
}
double angtime2(int i, int j, int u)
{
    double t;
    t=atan2(y[i]-y[u], x[i]-x[u])-atan2(y[j]-y[u], x[j]-x[u]);
    t = t<0?-180/PI*t:t/PI*180;
    return t<180?t:360-t;
}
void dijkstra(int n,int v0)
{
    double mindis,dist[31];
    int s[31], path[31];
    int i,j,u;
    for(i=1;i<=n;i++)
    {
        dist[i]=cost[v0][i]+angtime2(n, i, 1);
        s[i]=0;
        if(cost[v0][i]<INF)
            path[i]=v0;
        else
            path[i]=-1;
    }
    s[v0]=1;path[v0]=0;
    for(i=1;i<=n;i++)
    {
        mindis=INF;
        u=-1;
        for(j=1;j<=n;j++)
        {
            if(s[j]==0&&dist[j]<mindis && dist[j]>0)
            {
                u=j;
                mindis=dist[j];
            }
        }
        if (u!=-1)s[u]=1;
        for(j=1;j<=n;j++)
        {
            if(s[j]==0 && path[u]>=1 && path[u]<=n)
                if(cost[u][j]<INF&&dist[u]+cost[u][j]+angtime(path[u], j, u)<dist[j])
                {
                    dist[j]=dist[u]+cost[u][j]+angtime(path[u], j, u);
                    path[j]=u;
                }
        }
    }
    if(dist[n]>=INF)
        printf("impossible\n");
    else
        printf("%d\n",(int)(dist[n]+0.5));
}
int main()
{
    //freopen("D:/a.txt", "r", stdin);
    int n, i, j;
    while (scanf("%lf%d", &r, &n))
    {
        if(n==-1 && r==-1)
            break;
        for (i=1;i<=30;i++)
            for (j=1;j<=30;j++)
                cost[i][j]=INF;
        for (i=1;i<=n;i++)
            scanf("%lf%lf", &x[i], &y[i]);
        for (i=1;i<=n;i++)
        {
            for (j=1;j<=n;j++)
            {
                t=sqrt((x[i]-x[j])*(x[i]-x[j])+(y[i]-y[j])*(y[i]-y[j]));
                if (t<=r) cost[i][j]=cost[j][i]=t;
            }
        }
        dijkstra(n, 1);
    }
    //getch();
    return 0;
}