Cockchaf.java

import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Queue;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.HashMap;
 
import javax.management.Query;
import javax.print.attribute.HashDocAttributeSet;
 
 
public class Cockchaf {
	private static double countTime(Bod start, Bod cil, int speed, int anglespeed, int ax, int ay, int az){
		int xdiff = start.x - cil.x;
		int ydiff = start.y - cil.y;
		int zdiff = start.z - cil.z;
		double distance = Math.sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
		double alfa = 180 - Math.toDegrees(Math.acos( (ax*cil.x + ay*cil.y + az*cil.z) / (Math.sqrt(ax*ax + ay*ay + az*az)*Math.sqrt(cil.x*cil.x + cil.y*cil.y + cil.z*cil.z)) ));
 
		System.out.println((distance / speed) + (alfa / anglespeed));
		return (distance / speed) + (alfa / anglespeed);
 
	}
 
	public static void main(String[] args) throws Exception {
		BufferedReader br = new BufferedReader(new InputStreamReader(System.in));
		String str;
		String[] radek;
		int n,s,t;
		int[] start = new int[3];
		int[] konec = new int[3];
		int[] spoje;
		HashMap<Bod,List<Bod>> map = new HashMap<Bod,List<Bod>>();
		while((str=br.readLine()) != null){
			radek = str.split(" ");
			n = Integer.parseInt(radek[0]);
			s = Integer.parseInt(radek[1]);
			t = Integer.parseInt(radek[2]);
			spoje = new int[6];
 
			str = br.readLine();
			radek = str.split(" ");
			start[0] = Integer.parseInt(radek[0]);
			start[1] = Integer.parseInt(radek[1]);
			start[2] = Integer.parseInt(radek[2]);
 
			konec[0] = Integer.parseInt(radek[3]);
			konec[1] = Integer.parseInt(radek[4]);
			konec[2] = Integer.parseInt(radek[5]);
			Bod startpoint = new Bod (start[0], start[1], start[2]);
			Bod endpoint = new Bod( konec[0], konec[1], konec[2]);			
 
 
			PriorityQueue<Bod> q = new PriorityQueue<Bod>();
			q.add(startpoint);
 
			for(int i=0; i<n; i++){
				str = br.readLine();
				radek = str.split(" ");
				spoje[0] = Integer.parseInt(radek[0]);
				spoje[1] = Integer.parseInt(radek[1]);
				spoje[2] = Integer.parseInt(radek[2]);
				spoje[3] = Integer.parseInt(radek[3]);
				spoje[4] = Integer.parseInt(radek[4]);
				spoje[5] = Integer.parseInt(radek[5]);
 
				Bod st = new Bod(spoje[0],spoje[1],spoje[2]);
				Bod f = new Bod(spoje[3],spoje[4],spoje[5]);
 
				if(map.containsKey(s)){
					map.get(st).add(f);
				}
				else{
					LinkedList<Bod> l = new LinkedList<Bod>();
					l.add(f);
 
					if(map.containsKey(f)){
 
					}
					map.put(st,l);
				}
 
				if(map.containsKey(f)){
					map.get(f).add(st);
				}else{
					LinkedList<Bod> l = new LinkedList<Bod>();
					l.add(st);
					map.put(f,l);
				}
			}
 
			boolean firstiter = true;
			while(!q.isEmpty()){
				Bod bod = q.remove();				
 
				for(Bod bods: map.get(bod)){
					if(firstiter){
						bod.ix = bods.x - bod.x;
						bod.iy = bods.y - bod.y;
						bod.iz = bods.z - bod.z;
						firstiter = false;
					}
 
					double time = countTime(bod, bods, s, t, bod.ix, bod.iy, bod.iz );
					bods.time = time < bods.time ? time : bods.time;
					q.add(bods);
 
					System.out.println(bods);
 
					if(bods.equals(endpoint)){
						endpoint = bods;
						q.clear();
						break;
					}
				}
			}
 
			System.out.println(endpoint.time);
		}
 
		br.close();
	}
 
 
}
 
class Bod implements Comparable<Bod>{
	int x,y,z;
	int ix, iy, iz;
	double time;
	public Bod(int x,int y, int z){
		this.x = x;
		this.y = y;
		this.z = z;
	}
	@Override
	public int hashCode() {
		final int prime = 31;
		int result = 1;
		result = prime * result + x;
		result = prime * result + y;
		result = prime * result + z;
		return result;
	}
 
	@Override
	public int compareTo(Bod b){
		return (int)(this.time - b.time);
	}
 
	@Override
	public boolean equals(Object obj) {
		if (this == obj)
			return true;
		if (obj == null)
			return false;
		if (getClass() != obj.getClass())
			return false;
		Bod other = (Bod) obj;
		if (x != other.x)
			return false;
		if (y != other.y)
			return false;
		if (z != other.z)
			return false;
		return true;
	}
	@Override
	public String toString() {
		return "Bod [x=" + x + ", y=" + y + ", z=" + z
				+" ix: "+ix+" iy "+iy+" iz "+iz +", time=" + time + "]";
	}
 
 
}