View Code of Problem 2358

import java.io.*;
import java.util.*;
public class Main{
    InputStreamReader isr = new InputStreamReader(System.in);
    BufferedReader br = new BufferedReader(isr);
    String rawInput;
    String[] inputs;
    List<Vertex> points;
    List<Polygon> polygons;
    List<Graph> graphs;
    int genObj;
    Vertex r1 = new Vertex(3910.0, 852.0), r2 = new Vertex(4527.0, 1587.0);

    public static void main(String[] args) {
        try{
            new Main();
        }
        catch(Exception e) {
            System.out.println(e);
        }
    }

    public Main() throws IOException{
        String result = "";
        rawInput = br.readLine();
        final int n = Integer.parseInt(rawInput);
               // System.out.println(n);
        for(int i = 0; i < n; ++i) {
            genObj = 0;
            rawInput = br.readLine();
            inputs = rawInput.split(" ");
            int diagonal_n = Integer.parseInt(inputs[0]);
            Vertex mother = new Vertex(Integer.parseInt(inputs[3]), Integer.parseInt(inputs[4])), mothy = new Vertex(Integer.parseInt(inputs[1]), Integer.parseInt(inputs[2]));
            points = new ArrayList<Vertex>();
            polygons = new ArrayList<Polygon>();
            graphs = new ArrayList<Graph>();
            // points.add(mother);
            // points.add(mothy);
            listenDiagonals(diagonal_n);
            addNodes(mother, mothy, polygons, points);
            makeGraph();
            TreeSet<Vertex> queue = new TreeSet<Vertex>();
            mother.setRoot();
            for(Vertex p: mother.getNeighbors()) {
                p.setRoot(mother);
            }
            for(Vertex p: points) {
                if(!p.equals(mother))
                    queue.add(p);
            }
            while(!queue.isEmpty()) {
                Vertex p = queue.first();
                p.setFixed();
                queue.remove(p);
                if(p.equals(mothy)) break;
                for(Vertex q: p.getNeighbors()) {
                    if(!q.getFixed()) {
                        queue.remove(q);
                        q.setRoot(p);
                        queue.add(q);
                    }
                }
            }
            double d;
            int m;
            String str = "";
            d = mothy.getDistance();
            m = 1;
            Vertex iterate = mothy;
            if(d < Double.MAX_VALUE) {
                while(!iterate.getRoot().equals(iterate)) {
                    str += iterate + " ";
                    iterate = iterate.getRoot();
                    m++;
                }
                str += mother;
                System.out.println(d + " " + m);
                System.out.println(str);
            }
            // for(Polygon pol : polygons) {
            //     System.out.println(pol);
            // }
            result += (mothy.getDistance() == Double.MAX_VALUE) ? "-1\n" : String.format("%.3f\n", mothy.getDistance());
        }
        System.out.println(result);
    }

    private void listenDiagonals(final int n) throws IOException{
        for(int i = 0; i < n; ++i) {
            inputs = br.readLine().split(" ");
            int vertices_n = Integer.parseInt(inputs[0]);
            Polygon p = new Polygon();
            for(int k = 0; k < vertices_n; ++k) {
                Vertex v = new Vertex(Integer.parseInt(inputs[2 * k + 1]), Integer.parseInt(inputs[2 * k + 2]));
                if(points.contains(v)) {
                    v = points.get(points.indexOf(v));
                }
                p.addVertice(v);
                // points.add(v);
            }
            polygons.add(p);
        }
        for(Polygon p: polygons) {
            p.makeEdge();
        }

    }

    void addNodes(Vertex v, Vertex w, List<Polygon> pList, List<Vertex> vList) {
        for(Polygon p:pList) {
            if(v.in(p) || w.in(p)) return;
        }
        vList.add(v);vList.add(w);
        int j = 0;
        for(int i = 0; i < pList.size(); ++i){
            for(Vertex ve : pList.get(i).getVertices()) {
                for(j = 0; j < pList.size(); ++j) {
                    if(i != j && ve.in(pList.get(j))) {
                        break;
                    }
                }
                if(j == pList.size()) {
                    vList.add(ve);
                }
            }
        }
    }


