private void shortestPath(Intersection start, List<Intersection> inters) { for(Intersection i : inters) { i.setK(false); i.setP(null); i.setD(INFINITY); } Heap<Intersection> h = new Heap<Intersection>(); start.setD(0.0); h.insert(start.getD(), start); do { Intersection i = h.remove(); if(i.isK() == false) { i.setK(true); for(Road r : i.getAdjacent()) { Intersection next = r.getIntersection2(); if(next.getD() > i.getD() + r.getCost(measure)) { next.setD(i.getD() + r.getCost(measure)); next.setP(i); h.insert(next.getD(), next); } } } } while(!h.isEmpty()); }