Cockchaf.java

 
import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.Scanner;
import java.util.Set;
 
 
 
/**
 *
 * @author cteam018
 */
public class Cockchaf {
    static class Step implements Comparable<Step>{
        Point p;
        double dx,dy,dz;
        double time;
 
        Step (Point p_, double dx_, double dy_, double dz_,double time_){
            p = p_;
            dx = dx_;
            dy = dy_;
            dz = dz_;
            time = time_;
        }
 
        public int compareTo(Step s) {
            double e = 0.0000001;
            if (s.time-time > e) return -1;
            if (time-s.time > e) return 1;
            return 0;
        }
    }
 
    static class Point {
 
        @Override
        public boolean equals(Object obj) {
            if (obj == null) {
                return false;
            }
            if (getClass() != obj.getClass()) {
                return false;
            }
            final Point other = (Point) obj;
            if (this.x != other.x) {
                return false;
            }
            if (this.y != other.y) {
                return false;
            }
            if (this.z != other.z) {
                return false;
            }
            return true;
        }
 
        @Override
        public int hashCode() {
            int hash = 3;
            hash = 47 * hash + this.x;
            hash = 47 * hash + this.y;
            hash = 47 * hash + this.z;
            return hash;
        }
 
        int x,y,z;
        List<Point> e = new ArrayList<Point>();
 
        Point(int x,int y, int z){
            this.x = x;
            this.y = y;
            this.z = z;
 
        }
 
    };
 
    static double length(double x1, double x2, double x3, double y1, double y2, double y3){
        return Math.sqrt((x1-y1)*(x1-y1)+(x2-y2)*(x2-y2)+(x3-y3)*(x3-y3));
    }
 
    static double degToTurn(double u1,double u2,double u3,double v1,double v2,double v3){
        double d;
        d = Math.acos((u1*v1+u2*v2+u3*v3)/(length(0,0,0,u1,u2,u3)*length(0,0,0,v1,v2,v3)));
        return d*180/Math.PI;
    }
 
    public static void main(String[] args) {
        Scanner sc = new Scanner(System.in);
        while (sc.hasNextInt()){
            int N = sc.nextInt();
            int S = sc.nextInt();
            int T = sc.nextInt();
            Point start = new Point(sc.nextInt(),sc.nextInt(),sc.nextInt());
            Point end = new Point(sc.nextInt(),sc.nextInt(),sc.nextInt());
            List<Point> points = new ArrayList<Point>();
            points.add(start);
            points.add(end);
            for (int i=0; i<N; i++){
                Point p1 = new Point(sc.nextInt(),sc.nextInt(),sc.nextInt());
                Point p2 = new Point(sc.nextInt(),sc.nextInt(),sc.nextInt());
                Point p1R = null, p2R = null;
                for (int j=0; j<points.size(); j++){
                    if (points.get(j).equals(p1)){
                        p1R = points.get(j);
                    }
                    if (points.get(j).equals(p2)){
                        p2R = points.get(j);
                    }
                    if (p1R != null && p2R != null) break;
                }
                if (p1R == null){
                    p1R = p1;
                    points.add(p1R);
                }
                if (p2R == null){
                    p2R = p2;
                    points.add(p2R);
                }
 
                p1R.e.add(p2R);
                p2R.e.add(p1R);
            }
 
            Queue<Step> q = new PriorityQueue<Step>();
            //init
            for (int i=0; i<start.e.size(); i++){
                double d = length(start.x, start.y, start.z, start.e.get(i).x, start.e.get(i).y, start.e.get(i).z);
                double dx = start.e.get(i).x - start.x;
                double dy = start.e.get(i).y - start.y;
                double dz = start.e.get(i).z - start.z;
                q.add(new Step(start.e.get(i), dx, dy, dz, d/S));
                System.err.println(d/S);
            }
 
            while (true) {
                Step c = q.poll();
                if (c.p.equals(end)){
                    System.out.printf("%.4f\n", c.time);
                    break;
                }
                for (int i=0; i<c.p.e.size(); i++){
                    double d = length(c.p.x,c.p.y,c.p.z,c.p.e.get(i).x,c.p.e.get(i).y, c.p.e.get(i).z);
                    double dx = c.p.e.get(i).x  - c.p.x;
                    double dy = c.p.e.get(i).y  - c.p.y;
                    double dz = c.p.e.get(i).z  - c.p.z;
                    double t = degToTurn(c.dx, c.dy, c.dz, dx, dy, dz);
                    q.add(new Step(c.p.e.get(i), dx, dy, dz, c.time + d/S + t/T));
                }
            }
 
        }
    }
 
 
}