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;
}