Graphs { public static <E extends ConnectionData> List<Point> shortestPath( Graph<E> graph, final Point from, final Point to, GeomHeuristic h) { if (!graph.containsNode(from)) { throw new IllegalArgumentException("from should be valid node. " + from); } final Set<Point> closedSet = new LinkedHashSet<>(); final Map<Point, Double> gScore = new LinkedHashMap<>(); gScore.put(from, 0d); final Map<Point, Double> hScore = new LinkedHashMap<>(); hScore.put(from, h.estimateCost(graph, from, to)); final SortedMap<Double, Point> fScore = new TreeMap<>(); fScore.put(h.estimateCost(graph, from, to), from); final Map<Point, Point> cameFrom = new LinkedHashMap<>(); while (!fScore.isEmpty()) { final Point current = fScore.remove(fScore.firstKey()); if (current.equals(to)) { final List<Point> result = new ArrayList<>(); result.add(from); result.addAll(Graphs.reconstructPath(cameFrom, to)); return result; } closedSet.add(current); for (final Point outgoingPoint : graph.getOutgoingConnections(current)) { if (closedSet.contains(outgoingPoint)) { continue; } final double tgScore = gScore.get(current) + h.calculateCost(graph, current, outgoingPoint); boolean tIsBetter = false; if (!fScore.values().contains(outgoingPoint)) { hScore.put(outgoingPoint, h.estimateCost(graph, outgoingPoint, to)); tIsBetter = true; } else if (tgScore < gScore.get(outgoingPoint)) { tIsBetter = true; } if (tIsBetter) { cameFrom.put(outgoingPoint, current); gScore.put(outgoingPoint, tgScore); double fScoreValue = gScore.get(outgoingPoint) + hScore.get(outgoingPoint); while (fScore.containsKey(fScoreValue)) { fScoreValue = Double.longBitsToDouble(Double .doubleToLongBits(fScoreValue) + 1); } fScore.put(fScoreValue, outgoingPoint); } } } throw new PathNotFoundException("Cannot reach " + to + " from " + from); } private  Graphs(); static void addPath(Graph<E> graph,
      Point... path); static void addPath(Graph<E> graph,
      Iterable<Point> path); static void addBiPath(Graph<E> graph,
      Point... path); static void addBiPath(Graph<E> graph,
      Iterable<Point> path); static Graph<E> unmodifiableGraph(
      Graph<E> graph); static boolean equal(Graph<?> g1, @Nullable Object other); static List<Point> shortestPathEuclideanDistance(
      Graph<E> graph, final Point from, final Point to); static List<Point> shortestPath(
      Graph<E> graph, final Point from, final Point to, GeomHeuristic h); @Nullable static T findClosestObject(Point pos, Collection<T> objects,
      Function<T, Point> transformation); static List<T> findClosestObjects(Point pos,
      Collection<T> objects, Function<T, Point> transformation, int n); static double pathLength(List<Point> path); static ImmutableList<Point> getExtremes(Graph<?> graph);  }
@Test public void fastestPathConnData() { final Graph<MultiAttributeData> attributeGraph = new TableGraph<>(); Point A, B, C, D; A = new Point(0, 0); B = new Point(0, 10); C = new Point(10, 10); D = new Point(10, 0); attributeGraph.addConnection(A, B, MultiAttributeData.builder().setMaxSpeed(2).build()); attributeGraph.addConnection(B, C, MultiAttributeData.builder().setMaxSpeed(2).build()); attributeGraph.addConnection(A, D, MultiAttributeData.builder().setMaxSpeed(1).build()); attributeGraph.addConnection(D, C, MultiAttributeData.builder().setMaxSpeed(1).build()); assertEquals(asList(A, B, C), Graphs.shortestPath(attributeGraph, A, C, GeomHeuristics.time(50d))); attributeGraph.setConnectionData(A, D, MultiAttributeData.builder().setMaxSpeed(10).build()); attributeGraph.setConnectionData(D, C, MultiAttributeData.builder().setMaxSpeed(10).build()); assertEquals(asList(A, D, C), Graphs.shortestPath(attributeGraph, A, C, GeomHeuristics.time(50d))); }