POJ1556 The Doors 叉积+最短路

#include<cstdio>
#include<cstdlib>
#include<cmath>
#include<iostream>
#include<algorithm>
#include<cstring>
#include<vector>
#include<queue>
#define INF 0x3f3f3f3f
#define MAX 1005

double Map[MAX][MAX],dist[MAX];
int n,vis[MAX],G[MAX][MAX];//G储存点的序号
struct node
{
    double x[5],y[5];
    int len;
}point[MAX];

double Cross(double x1,double y1,double x2,double y2,double x3,double y3,double x4,double y4)//叉积
{
    double a=(x2-x1)*(y3-y1)-(x3-x1)*(y2-y1);
    double b=(x2-x1)*(y4-y1)-(x4-x1)*(y2-y1);
    return a*b;
}

int check(double x1,double y1,double x2,double y2,int pos1,int pos2)//判断两点的连线是否与某一段墙相交
{
    for(int i=pos1+1;i<pos2;i++)
    {
        if(Cross(x1,y1,x2,y2,point[i].x[1],0,point[i].x[1],point[i].y[1])<1e-10 && Cross(point[i].x[1],0,point[i].x[1],point[i].y[1],x1,y1,x2,y2)<1e-10)
            return 0;
        if(Cross(x1,y1,x2,y2,point[i].x[2],point[i].y[2],point[i].x[3],point[i].y[3])<1e-10 && Cross(point[i].x[2],point[i].y[2],point[i].x[3],point[i].y[3],x1,y1,x2,y2)<1e-10)
            return 0;
        if(Cross(x1,y1,x2,y2,point[i].x[4],point[i].y[4],point[i].x[1],10)<1e-10 && Cross(point[i].x[4],point[i].y[4],point[i].x[1],10,x1,y1,x2,y2)<1e-10)
            return 0;
    }
    return 1;
}

double Dist(double x1,double y1,double x2,double y2)//求两点间距离
{
    return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
}

double dij()//最短路
{
    int k;
    double minn;
    memset(vis,0,sizeof(vis));
    for(int i=1;i<=4*n+2;i++)
        dist[i]=Map[1][i];
    dist[1]=0;
    vis[1]=0;
    for(int i=1;i<4*n+2;i++)
    {
        minn=INF;
        for(int j=1;j<=4*n+2;j++)
        {
            if(minn>dist[j] && !vis[j])
            {
                k=j;
                minn=dist[j];
            }
        }
        vis[k]=1;
        for(int j=1;j<=4*n+2;j++)
        {
            if(dist[j] > Map[k][j]+dist[k])
                dist[j]=Map[k][j]+dist[k];
        }
    }
    return dist[4*n+2];
}

int main()
{
    int cnt;
    double x,y;
    while(scanf("%d",&n),n!=-1)
    {
        cnt=1;
        point[0].len=1;
        point[n+1].len=1;
        G[0][1]=1;
        point[0].x[1]=0;
        point[0].y[1]=5;
        point[n+1].x[1]=10;
        point[n+1].y[1]=5;
        G[n+1][1]=4*n+2;
        for(int i=0;i<MAX;i++)
        for(int j=0;j<MAX;j++)
        Map[i][j]=INF;
        for(int i=1;i<=n;i++)
        {
            scanf("%lf",&x);
            point[i].len=4;
            for(int j=1;j<=4;j++)
            {
                scanf("%lf",&y);
                point[i].x[j]=x;
                point[i].y[j]=y;
                G[i][j]=++cnt;
            }
        }
        for(int i=0;i<=n;i++)
        {
            for(int j=1;j<=point[i].len;j++)
            {
                x=point[i].x[j];
                y=point[i].y[j];
                for(int q=i+1;q<=n+1;q++)
                {
                    for(int f=1;f<=point[q].len;f++)
                    {
                        double x1=point[q].x[f];
                        double y1=point[q].y[f];
                        int op=check(x,y,x1,y1,i,q);
                        if(op)
                        {
                            int a=G[i][j];
                            int b=G[q][f];
                            Map[a][b]=Dist(x,y,x1,y1);
                        }
                    }
                }
            }
        }
        double ans=dij();
        printf("%.2lf\n",ans);
    }
    return 0;
}