persolijn

Graph.java (10417B)

---

 package osm.protobuf;
 
 import java.util.Collection;
 import java.util.HashMap;
 import java.util.HashSet;
 import java.util.Map;
 import java.util.Map.Entry;
 import java.util.NoSuchElementException;
 import java.util.Set;
 import java.util.function.Consumer;
 import java.util.stream.Collector;
 import java.util.stream.Collector.Characteristics;
 
 import osm.common.Container;
 import osm.common.ContainerCollector;
 import osm.common.Edge;
 import osm.geo.Point;
 import osm.message.Entity;
 import osm.message.Node;
 import osm.message.Way;
 import osm.routing.RoutingGraph;
 
 /**
  * Represents a graph used for routing on OpenStreetMap data. Implements the
  * {@link Container} and {@link RoutingGraph} interfaces.
  */
 public class Graph implements Container<Graph, Entity>, RoutingGraph<Node, Edge> {
     /**
      * Constant representing the nanosecond unit.
      */
     public static final double NANO = 1e-9;
 
     /**
      * The default speed for the router, in meters per second.
      */
     public static final double DEFAULT_SPEED = 60 / 3.6;
 
     /**
      * Constant representing the additional distance range when searching for nodes.
      */
     public static final double DISTANCE_RANGE = 5; // adding 5km to the result
 
     /**
      * Constant representing the multiplier for the additional distance range.
      */
     public static final double DISTANCE_RANGE_MULTIPLIER = 1.25; // adding 25% to result
 
     /**
      * Returns a {@link Collector} that accumulates elements into a {@link Graph}.
      *
      * @param points The collection of points to center the graph around.
      * @return A {@link Collector} for accumulating elements into a {@link Graph}.
      */
     public static Collector<Entity, ?, Graph> asCollector(Collection<Point> points) {
         Point center = Point.center(points);
 
         long diameter = points.stream()
                 .mapToLong(center::distanceTo)
                 .max()
                 .orElseThrow();
 
         long range = Math.round(DISTANCE_RANGE + DISTANCE_RANGE_MULTIPLIER * diameter);
 
         return new ContainerCollector<>(() -> new Graph(points, center, range),
                 Set.of(Characteristics.CONCURRENT, Characteristics.UNORDERED));
     }
 
     private final Collection<Point> points;
     private final Point center;
     private final long range;
 
     public final Map<Long, Node> nodes = new HashMap<>();
     public final Map<Long, Way> ways = new HashMap<>();
     public final Map<Node, Set<Edge>> neighbors = new HashMap<>();
     public final Map<Point, Node> pointNode = new HashMap<>();
     public final Map<Point, Long> pointDistance = new HashMap<>();
 
     /**
      * Constructs a {@link Graph} with the specified points, center, and range.
      *
      * @param points The collection of points in the graph.
      * @param center The center point of the graph.
      * @param range  The range around the center within which nodes are considered.
      */
     public Graph(Collection<Point> points, Point center, long range) {
         this.points = points;
         this.center = center;
         this.range = range;
     }
 
     /**
      * Combines this graph with another graph, updating its nodes and neighbors.
      *
      * @param other The other graph to combine with.
      * @return The combined graph.
      */
     @Override
     public void fold(Graph other) {
         nodes.putAll(other.nodes);
         for (Entry<Node, Set<Edge>> neigh : other.neighbors.entrySet())
             neighbors.computeIfAbsent(neigh.getKey(), k -> new HashSet<>())
                     .addAll(neigh.getValue());
     }
 
     /**
      * Finalizes the graph by connecting ways and determining the closest nodes to
      * each point.
      */
     @Override
     public void finish() {
         ways.forEach((i, w) -> connectWay(w));
 
         for (Node node : nodes.values()) {
             if (!neighbors.containsKey(node))
                 continue;
 
             for (Point p : points) {
                 if (!pointNode.containsKey(p) || node.distanceTo(p) < pointDistance.get(p)) {
                     pointNode.put(p, node);
                     pointDistance.put(p, node.distanceTo(p));
                 }
             }
         }
 
         System.out.printf("nodes: \t\t%d\nneighbors: \t%d\n", nodes.size(), neighbors.size());
     }
 
     /**
      * Connects the nodes of a way by creating edges between consecutive nodes.
      *
      * @param w The way to connect.
      */
     protected void connectWay(Way w) {
         Node previous = null;
 
         for (long currentID : w.children) {
             if (!nodes.containsKey(currentID))
                 continue;
 
             Node current = nodes.get(currentID);
 
             if (previous != null) {
                 long dist = current.distanceTo(previous);
 
                 neighbors.computeIfAbsent(previous, k -> new HashSet<>())
                         .add(new Edge(current, dist, (double) dist / w.speedForwards));
 
                 neighbors.computeIfAbsent(current, k -> new HashSet<>())
                         .add(new Edge(previous, dist, (double) dist / w.speedBackwards));
             }
 
             previous = current;
         }
     }
 
     /**
      * Accumulates an entity into the graph by adding nodes and ways based on their
      * distance to the graph center.
      *
      * @param element The entity to accumulate.
      */
     @Override
     public void accumulate(Entity element) {
         if (element instanceof Node node) {
             if (node.distanceTo(center) < range)
                 nodes.put(node.getID(), node);
         } else if (element instanceof Way way) {
             ways.put(way.getID(), way);
         }
     }
 
     /**
      * Retrieves a node from the graph based on its point location.
      *
      * @param point The point location of the node to retrieve.
      * @return The node corresponding to the specified point.
      */
     public Node getNode(Point point) {
         return pointNode.get(point);
     }
 
     /**
      * Retrieves a node from the graph using its ID.
      *
      * @param id The ID of the node to retrieve.
      * @return The node with the specified ID.
      * @throws NoSuchElementException If the node with the given ID is not found.
      */
     public Node getNode(long id) throws NoSuchElementException {
         if (!nodes.containsKey(id))
             throw new NoSuchElementException(String.format("node `%d` not found", id));
 
         return nodes.get(id);
     }
 
     /**
      * Calculates the cost of moving from the current node to the next node.
      *
      * @param current The current node.
      * @param next    The next node to move to.
      * @return The cost of moving from the current node to the next node.
      */
     @Override
     public long getCost(Node current, Edge next) {
         double time = next.getTime()
                 + Math.abs(Point.getAngle(current.getParent(), current, next.getDestination())) / Math.PI * 30;
         return Math.round(next.getDistance() / DEFAULT_SPEED * 0.2 + time * 0.8);
     }
 
     /**
      * Gets the closest distance from a node to a set of target nodes.
      *
      * @param targets The target nodes and their distances.
      * @param node    The node for which to find the closest distance.
      * @return The closest distance from the node to any of the target nodes.
      */
     @Override
     public long getClosestDistance(Map<Node, Long> targets, Node node) {
         return targets.entrySet().stream()
                 .map(e -> e.getKey().distanceTo(node) + e.getValue())
                 .min(Long::compare)
                 .orElseThrow();
     }
 
     /**
      * Called when progress is made during the route calculation.
      *
      * @param progress The progress made, represented as a percentage.
      * @param start    The starting node.
      * @param targets  The target nodes.
      */
     @Override
     public void onProgress(double progress, Node start, Collection<Node> targets) {
         System.out.printf("%s -> %s %6.2f%% [%s>%s]\r",
                 start, targets,
                 progress * 100,
                 "=".repeat((int) Math.round(progress * 50)),
                 " ".repeat((int) Math.round((1 - progress) * 50)));
     }
 
     /**
      * Checks if a node is a target node.
      *
      * @param targets The target nodes and their distances.
      * @param node    The node to check.
      * @return {@code true} if the node is a target node, {@code false} otherwise.
      */
     @Override
     public boolean isTarget(Map<Node, Long> targets, Node node) {
         return targets.containsKey(node);
     }
 
     /**
      * Calculates a heuristic value for a node based on its distance to target
      * nodes.
      *
      * @param targets The target nodes and their distances.
      * @param node    The node for which to calculate the heuristic.
      * @return The heuristic value for the given node.
      */
     @Override
     public long getHeuristic(Map<Node, Long> targets, Node node) {
         long dist = targets.entrySet().stream()
                 .map(e -> e.getKey().distanceTo(node) + e.getValue())
                 .min(Long::compare)
                 .orElseThrow();
 
         return Math.round(0.5 * dist / DEFAULT_SPEED);
     }
 
     /**
      * Gets the distance from the start node to a given node.
      *
      * @param start The start node.
      * @param node  The node for which to calculate the distance.
      * @return The distance from the start node to the given node.
      */
     @Override
     public long getStartDistance(Node start, Node node) {
         return start.distanceTo(node);
     }
 
     /**
      * Iterates over the neighbors of a node and applies the provided consumer
      * function.
      *
      * @param node     The node for which to iterate over neighbors.
      * @param consumer The consumer function to apply to each neighbor.
      */
     @Override
     public void forNeighbor(Node node, Consumer<Edge> consumer) {
         // System.out.printf("%s: %s\n", node, neighbors.get(node.getID()));
 
         if (!neighbors.containsKey(node))
             return;
 
         neighbors.get(node).forEach(n -> {
             consumer.accept(n);
         });
     }
 
     /**
      * Creates an array to store the history of a node with the given size.
      *
      * @param size The size of the history array.
      * @return An array capable of storing the history of a node.
      */
     @Override
     public Node[] createHistoryArray(int size) {
         return new Node[size];
     }
 }