    private void makeGraph() {
        for(int i = 0; i < points.size(); ++i) {
            for(int j = i + 1; j < points.size(); ++j) {
                points.get(i).addNeighbor(points.get(j));
            }
        }
    }


    class Graph {
        private Vertex v, w;
        private double length;
        private int gen_id;
        public Graph (Vertex v, Vertex w){
            this.v = v;
            this.w = w;
            length = v.calcDistance(w);
            this.gen_id = genObj;
            genObj++;
        }

        public boolean equals(Graph g) {
            return (this.v.equals(g.v) && this.w.equals(g.w)) || (this.v.equals(g.w) && this.w.equals(g.v));
        }
        
        public double getDistance() {
            return length;
        }

        public String toString() {
            return v + "-" + w;
        }

        public boolean crossesWith(Graph g) {
            if(this.v.equals(g.v) || this.w.equals(g.v) || this.v.equals(g.w) || this.v.equals(g.w))
                return false;
            else 
                return ((this.w.isLeft(this.v, g.v) &&
                         g.w.isLeft(this.v, g.v)&&
                         this.v.isLeft(g.v, this.w) &&
                         g.w.isLeft(g.v, this.w) &&
                         this.v.isLeft(this.w, g.w) &&
                         g.v.isLeft(this.w, g.w) &&
                         this.w.isLeft(g.w, this.v) &&
                         g.v.isLeft(g.w, this.v)) ||
                        (this.w.isLeft(this.v, g.w) &&
                         g.v.isLeft(this.v, g.w) &&
                         this.v.isLeft(g.w, this.w) &&
                         g.v.isLeft(g.w, this.w) &&
                         this.v.isLeft(this.w, g.v) &&
                         g.w.isLeft(this.w, g.v) &&
                         this.w.isLeft(g.v, this.v) &&
                         g.w.isLeft(g.v, this.v)));
                // return ((this.v.isLeft(g.v, this.w) &&
                //      g.w.isLeft(g.v, this.w) &&
                //      this.v.isLeft(this.w, g.w) &&
                //      g.v.isLeft(this.w, g.w) &&
                //      g.v.isLeft(g.w, this.v) &&
                //      this.w.isLeft(g.w, this.v) &&
                //      this.w.isLeft(this.v, g.v) &&
                //      g.w.isLeft(this.v, g.v)) ||
                //     (this.v.isLeft(g.w, this.w) &&
                //      g.v.isLeft(g.w, this.w) &&
                //      this.v.isLeft(this.w, g.v) &&
                //      g.w.isLeft(this.w, g.v) &&
                //      g.w.isLeft(g.v, this.v) &&
                //      this.w.isLeft(g.v, this.v) &&
                //      this.w.isLeft(this.v, g.w) &&
                //      g.v.isLeft(this.v, g.w)));
        }

        public boolean throwghs(Polygon p) {
            List<Graph> edges = p.getEdges();
            for(Graph g : edges) {
                if(this.crossesWith(g)) 
                    return true;
                
            }
            return false;
        }


        public boolean throwghAnyPoint() {
            Vertex vec1 = new Vertex(w.getX() - v.getX(), w.getY() - v.getY());
            for(Vertex vv : points) {
                if(!vv.equals(w) && !vv.equals(v)) {
                    Vertex vec2 = new Vertex(vv.getX() - v.getX(), vv.getY() - v.getY());
                    if(vec2.getX()/vec1.getX() * vec1.getY() == vec2.getY()) {
                        return true;
                    }
                }
            }
            return false;
        }
        
