P3831 [SHOI2012]回家的路 分层最短路

#include<bits/stdc++.h>
using namespace std;
#define INF 0x7fffffff
const int maxm = 100000;
const int maxn = 200000;
int n,m,S,T;
struct NO{
	int x,y,id;
};
NO no[maxm];
struct Edge{
	int v,w;
	int id;
};
Edge edge[8*maxm];
int head[8*maxm],book[2*maxn];
int dis[2*maxn];
int cnt;
bool cmp2(NO a,NO b){
	if(a.y==b.y)return a.x<b.x;
	else return a.y<b.y;
}
bool cmp1(NO a,NO b){
	if(a.x==b.x)return a.y<b.y;
	else return a.x<b.x;
}

void add(int u,int v,int w){
	cnt++;
	edge[cnt].v = v;
	edge[cnt].w = w;
	edge[cnt].id = head[u];
	head[u] = cnt;
	cnt++;
	edge[cnt].v = u;
	edge[cnt].w = w;
	edge[cnt].id = head[v];
	head[v] = cnt;
}
void spfa()
{
	for(int i=1;i<=2*m+4;i++)dis[i] = INF;
	dis[S] = 0; book[S] = 1; queue<int> q; q.push(S);
	while(!q.empty()){
		int u = q.front(); book[u] = 0; q.pop();
		for(int v,w,i=head[u];i;i=edge[i].id){
			v = edge[i].v; w = edge[i].w+dis[u];
			if(dis[v]>w){
				dis[v] = w; 
				if(!book[v])book[v]=1,q.push(v); 
			}
		}
	}
 } 
int main()
{
	scanf("%d %d",&n,&m);
	S = m+1; T = m+2;
	for(int i=1;i<=m+2;i++){
		scanf("%d %d",&no[i].x,&no[i].y);
		no[i].id = i;
	}
            //横向图
	sort(no+1,no+m+3,cmp1);
	for(int i=1;i<m+2;i++)
	if(no[i].x==no[i+1].x)add(no[i].id,no[i+1].id,2*(no[i+1].y-no[i].y));
	
            //纵向图
	sort(no+1,no+m+3,cmp2);
	for(int i=1;i<m+2;i++)
	if(no[i].y==no[i+1].y)add(no[i].id+m+2,no[i+1].id+m+2,2*(no[i+1].x-no[i].x));
	
            //两层图之间路径为1
	for(int i=1;i<=m;i++)
	 add(i,i+m+2,1);
            //起点终点为0
	add(m+1,2*m+3,0); add(m+2,2*m+4,0);
	
            //普通spfa
	spfa();
	if(dis[T]==INF){
		printf("-1");
	}else printf("%d",dis[T]);
	
	return 0;
}