几何建图(矩形)

#include <bits/stdc++.h>
using namespace std;
double d[109][109];
struct node{
    double x,y,t;
    int city;
}a[409];
double cheng[109];
int n,s,t,q,w;
double dis(int l,int r){
    return sqrt((a[l].x-a[r].x)*(a[l].x-a[r].x)+(a[l].y-a[r].y)*(a[l].y-a[r].y));
}
int idis(int l,int r){
    return (a[l].x-a[r].x)*(a[l].x-a[r].x)+(a[l].y-a[r].y)*(a[l].y-a[r].y);
}
void getrce(int x1,int y1,int x2,int y2,int x3,int y3,int w)
{
    int k=w-1;
    int ab=idis(k-2,k-1),ac=idis(k-2,k),bc=idis(k-1,k);
    double x4,y4;
    if(ab+ac==bc)    x4=x2+x3-x1,y4=y2+y3-y1;
    if(ab+bc==ac)    x4=x3+x1-x2,y4=y3+y1-y2;
    if(bc+ac==ab)    x4=x1+x2-x3,y4=y1+y2-y3;
    k++;
    a[k].x=x4,a[k].y=y4,a[k].city=a[w-1].city;
}
int main()
{
    cin>>n;
    while(n--)
    {
        int T;
        cin>>s>>T>>q>>w;
        double inf=99999999;
        for(int i=0;i<=100;i++)
        for(int j=0;j<=100;j++)
        {
        if(i==j)    d[i][j]=0;
        else d[i][j]=inf;
        }
        for(int i=1;i<=s;i++)
        {
            for(int j=1;j<=3;j++)
            {
                int k=4*(i-1)+j;
                a[k].city=i;
                cin>>a[k].x>>a[k].y;
            } 
            int k=i*4;
            cin>>cheng[i];
            getrce(a[k-3].x,a[k-3].y,a[k-2].x,a[k-2].y,a[k-1].x,a[k-1].y,k);
        }
        for(int i=1;i<=s*4;i++)
        {
            for(int j=1;j<=s*4;j++)
            {
                if(i==j)    continue;
                double k=dis(i,j);
                if(a[i].city==a[j].city)
                    d[i][j]=d[j][i]=k*cheng[a[i].city];
                else d[i][j]=d[j][i]=k*T;
            }
        }
        for(int k=1;k<=s*4;k++)
        {
            for(int i=1;i<=s*4;i++)//弗洛伊德 
            {
                if(i==k)    continue;
                for(int j=1;j<=s*4;j++)
                {
                    if(j==k||j==i)    continue;
                    if(d[i][k]+d[k][j]<d[i][j])
                        d[i][j]=d[i][k]+d[k][j];
                }
            }
        }
        double ans=99999999;
        for(int i=(q-1)*4+1;i<=q*4;i++)
            for(int j=(w-1)*4+1;j<=w*4;j++)
                ans=min(ans,d[i][j]);
        printf("%.1lf\n",ans);
    }
}