Street Gym - 101147B

https://cn.vjudge.net/problem/Gym-101147B 

#include <iostream>
#include <cstring>
#include <queue>
#include <vector>
#include <algorithm>
#include <cstdio>
#include <set>
#include <map>
#include <stack>
//#include <tr1/unordered_map>
//#include <unordered_map>
#include <cmath>
//#include<bits/stdc++.h>
using namespace std;

#define sfi(i) scanf("%d",&i)
#define sfl(i) scanf("%lld",&i)
#define sfs(i) scanf("%s",(i))
#define pri(i) printf("%d\n",i)
#define prl(i) printf("%I64d\n",i)
#define sff(i) scanf("%lf",&i)
#define ll long long
#define ull unsigned long long
#define uint unsigned int
#define mem(x,y) memset(x,y,sizeof(x))
#define INF 0x3f3f3f3f
#define inf 1e18
#define eps 1e-10
#define PI acos(-1.0)
#define lowbit(x) ((x)&(-x))
#define fl() printf("flag\n")
#define MOD(x) ((x%mod)+mod)%mod
#define endl '\n'
#define pb push_back
#define lson (rt<<1)
#define rson (rt<<1|1)
#define FAST_IO ios::sync_with_stdio(false);cin.tie(0);cout.tie(0)

template<typename T>inline void read(T &x)
{
    x=0;
    static int p;p=1;
    static char c;c=getchar();
    while(!isdigit(c)){if(c=='-')p=-1;c=getchar();}
    while(isdigit(c)) {x=(x<<1)+(x<<3)+(c-48);c=getchar();}
    x*=p;
}

//-----------------------------------------------
const int maxn=2e5+9;
const int mod=1e9+7;

int n;
double U,L;
double d[maxn];

struct node
{
    int to;
    double w;
    bool operator < (const node &x) const
    {
        return w<x.w;
    }
    node(int _to,double _w)
    {
        to=_to;
        w=_w;
    }
};

vector<node> g[maxn];

void addedge(int u,int v,double w)
{
    g[u].pb({v,w});
    g[v].pb({u,w});
}

struct M
{
    double h,w,d;
    int k;
}a[maxn];

double Dis(double len1,double len2)
{
    return sqrt(len1*len1+len2*len2);
}

void join(int x, int y)
{
    double dis=0;
    if(a[x].k==a[y].k||a[x].w+a[y].w>U)
    {
        dis=max(a[x].d-a[y].d-a[y].h,a[y].d-a[x].d-a[x].h);
    }
    else
    {
        if(a[x].d>a[y].d+a[y].h) dis=Dis(U-a[x].w-a[y].w,a[x].d-a[y].d-a[y].h);
        else if(a[y].d>a[x].d+a[x].h) dis=Dis(U-a[x].w-a[y].w,a[y].d-a[x].d-a[x].h);
        else dis=U-a[x].w-a[y].w;
    }

    //cout<<a[x].w<<" "<<a[y].w<<" "<<dis<<endl;

    addedge(x,y,dis);
}

void djs(int s)
{
    priority_queue<node>q;
    for(int i=0;i<=n+1;i++) d[i]=INF;

    d[s]=0;
    q.push(node({s,d[s]}));
    while(!q.empty())
    {
        node tmp=q.top();
        q.pop();

        int u=tmp.to;

        for(int i=0;i<g[u].size();i++)
        {
            int v=g[u][i].to;
            double w=g[u][i].w;

            if(d[v]>d[u]+w)
            {
                d[v]=d[u]+w;
                q.push(node(v,d[v]));
            }

        }
    }
}

void init()
{
    for(int i=0;i<=n+1;i++) g[i].clear();
}

int main()
{
    //FAST_IO;

    //freopen("input.txt","r",stdin);
    freopen("street.in","r",stdin);

    int T;
    //cin>>T;
    sfi(T);
    while(T--)
    {

        //cin>>n>>L>>U;
        scanf("%d%lf%lf", &n, &L, &U);

        init();
        for(int i=1;i<=n;i++)
        {
            //cin>>a[i].h>>a[i].w>>a[i].d>>a[i].k;
            scanf("%lf%lf%lf%d", &a[i].h, &a[i].w, &a[i].d, &a[i].k);
        }

        for(int i=1;i<=n;i++)
        {
            addedge(0,i,a[i].d);
            addedge(n+1,i,L-a[i].d-a[i].h);
        }

        for(int i=1;i<n;i++)
        {
            for(int j=i+1;j<=n;j++)
            {
                join(i,j);
            }
        }

        for(int i=1;i<=n;i++)
        {
            for(int j=0;j<g[i].size();j++)
            {
                //cout<<i<<" "<<g[i][j].to<<" "<<g[i][j].w<<endl;
            }
        }

        djs(0);
        printf("%.6lf\n",d[n+1]);
    }


    return 0;
}