Poj Solution 1685

http://poj.org/problem?id=1685

#include<iostream>
#include"math.h"
#include"stdio.h"
using namespace std;
double x[60][2],y[60][2],xs,ys,xt,yt;

double best1[62][2],best2[62][2],len[62];
int color[60],cl,c[62];

inline double  jl(double a,double c,double b,double d)
{return sqrt((a-b)*(a-b)+(c-d)*(c-d));}

int main()
{int t,i,j,k,n;double ss,tt;
    cin>>t;
    for(;t>0;t--)
    {cin>>xs>>ys>>xt>>yt;
    cin>>cl;
    for(i=0;i<cl;i++)cin>>color[i];
    cin>>n;
    for(i=0;i<n;i++){cin>>x[i][0]>>y[i][0]>>x[i][1]>>y[i][1]>>c[i];
                len[i]=jl(x[i][0],y[i][0],x[i][1],y[i][1]);}

    for(i=0;i<n;i++)
        if(c[i]==color[0]){best1[i][0]=jl(xs,ys,x[i][0],y[i][0])+len[i];
                            best1[i][1]=jl(xs,ys,x[i][1],y[i][1])+len[i];}
        else best1[i][0]=best1[i][1]=999999999999;

    
    for(i=1;i<cl;i++)
    {for(j=0;j<n;j++){best2[j][0]=best1[j][0];best2[j][1]=best1[j][1];}
     for(j=0;j<n;j++)
     {best1[j][0]=9999999999;best1[j][1]=9999999999999;
        if(c[j]==color[i])
        
            for(k=0;k<n;k++)if(c[k]==color[i-1]){
                ss=best2[k][0]+jl(x[k][1],y[k][1],x[j][0],y[j][0]);
                tt=best2[k][1]+jl(x[k][0],y[k][0],x[j][0],y[j][0]);
                if(tt<ss)ss=tt;
                
                if(ss+len[j]<best1[j][0])best1[j][0]=ss+len[j];
                
                ss=best2[k][0]+jl(x[k][1],y[k][1],x[j][1],y[j][1]);
                tt=best2[k][1]+jl(x[k][0],y[k][0],x[j][1],y[j][1]);
                if(tt<ss)ss=tt;
                
                if(ss+len[j]<best1[j][1])best1[j][1]=ss+len[j];
                                                }

     }
    }
    double answer=2000000000;
    for(i=0;i<n;i++)
        if(c[i]==color[cl-1]){tt=best1[i][0]+jl(x[i][1],y[i][1],xt,yt);
                              ss=best1[i][1]+jl(x[i][0],y[i][0],xt,yt);    
                            if(tt<ss)ss=tt;
                            if(answer>ss)answer=ss;
                        }
    
    printf("%.3fn",answer);
    }
return 1;
}