【最短路】【Heap-dijkstra】Gym - 101147B - Street

#include<cstdio>
#include<algorithm>
#include<queue>
#include<cmath>
#include<cstring>
using namespace std;
#define EPS 0.00000001
#define N 109
int T,n,H,X;
int hs[N],xs[N],ds[N];
bool is[N];
double sqr(int x)
{
	return (double)x*(double)x;
}
int e,v[N*N*4],first[N],next[N*N*4];
double w[N*N*4];
void AddEdge(int U,int V,double W)
{
	v[++e]=V;
	w[e]=W;
	next[e]=first[U];
	first[U]=e;
}
double d[N];
bool vis[N];
struct Point{double d;int u;
Point(const double &X,const int &Y){d=X;u=Y;}
Point(){}};
bool operator < (Point a,Point b){return a.d>b.d;}
priority_queue<Point>q;
void dijkstra(int S)
{
    for(int i=1;i<=n+2;++i) d[i]=1000000007.0;
    d[S]=0; q.push(Point(0.0,S));
    while(!q.empty())
      {
        Point x=q.top(); q.pop();
        if(!vis[x.u])
          {
            vis[x.u]=1;
            for(int i=first[x.u];i;i=next[i])
              if(d[v[i]]-d[x.u]-w[i]>EPS)
                {
                  d[v[i]]=d[x.u]+w[i];
                  q.push(Point(d[v[i]],v[i]));
                }
          }
      }
}
int main()
{
	freopen("street.in","r",stdin);
	scanf("%d",&T);
	for(;T;--T)
	  {
	  	memset(v,0,sizeof(v));
	  	memset(first,0,sizeof(first));
	  	memset(w,0,sizeof(w));
	  	memset(next,0,sizeof(next));
	  	memset(vis,0,sizeof(vis));
	  	e=0;
	  	scanf("%d%d%d",&n,&H,&X);
	  	for(int i=1;i<=n;++i)
	  	  scanf("%d%d%d%d",&hs[i],&xs[i],&ds[i],&is[i]);
	  	for(int i=1;i<=n;++i)
	  	  for(int j=i+1;j<=n;++j)
	  	    if(is[i]==is[j] || (is[i]!=is[j] && xs[i]+xs[j]>X))
	  	      {
	  	      	AddEdge(i,j,ds[j]-(ds[i]+hs[i])>=0 ? ds[j]-(ds[i]+hs[i]) : ds[i]-(ds[j]+hs[j]));
	  	      	AddEdge(j,i,ds[j]-(ds[i]+hs[i])>=0 ? ds[j]-(ds[i]+hs[i]) : ds[i]-(ds[j]+hs[j]));
	  	      }
	  	    else if((ds[i]+hs[i]>=ds[j] && ds[i]<=ds[j]+hs[j]) ||
			  		(ds[j]+hs[j]>=ds[i] && ds[j]<=ds[i]+hs[i]))
	  	      {
	  	      	AddEdge(i,j,X-(xs[i]+xs[j]));
	  	      	AddEdge(j,i,X-(xs[i]+xs[j]));
	  	      }
	  	    else if(ds[i]+hs[i]<ds[j])
	  	      {
	  	      	AddEdge(i,j,sqrt(sqr(ds[j]-(ds[i]+hs[i]))+sqr(X-(xs[i]+xs[j]))));
	  	      	AddEdge(j,i,sqrt(sqr(ds[j]-(ds[i]+hs[i]))+sqr(X-(xs[i]+xs[j]))));
	  	      }
	  	    else
	  	      {
	  	      	AddEdge(i,j,sqrt(sqr(ds[i]-(ds[j]+hs[j]))+sqr(X-(xs[i]+xs[j]))));
	  	      	AddEdge(j,i,sqrt(sqr(ds[i]-(ds[j]+hs[j]))+sqr(X-(xs[i]+xs[j]))));
	  	      }
	  	for(int i=1;i<=n;++i)
	  	  {
	  	  	AddEdge(n+1,i,ds[i]);
	  	  	AddEdge(i,n+1,ds[i]);
	  	  	AddEdge(i,n+2,H-(ds[i]+hs[i]));
	  	  	AddEdge(n+2,i,H-(ds[i]+hs[i]));
	  	  }
	  	dijkstra(n+1);
	  	printf("%.6lf\n",d[n+2]);
	  }
	return 0;
}