B题90分求助

#include<cstring>
#include<cstdio>
#include<queue>
#include<map>
#include<iostream>
#include<cmath>
#define DBL_MAX 1.7976931348623158e+308 /* max value */
using namespace std;

int n,m,k,cnt,head[40100];

struct point{
    int x;
    int y;
    point(int x=0,int y=0):x(x),y(y){};
    bool operator <(const point &a)const
    {
        if (a.x==x)return a.y<y;
		return a.x<x;
    }
}s,e;

point operator - (point a,point b)
{
    return  point(a.x-b.x,a.y-b.y);
}
map <point,int> mp;
struct ***
{
	int y;
	double val;
	int next;
}a[40100];

struct line{
    point a;
    point b;
}kk[510];

void add(int x,int y,double val)
{
	cnt++;
	a[cnt].y=y;
	a[cnt].val=val;
	a[cnt].next=head[x];
	head[x]=cnt;
	//printf("%d %d\n",x,y);
}

double cross(point a,point b)
{
    return a.x*b.y-a.y*b.x;
}

int dcmp(double x)
{
    if(fabs(x)<0.000001) return 0;
    else return x<0?-1:1;
}



bool judge(point a1,point a2)
{
	for (int i=1;i<=k;i++)
    {
        point b1=kk[i].a;
        point b2=kk[i].b;
        double c1=cross(a2-a1,b1-a1),c2=cross(a2-a1,b2-a1),c3=cross(b2-b1,a1-b1),c4=cross(b2-b1,a2-b1);
        if (dcmp(c1)*dcmp(c2)<0&&dcmp(c3)*dcmp(c4)<0) return 0;
    }
    return 1;
}

double dis(point a,point b)
{
    return sqrt((double)(b.y-a.y)*(double)(b.y-a.y)+(double)(b.x-a.x)*(double)(b.x-a.x));
}

int cont;
void push(point x)
{
    mp[x]=++cont;
}

double f[100010];
bool v[100010];

void spfa(int s)
{
	queue <int> q;
	for (int i=1;i<=cont;i++)
	{
		f[i]=DBL_MAX;
	}
	q.push(s);
	v[s]=1; f[s]=0;
	while (!q.empty())
	{
		int x=q.front(); q.pop();
		v[x]=0;
		for (int i=head[x];i;i=a[i].next)
		{
			int y=a[i].y;
			double val=a[i].val;
			if (f[y]>f[x]+val)
			{
				f[y]=min(f[y],f[x]+val);
				if (v[y]==0)
				{
					q.push(y);
					v[y]=1;
				}
			}
		}
	}
}

int main()
{
    scanf("%d%d%d",&n,&m,&k);
    for (int i=1;i<=k;i++)
    {
        scanf("%d%d%d%d",&kk[i].a.x,&kk[i].a.y,&kk[i].b.x,&kk[i].b.y);
        push(kk[i].a);
        if (kk[i].a.x==kk[i].b.x&&kk[i].a.y==kk[i].b.y) continue;
        push(kk[i].b);
       
    }
    scanf("%d%d%d%d",&s.x,&s.y,&e.x,&e.y);
    push(s);
    push(e);

	if (judge(s,e))
    {
        printf("%.4f",dis(s,e));
        return 0;
    }

    for (int i=1;i<=k;i++)
	{
        if (judge(s,kk[i].a))
        {
            int x=mp[s];
            int y=mp[kk[i].a];
            double d=dis(s,kk[i].a);
            add(x,y,d);
            add(y,x,d);
        }
        if (judge(s,kk[i].b))
        {
            int x=mp[s];
            int y=mp[kk[i].b];
            double d=dis(s,kk[i].b);
            add(x,y,d);
            add(y,x,d);
        }
    }

    for (int i=1;i<=k;i++)
    {
        if (judge(e,kk[i].a))
        {
            int x=mp[e];
            int y=mp[kk[i].a];
            double d=dis(e,kk[i].a);
            add(x,y,d);
            add(y,x,d);
        }

        if (judge(e,kk[i].b))
        {
            int x=mp[e];
            int y=mp[kk[i].b];
            double d=dis(e,kk[i].b);
            add(x,y,d);
            add(y,x,d);
        }
    }

    for (int i=1;i<=k;i++)
    {
        int x=mp[kk[i].a];
        int y=mp[kk[i].b];
        double d=dis(kk[i].a,kk[i].b);
        add(x,y,d);
        add(y,x,d);
        for (int j=i+1;j<=k;j++)
        {
            if (judge(kk[i].a,kk[j].a))
            {
                int x=mp[kk[i].a];
                int y=mp[kk[j].a];
                //if (x==y) continue;
                double d=dis(kk[i].a,kk[j].a);
                add(x,y,d);
                add(y,x,d);
            }

            if (judge(kk[i].a,kk[j].b))
            {
                int x=mp[kk[i].a];
                int y=mp[kk[j].b];
                //if (x==y) continue;
                double d=dis(kk[i].a,kk[j].b);
                add(x,y,d);
                add(y,x,d);
            }
            if (judge(kk[i].b,kk[j].a))
            {
                int x=mp[kk[i].b];
                int y=mp[kk[j].a];
                //if (x==y) continue;
                double d=dis(kk[i].b,kk[j].a);
                add(x,y,d);
                add(y,x,d);
            }

            if (judge(kk[i].b,kk[j].b))
            {
                int x=mp[kk[i].b];
                int y=mp[kk[j].b];
                //if (x==y) continue;
                double d=dis(kk[i].b,kk[j].b);
                add(x,y,d);
                add(y,x,d);
            }
        }
    }

    spfa(mp[s]);
    printf("%.4f",f[mp[e]]);

}