        // public boolean throwghAnyPoint() {
        //     Vertex vec1 =(w.getX() > v.getX()) ?  new Vertex(w.getX() - v.getX(), w.getY() - v.getY()) : new Vertex(v.getX() - w.getX(), v.getY() - w.getY());
        //     if(vec1.getX() != 0.0) {
        //         for(double i = 1.0; i < vec1.getX(); i+=1.0) {
        //             double vexy  = vec1.getY()*i/vec1.getX();
        //             if(vexy % 1.0 == 0.0) {
        //                 Vertex vex = (w.getX() > v.getX()) ? new Vertex(v.getX()  + i, v.getY() + vexy) : new Vertex(w.getX() + i, w.getY() + vexy);
        //                 if(points.contains(vex)) return true;
        //             }
        //         }
        //     }
        //     else {
        //         for(double i = 1.0; i < vec1.getY(); i += 1.0) {
        //             Vertex vex = (w.getY() > v.getY()) ? new Vertex(v.getX(), v.getY() + i) : new Vertex(w.getX(), w.getY() + i);
        //             if(points.contains(vex)) return true;
        //         }
        //     }
        //     return false;
        // }

    }

    class Vertex implements Comparable<Vertex>{
        private double x, y, distance;
        private int id;
        private final int gen_id;
        private List<Vertex> neighbors;
        private List<Polygon> affiliation;
        private boolean fixed;
        private Vertex root;
        public Vertex(int x, int y) {
            this.x = (double) x;
            this.y = (double) y;
            this.id = -1;
            this.root = null;
            this.gen_id = genObj;
            this.fixed = false;
            affiliation = new ArrayList<Polygon>();
            neighbors = new ArrayList<Vertex>();
            genObj++;
            this.distance = Double.MAX_VALUE;
        }

        public boolean adjoins(Vertex v) {
            return graphs.contains(new Graph(this, v));
        }
        
        public void addAffiliation(Polygon p) {
            if(!affiliation.contains(p)) {
                affiliation.add(p);
            }
                                         
        }

        public void setFixed() {
            fixed = true;
        }

        public boolean getFixed() {
            return fixed;
        }

        public boolean equals(Vertex v) {
            //            return gen_id == v.gen_id;
            return x == v.x && y == v.y;
        }

        public String toString() {
            return this.x + " " + this.y;
        }

        public boolean hasCoaffiliationWith(Vertex v) {
            for(Polygon p: affiliation) {
                if(v.affiliation.contains(p)) {
                    return true;
                }
            }
            return false;
                                           
        }

        public boolean hasCoaffiliationWith(Vertex v, Polygon exception) {
            for(Polygon p: affiliation) {
                if(v.affiliation.contains(p) && !p.equals(exception)) {
                    return true;
                }
            }
            return false;
                                           
        }

        public Vertex(double x, double y) {
            this.x = x;
            this.y = y;
            this.id = -1;
            this.root = null;
            this.gen_id = genObj;
            this.fixed = false;
            neighbors = new ArrayList<Vertex>();
            genObj++;
            this.distance = Double.MAX_VALUE;
        }

        public void addNeighbor(Vertex v) {
            if(!this.equals(v) && this.visible(v) && !neighbors.contains(v) ){
                // if(this.equals(r1)) {
                //     for(Vertex vv : this.getNeighbors()) {
                //         System.out.println(vv);
                //         if(vv.equals(r2)) {
                //             System.out.println(this.equals(v) +", "+ this.visible(v) +", " + neighbors.contains(v) +", " + (!this.hasCoaffiliationWith(v)) +  ", " + graphs.contains(new Graph(this, v)));
                //         }
                //     }
                // }
                Graph tmp1 = new Graph(r1, r2), tmp2 = new Graph(this, v);
             
                neighbors.add(v);
                v.addNeighbor(this);
            }
        }

        public boolean visible(Vertex v) {
            Graph g = new Graph(this, v);
            for(Polygon p: polygons) {
                if(g.throwghs(p)) 
                    return false;
            }
            return !g.throwghAnyPoint() && (!this.hasCoaffiliationWith(v) || graphs.contains(g));
        }

