org.openlr.map.ShortestPathFinder Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of openlr-map Show documentation
There is a newer version: 2.0-beta3
package org.openlr.map;

import lombok.Data;
import org.openlr.geo.Geo;

import java.util.*;

public class ShortestPathFinder {
    private final Geo geo;
    private final double precisionError;

    ShortestPathFinder(Geo geo, double precisionError) {
        this.geo = geo;
        this.precisionError = precisionError;
    }

    public ShortestPathFinder(Geo geo) {
        this(geo, 0.001);
    }

    public ShortestPathFinder() {
        this(new Geo());
    }

    public > Path find(PointAlongLine from, PointAlongLine to, double maxLength) {
        return find(from, to, maxLength, null);
    }

    public > Path find(PointAlongLine from, PointAlongLine to, double maxLength, FunctionalRoadClass lowestFunctionalRoadClass) {
        if (from.getLine().equals(to.getLine()) && from.getPositiveOffset() < to.getPositiveOffset()) {
            double length = to.getPositiveOffset() - from.getPositiveOffset();

            if (length > maxLength) {
                return null;
            }

            return new PathImpl<>(List.of(from.getLine()), from.getPositiveOffset(), to.getNegativeOffset(), geo);
        }
        else {
            double intermediateMaxLength = maxLength - from.getNegativeOffset() - to.getPositiveOffset();

            if (intermediateMaxLength + precisionError < 0) {
                return null;
            }

            List intermediates = findIntermediateShortestPath(from.getLine(), to.getLine(), intermediateMaxLength, lowestFunctionalRoadClass);

            if (intermediates == null) {
                return null;
            }

            List lines = new ArrayList<>();
            lines.add(from.getLine());
            lines.addAll(intermediates);
            lines.add(to.getLine());

            return new PathImpl<>(lines, from.getPositiveOffset(), to.getNegativeOffset(), geo);
        }
    }

    private >  List findIntermediateShortestPath(L from, L to, double maxLength, FunctionalRoadClass lowestFunctionalRoadClass) {
        Node startNode = from.getEndNode();
        Node goalNode = to.getStartNode();

        double initialCost = 0;
        double initialHeuristic = geo.calculateDistance(startNode.getGeometry(), goalNode.getGeometry());
        double initialTotal = initialCost + initialHeuristic;

        if (initialTotal > maxLength + precisionError) {
            return null;
        }

        QueueItem initialItem = new QueueItem<>(startNode, from, initialCost, initialHeuristic, initialTotal, null);

        PriorityQueue> queue = new PriorityQueue<>(Comparator.comparingDouble(QueueItem::getTotal));
        queue.add(initialItem);

        Set visited = new HashSet<>();

        while (!queue.isEmpty()) {
            QueueItem currentItem = queue.poll();
            Node currentNode = currentItem.getNode();

            if (currentNode.equals(goalNode)) {
                LinkedList intermediates = new LinkedList<>();

                for (QueueItem item = currentItem; item.getPrevious() != null; item = item.getPrevious()) {
                    intermediates.addFirst(item.getIncomingLine());
                }

                return intermediates;
            }

            if (visited.contains(currentNode)) {
                continue;
            }

            for (L outgoingLine : currentItem.getIncomingLine().getOutgoingLines()) {
                if (lowestFunctionalRoadClass != null && outgoingLine.getFunctionalRoadClass().ordinal() > lowestFunctionalRoadClass.ordinal()) {
                    continue;
                }

                Node nextNode = outgoingLine.getEndNode();

                if (visited.contains(nextNode)) {
                    continue;
                }

                double cost = currentItem.getCost() + outgoingLine.getLength();
                double heuristic = geo.calculateDistance(nextNode.getGeometry(), goalNode.getGeometry());
                double total = cost + heuristic;

                if (total > maxLength + precisionError) {
                    continue;
                }

                QueueItem nextItem = new QueueItem<>(nextNode, outgoingLine, cost, heuristic, total, currentItem);
                queue.add(nextItem);
            }

            visited.add(currentNode);
        }

        return null;
    }

    @Data
    private static class QueueItem>  {
        private final Node node;
        private final L incomingLine;
        private final double cost;
        private final double heuristic;
        private final double total;
        private final QueueItem previous;
    }
}