POJ - 1556 The Doors

题目链接

求房间左边中点绕开障碍到右边中点的最短距离，即最短路。

#include<cstdio>
#include<cmath>
#include<cstring>
#include<algorithm>
using namespace std;
const int INF=1e8;
const int N=100;
double d[N][N];
struct point{
	double x,y;
	point(){}
	point(double sx,double sy):x(sx),y(sy){	};
	point operator-(const point &w)const{
		return point(x-w.x,y-w.y);
	}
	double operator*(const point &w)const{
		return x*w.x+y*w.y;
	}
}p[N];

double dist(point a,point b){  //两点距离 
	double res=sqrt((a-b)*(a-b));
	return res;
}

double cross(point a,point b){
	double res=a.x*b.y-b.x*a.y;
	return res;
}

bool judge(point a,point b,point c,point d){
	double res1=cross(c-a,d-a);
	double res2=cross(c-b,d-b);
	return res1*res2<0?true:false;
}

int main(){
	int n;
	while(~scanf("%d",&n)&&n!=-1){
		p[0]=point(0,5);
		double x,y1,y2,y3,y4;
		for(int i=0;i<n;i++){
			scanf("%lf%lf%lf%lf%lf",&x,&y1,&y2,&y3,&y4);
			p[i*4+1]=point(x,y1);
			p[i*4+2]=point(x,y2);
			p[i*4+3]=point(x,y3);
			p[i*4+4]=point(x,y4); 
		}
		p[n*4+1]=point(10,5);
		
		memset(d, 0, sizeof(d));
        for (int i = 0 ; i <= 4 * n + 1 ; i++) {
            for (int j = i + 1 ; j <= 4 * n + 1 ; j++) {
                int l = (i - 1) / 4, r = (j - 1) / 4;  //l是i所处的排数，r是j所在的排数 
                if (i == 0) l = -1;
                int flag = 1;
                for (int k = l + 1 ; k < r ; k++) {
                    if (judge(p[4 * k + 1],point(p[4 * k + 1].x, 0),p[i],p[j])) {
                        flag = 0;break;
                    }
                    if (judge(p[4 * k + 2],p[4 * k + 3],p[i],p[j])) {
                        flag = 0;break;
                    }
                    if (judge(p[4 * k + 4],point(p[4 * k + 4].x, 10),p[i],p[j])) {
                        flag = 0;break;
                    }
                }
                d[i][j] = d[j][i] = (flag?dist(p[i], p[j]):INF) ;
            }
        }
        for (int k = 0 ; k <= 4 * n + 1 ; k++)
            for (int i = 0 ; i <= 4 * n + 1 ; i++)
                for (int j = 0 ; j <= 4 * n + 1 ; j++)
                    d[i][j] = min(d[i][j], d[i][k] + d[k][j]);
        printf("%.2f\n", d[0][4 * n + 1]);	
	}
	return 0;
}