Data Structures and Algorithms (English) 7-11 Saving James Bond - Hard Version （30 分）

#include 

using namespace std;

#define INFINITY 65535	/*无穷大设为双字节无符号整数的最大值*/ 
#define MAXNUMBER 100 /*鳄鱼最大数设为100*/
#define RADIUS 50 /*湖的半径*/
#define ISLAND 7.5 /*岛的半径*/ 

struct position{
	int x;
	int y;
}Crocodiles[MAXNUMBER];

double Distance(int x1, int y1, int x2, int y2); /*计算两个位置之间的距离*/ 
void Initialization(); /*初始化*/
void BFS(int s); /*广度优先算法*/
 
int head;
int N, D, flag;
int visited[100];
int path[100];
int layer[100];

int main()
{
	#ifdef ONLINE_JUDGE
	#else
	freopen("in.txt", "r", stdin);
	#endif
	
	cin >> N >> D;
	
	for(int i = 0; i < N; i++){
		cin >> Crocodiles[i].x >> Crocodiles[i].y;
	}
	
	if(D >= RADIUS - ISLAND){
		cout << "1" << endl;
		return 0;
	} 
	
	int FirstJump[N];
	int CouldFirst = 0;
	
	for(int i = 0; i < N; i++){
		if((D + ISLAND) >= Distance(Crocodiles[i].x, Crocodiles[i].y, 0, 0)){
			
			FirstJump[CouldFirst++] = i;
	
		}
	}
	
	if(CouldFirst == 0){
		cout << "0" << endl;
		return 0;
	}
	
	int min = INFINITY;
	vector minimum;
	
	for(int i = 0; i < CouldFirst; i++){
		
		Initialization();
		
		BFS(FirstJump[i]);
		if(flag == 1){
			if(layer[head] < min){
				min = layer[head];
				minimum.clear();
				minimum.push_back(FirstJump[i]);
			}
			else if(layer[head] == min){
				minimum.push_back(FirstJump[i]);
			}
		}
	}
	
	if(minimum.empty()){
		
		printf("0");
		return 0;

	}

	
	min = INFINITY;
	int min_index;
	
	for(int i = 0; i < minimum.size(); i++){
		
		int index = minimum[i];
		
		if(min > Distance(Crocodiles[index].x, Crocodiles[index].y, 0, 0) ){
			min = Distance(Crocodiles[index].x, Crocodiles[index].y, 0, 0);
			min_index = index;
		} 
	}
	
	Initialization();
	BFS(min_index);
	cout << layer[head] << endl;
	
	stack s;
	do{
		s.push(head);
		head = path[head];
	}while(head != -2);
	
	while(!s.empty()){
		int tmp = s.top();
		s.pop();
		cout << Crocodiles[tmp].x << " " << Crocodiles[tmp].y << endl;
	} 
	
	return 0;
}

double Distance(int x1, int y1, int x2, int y2)
{
	return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
} 

void BFS(int s)
{	
	visited[s] = 1;
	path[s] = -2;
	queue q;
	q.push(s);
	layer[s] = 1;
	
	while(!q.empty()){
		
		head = q.front();
		q.pop(); 
		
		if(RADIUS - abs(Crocodiles[head].x) <= D || RADIUS - abs(Crocodiles[head].y) <= D){
			layer[head]++;
			flag = 1;
			break;
		}
		
		for(int i = 0; i < N; i++){
			if(visited[i] == 0 && Distance(Crocodiles[head].x, Crocodiles[head].y, Crocodiles[i].x, Crocodiles[i].y) <= D){
				path[i] = head;
				layer[i] = layer[head] + 1;
				visited[i] = 1;
				q.push(i);	
			}
		} 
	}
} 

void Initialization()
{
	flag = 0;
	
	for(int i = 0; i < N; i++){
		visited[i] = 0;
		path[i] = 0;
		layer[i] = 0;
	}
}