        public double getDistance() {
            return distance;
        }

        @Override
        public int compareTo(Vertex v) {
            if(this.distance == v.distance) {
                return this.gen_id - v.gen_id;
            }
            else {
                return (int)(this.distance - v.distance);
            }
        }

        public double calcDistance(Vertex v) {
            return Math.sqrt(Math.pow(v.x - this.x, 2.0) + Math.pow(v.y - this.y, 2.0));
        }

        public boolean isLeft(Vertex v, Vertex w) {
            Vertex vec1 = new Vertex(this.x - v.x, this.y - v.y), vec2 = new Vertex(w.x - v.x, w.y - v.y);
            return vec1.x*vec2.y - vec1.y*vec2.x > 0.0;
        }
        
        public double getX() { return x;}
        public double getY() { return y;}

        
        public boolean in(Polygon p) {
            return p.containsIn(this);
        }

        public List<Vertex> getNeighbors() {
            return neighbors;
        }

        public void setRoot() {
            distance = 0.0;
            root = this;
        }


        public void setRoot(Vertex v) {
            double ndistance = v.distance + calcDistance(v);
            if(distance > ndistance) {
                root = v;
                distance = ndistance;
            }
        }

        public Vertex getRoot() {
            return root;
        }

    }

    class Polygon{
        private List<Vertex> vertices;
        private List<Graph> edges;
        private int gen_id;
        public Polygon() {
            this.vertices = new ArrayList<Vertex>();
            this.edges = new ArrayList<Graph>();
            this.gen_id = genObj;
            genObj++;
        }

        public void makeEdge() {
            for(int i = 0; i < this.vertices.size(); ++i) {
                Vertex s = this.vertices.get(i), t = this.vertices.get((i + 1) % this.vertices.size());
                //                s.addNeighbor(t);
                Graph g = new Graph(s, t);
                this.edges.add(g);
                if(!graphs.contains(g) && s.hasCoaffiliationWith(t, this)) {
                    graphs.add(g);
                }
            }
        }

        public Polygon(List<Vertex> vertices) {
            this.vertices = new ArrayList<Vertex>();
            this.edges = new ArrayList<Graph>();
            for(Vertex v: vertices) {
                addVertice(v);
            }
            makeEdge();
            this.gen_id = genObj;
            genObj++;
        }

        public boolean containsIn(Vertex v) {
            if(vertices.contains(v)) return false;
            final int vertices_size = vertices.size();
            for(int i = 0; i < vertices_size; ++i) {
                Vertex s = vertices.get(i), t = vertices.get((i + 1) % vertices_size);
                if(!v.isLeft(s, t)) {
                    return false;
                }
            }
            return true;
        }

        public boolean equals(Polygon p) {
            return gen_id == p.gen_id;
        }

        public List<Graph> getEdges() {
            return this.edges;
        }

        public List<Vertex> getVertices() {
            return this.vertices;
        }
        
        public void addVertice(Vertex vertice) {
            vertice.addAffiliation(this);
            if(vertices.size() <= 1) {
                vertices.add(vertice);
            }
            else {
                for(int i = 0; i < vertices.size(); ++i) {
                    Vertex s = vertices.get(i), t = vertices.get((i + 1)%vertices.size());
                    if((i + 1)%vertices.size() != 0 && !vertice.isLeft(s, t)) {
                        vertices.set(i + 1, vertice);
                        i++;
                        for(;i < vertices.size(); ++i) {
                            s = vertices.get((i) % vertices.size());
                            vertices.set(i, t);
                            t = s;
                        }
                        vertices.add(t);
                        break;
                    }
                    else if((i + 1)%vertices.size() ==0 ) {
                        vertices.add(vertice);
                        break;
                    }
                }
            }

        }
        public String toString() {
            String str = vertices.size() + ":";
            for(Vertex vertex:vertices) {
                str += vertex + ", ";
            }
            return str;
        }
                                
    }

}