计算几何基础 POJ - 1556 The Doors【抠关键点求最短路】

#include <iostream>
#include <algorithm>
#include <cstdio>
#include <cmath>
#include <cstring>
#include <queue>
using namespace std;
const int maxn=100;
const double eps=1e-8;
typedef long long ll;
struct point
{
    double x,y;
    point(double _X=0,double _Y=0)
    {
        x=_X;y=_Y;
    }
    point operator - (const point &oth)const
    {
        return point(x-oth.x,y-oth.y);
    }
        point operator + (const point &oth)const
    {
        return point(x+oth.x,y+oth.y);
    }
        point operator * (const double oth)const
    {
        return point(x*oth,y*oth);
    }
        point operator / (const double oth)const
    {
        return point(x/oth,y/oth);
    }
};
point p[maxn];
pair<point,point>seg[maxn];
int idx=0;
int idx2=0;
double g[maxn][maxn];
int dcmp(double x)
{
    if(fabs(x)<eps)return 0;
    else return x<0?-1:1;
}
double cross(point a,point b)
{
    return a.x*b.y-b.x*a.y;
}
bool segment(point a,point b,point c,point d)
{
    double c1 = cross(b-a,c-a),c2 = cross(b-a,d-a);
    double d1 = cross(d-c,a-c),d2 = cross(d-c,b-c);
    return dcmp(c1)*dcmp(c2)<0&&dcmp(d1)*dcmp(d2)<0;
}
point glt(point a,point a1,point b,point b1)
{
    point v= a1-a;
    point w=b1-b,u = a-b;
    double t = cross(w,u)/cross(v,w);
    return a+v*t;
}
bool check(point a,point b)
{
    for(int i=0;i<idx2;i++)
    {
        point c=seg[i].first;
        point d=seg[i].second;
        if(segment(a,b,c,d))return false;
    }
    return true;
}
double dist(point a,point b)
{
    double t=sqrt((a.x-b.x)*(a.x-b.x) +(a.y-b.y)*(a.y-b.y));
    //cout<<a.x<<" "<<a.y<<" "<<b.x<<" "<<b.y<<" "<<"dist="<< " "<<t<<endl;
    return t;
}
double d[maxn];
struct node
{
    int top;
    double dist;
    node(int s=0,double d=0)
    {
        top=s;
        dist=d;
    }
    bool operator<(const node&oth)const
    {
        return dcmp(dist-oth.dist)>0;
    }
};
double dijstra(int st)
{
    for(int i=0;i<=idx;i++) d[i]=1e18;
    priority_queue<node>pq;
    pq.push(node(st,0));
    d[st]=0;
    while(!pq.empty())
    {
        node tmp = pq.top();
        pq.pop();
        int u = tmp.top;
        double c =tmp.dist;
        for(int i=0;i<idx;i++)
        {
            if(dcmp(g[u][i])!=0)
            {
                int v= i;
                double w =g[u][i];
                if(dcmp(d[v]-d[u]-w)>0)
                {
                    d[v]=d[u]+w;
                    pq.push(node(v,w));
                }
            }
        }
    }
    return d[idx-1];
}
int main()
{
    //freopen("d://duipai//data.txt","r",stdin);
    //freopen("d://duipai//out1.txt","w",stdout);
    int n;
    while(scanf("%d",&n)&&(n!=-1))
    {
        memset(g,0,sizeof(g));
        memset(p,0,sizeof(p));
        idx=0;
        p[idx++]=point(0,5);
        idx2=0;
        for(int i=1;i<=n;i++)
        {
            double x,y;
            scanf("%lf",&x);
            point q = point(x,0);
            for(int j=1;j<=4;j++)
            {
                scanf("%lf",&y);
                p[idx]=point(x,y);
                if(j==1||j==3)
                seg[idx2++]=make_pair(q,p[idx]);
                q=p[idx];
                idx++;
            }
            seg[idx2++]=make_pair(q,point(x,10));
        }
        p[idx++]=point(10,5);
        for(int i=0;i<idx;i++)
        {
            for(int j=i+1;j<idx;j++)
            {
                 if(!dcmp(p[i].x-p[j].x))continue;
                 else if(check(p[i],p[j]))
                 {
                     g[i][j]=dist(p[i],p[j]);
                 }

            }
        }

        printf("%.2lf\n",dijstra(0));

    }
    return 0;
}