Examples with BinHeap org.opentripplanner.common.pqueue.BinHeap used on opensource projects

Example 1 with BinHeap

use of org.opentripplanner.common.pqueue.BinHeap in project OpenTripPlanner by opentripplanner.

the class StreetMatcher method match.

@SuppressWarnings("unchecked")
public List<Edge> match(Geometry routeGeometry) {
    routeGeometry = removeDuplicatePoints(routeGeometry);
    if (routeGeometry == null)
        return null;
    routeGeometry = DouglasPeuckerSimplifier.simplify(routeGeometry, 0.00001);
    // initial state: start midway along a block.
    LocationIndexedLine indexedLine = new LocationIndexedLine(routeGeometry);
    LinearLocation startIndex = indexedLine.getStartIndex();
    Coordinate routeStartCoordinate = startIndex.getCoordinate(routeGeometry);
    Envelope envelope = new Envelope(routeStartCoordinate);
    double distanceThreshold = DISTANCE_THRESHOLD;
    envelope.expandBy(distanceThreshold);
    BinHeap<MatchState> states = new BinHeap<MatchState>();
    List<Edge> nearbyEdges = index.query(envelope);
    while (nearbyEdges.isEmpty()) {
        envelope.expandBy(distanceThreshold);
        distanceThreshold *= 2;
        nearbyEdges = index.query(envelope);
    }
    // compute initial states
    for (Edge initialEdge : nearbyEdges) {
        Geometry edgeGeometry = initialEdge.getGeometry();
        LocationIndexedLine indexedEdge = new LocationIndexedLine(edgeGeometry);
        LinearLocation initialLocation = indexedEdge.project(routeStartCoordinate);
        double error = MatchState.distance(initialLocation.getCoordinate(edgeGeometry), routeStartCoordinate);
        MidblockMatchState state = new MidblockMatchState(null, routeGeometry, initialEdge, startIndex, initialLocation, error, 0.01);
        // make sure all initial states are visited by inserting them at 0
        states.insert(state, 0);
    }
    // search for best-matching path
    int seen_count = 0, total = 0;
    HashSet<MatchState> seen = new HashSet<MatchState>();
    while (!states.empty()) {
        double k = states.peek_min_key();
        MatchState state = states.extract_min();
        if (++total % 50000 == 0) {
            log.debug("seen / total: " + seen_count + " / " + total);
        }
        if (seen.contains(state)) {
            ++seen_count;
            continue;
        } else {
            if (k != 0) {
                // but do not mark states as closed if we start at them
                seen.add(state);
            }
        }
        if (state instanceof EndMatchState) {
            return toEdgeList(state);
        }
        for (MatchState next : state.getNextStates()) {
            if (seen.contains(next)) {
                continue;
            }
            states.insert(next, next.getTotalError() - next.getDistanceAlongRoute());
        }
    }
    return null;
}

Example 2 with BinHeap

use of org.opentripplanner.common.pqueue.BinHeap in project OpenTripPlanner by opentripplanner.

the class EarliestArrivalSearch method getShortestPathTree.

public ShortestPathTree getShortestPathTree(RoutingRequest options, double relTimeout, SearchTerminationStrategy terminationStrategy) {
    // clone options before modifying, otherwise disabling resource limiting will cause
    // SPT cache misses for subsequent requests.
    options = options.clone();
    // disable any resource limiting, which is algorithmically invalid here
    options.maxTransfers = Integer.MAX_VALUE;
    options.setMaxWalkDistance(Double.MAX_VALUE);
    if (options.clampInitialWait < 0)
        options.clampInitialWait = (60 * 30);
    // impose search cutoff
    final long maxt = maxDuration + options.clampInitialWait;
    options.worstTime = options.dateTime + (options.arriveBy ? -maxt : maxt);
    // SPT cache does not look at routing request in SPT to perform lookup,
    // so it's OK to construct with the local cloned one
    ShortestPathTree spt = new DominanceFunction.EarliestArrival().getNewShortestPathTree(options);
    State initialState = new State(options);
    spt.add(initialState);
    BinHeap<State> pq = new BinHeap<State>();
    pq.insert(initialState, 0);
    while (!pq.empty()) {
        State u = pq.extract_min();
        Vertex u_vertex = u.getVertex();
        if (!spt.visit(u))
            continue;
        Collection<Edge> edges = options.arriveBy ? u_vertex.getIncoming() : u_vertex.getOutgoing();
        for (Edge edge : edges) {
            for (State v = edge.traverse(u); v != null; v = v.getNextResult()) {
                if (isWorstTimeExceeded(v, options)) {
                    continue;
                }
                if (spt.add(v)) {
                    // activeTime?
                    pq.insert(v, v.getActiveTime());
                }
            }
        }
    }
    return spt;
}

Example 3 with BinHeap

use of org.opentripplanner.common.pqueue.BinHeap in project OpenTripPlanner by opentripplanner.

the class InterleavedBidirectionalHeuristic method streetSearch.

/**
 * Explore the streets around the origin or target, recording the minimum weight of a path to each street vertex.
 * When searching around the target, also retain the states that reach transit stops since we'll want to
 * explore the transit network backward, in order to guide the main forward search.
 *
 * The main search always proceeds from the "origin" to the "target" (names remain unchanged in arriveBy mode).
 * The reverse heuristic search always proceeds outward from the target (name remains unchanged in arriveBy).
 *
 * When the main search is departAfter:
 * it gets outgoing edges and traverses them with arriveBy=false,
 * the heuristic search gets incoming edges and traverses them with arriveBy=true,
 * the heuristic destination street search also gets incoming edges and traverses them with arriveBy=true,
 * the heuristic origin street search gets outgoing edges and traverses them with arriveBy=false.
 *
 * When main search is arriveBy:
 * it gets incoming edges and traverses them with arriveBy=true,
 * the heuristic search gets outgoing edges and traverses them with arriveBy=false,
 * the heuristic destination street search also gets outgoing edges and traverses them with arriveBy=false,
 * the heuristic origin street search gets incoming edges and traverses them with arriveBy=true.
 * The streetSearch method traverses using the real traverse method rather than the lower bound traverse method
 * because this allows us to keep track of the distance walked.
 * Perhaps rather than tracking walk distance, we should just check the straight-line radius and
 * only walk within that distance. This would avoid needing to call the main traversal functions.
 *
 * TODO what if the egress segment is by bicycle or car mode? This is no longer admissible.
 */
private TObjectDoubleMap<Vertex> streetSearch(RoutingRequest rr, boolean fromTarget, long abortTime) {
    LOG.debug("Heuristic street search around the {}.", fromTarget ? "target" : "origin");
    rr = rr.clone();
    if (fromTarget) {
        rr.setArriveBy(!rr.arriveBy);
    }
    // Create a map that returns Infinity when it does not contain a vertex.
    TObjectDoubleMap<Vertex> vertices = new TObjectDoubleHashMap<>(100, 0.5f, Double.POSITIVE_INFINITY);
    ShortestPathTree spt = new DominanceFunction.MinimumWeight().getNewShortestPathTree(rr);
    // TODO use normal OTP search for this.
    BinHeap<State> pq = new BinHeap<State>();
    Vertex initVertex = fromTarget ? rr.rctx.target : rr.rctx.origin;
    State initState = new State(initVertex, rr);
    pq.insert(initState, 0);
    while (!pq.empty()) {
        if (abortTime < Long.MAX_VALUE && System.currentTimeMillis() > abortTime) {
            return null;
        }
        State s = pq.extract_min();
        Vertex v = s.getVertex();
        // This is the lowest cost we will ever see for this vertex. We can record the cost to reach it.
        if (v instanceof TransitStop) {
            // place vertices on the transit queue so we can explore the transit network backward later.
            if (fromTarget) {
                double weight = s.getWeight();
                transitQueue.insert(v, weight);
                if (weight > maxWeightSeen) {
                    maxWeightSeen = weight;
                }
            }
            continue;
        }
        // Record the cost to reach this vertex.
        if (!vertices.containsKey(v)) {
            // FIXME time or weight? is RR using right mode?
            vertices.put(v, (int) s.getWeight());
        }
        for (Edge e : rr.arriveBy ? v.getIncoming() : v.getOutgoing()) {
            // arriveBy has been set to match actual directional behavior in this subsearch.
            // Walk cutoff will happen in the street edge traversal method.
            State s1 = e.traverse(s);
            if (s1 == null) {
                continue;
            }
            if (spt.add(s1)) {
                pq.insert(s1, s1.getWeight());
            }
        }
    }
    LOG.debug("Heuristric street search hit {} vertices.", vertices.size());
    LOG.debug("Heuristric street search hit {} transit stops.", transitQueue.size());
    return vertices;
}

Example 4 with BinHeap

use of org.opentripplanner.common.pqueue.BinHeap in project OpenTripPlanner by opentripplanner.

the class ElevationModule method assignMissingElevations.

/**
 * Assign missing elevations by interpolating from nearby points with known
 * elevation; also handle osm ele tags
 */
private void assignMissingElevations(Graph graph, List<StreetEdge> edgesWithElevation, HashMap<Vertex, Double> knownElevations) {
    log.debug("Assigning missing elevations");
    BinHeap<ElevationRepairState> pq = new BinHeap<ElevationRepairState>();
    // elevation for each vertex (known or interpolated)
    // knownElevations will be null if there are no ElevationPoints in the data
    // for instance, with the Shapefile loader.)
    HashMap<Vertex, Double> elevations;
    if (knownElevations != null)
        elevations = (HashMap<Vertex, Double>) knownElevations.clone();
    else
        elevations = new HashMap<Vertex, Double>();
    HashSet<Vertex> closed = new HashSet<Vertex>();
    // initialize queue with all vertices which already have known elevation
    for (StreetEdge e : edgesWithElevation) {
        PackedCoordinateSequence profile = e.getElevationProfile();
        if (!elevations.containsKey(e.getFromVertex())) {
            double firstElevation = profile.getOrdinate(0, 1);
            ElevationRepairState state = new ElevationRepairState(null, null, e.getFromVertex(), 0, firstElevation);
            pq.insert(state, 0);
            elevations.put(e.getFromVertex(), firstElevation);
        }
        if (!elevations.containsKey(e.getToVertex())) {
            double lastElevation = profile.getOrdinate(profile.size() - 1, 1);
            ElevationRepairState state = new ElevationRepairState(null, null, e.getToVertex(), 0, lastElevation);
            pq.insert(state, 0);
            elevations.put(e.getToVertex(), lastElevation);
        }
    }
    // back pointers through the region of unknown elevation, setting elevations via interpolation.
    while (!pq.empty()) {
        ElevationRepairState state = pq.extract_min();
        if (closed.contains(state.vertex))
            continue;
        closed.add(state.vertex);
        ElevationRepairState curState = state;
        Vertex initialVertex = null;
        while (curState != null) {
            initialVertex = curState.vertex;
            curState = curState.backState;
        }
        double bestDistance = Double.MAX_VALUE;
        double bestElevation = 0;
        for (Edge e : state.vertex.getOutgoing()) {
            if (!(e instanceof StreetEdge)) {
                continue;
            }
            StreetEdge edge = (StreetEdge) e;
            Vertex tov = e.getToVertex();
            if (tov == initialVertex)
                continue;
            Double elevation = elevations.get(tov);
            if (elevation != null) {
                double distance = e.getDistance();
                if (distance < bestDistance) {
                    bestDistance = distance;
                    bestElevation = elevation;
                }
            } else {
                // continue
                ElevationRepairState newState = new ElevationRepairState(edge, state, tov, e.getDistance() + state.distance, state.initialElevation);
                pq.insert(newState, e.getDistance() + state.distance);
            }
        }
        for (Edge e : state.vertex.getIncoming()) {
            if (!(e instanceof StreetEdge)) {
                continue;
            }
            StreetEdge edge = (StreetEdge) e;
            Vertex fromv = e.getFromVertex();
            if (fromv == initialVertex)
                continue;
            Double elevation = elevations.get(fromv);
            if (elevation != null) {
                double distance = e.getDistance();
                if (distance < bestDistance) {
                    bestDistance = distance;
                    bestElevation = elevation;
                }
            } else {
                // continue
                ElevationRepairState newState = new ElevationRepairState(edge, state, fromv, e.getDistance() + state.distance, state.initialElevation);
                pq.insert(newState, e.getDistance() + state.distance);
            }
        }
        // in the case of islands missing elevation (and some other cases)
        if (bestDistance == Double.MAX_VALUE && state.distance > 2000) {
            log.warn("While propagating elevations, hit 2km distance limit at " + state.vertex);
            bestDistance = state.distance;
            bestElevation = state.initialElevation;
        }
        if (bestDistance != Double.MAX_VALUE) {
            // we have found a second vertex with elevation, so we can interpolate the elevation
            // for this point
            double totalDistance = bestDistance + state.distance;
            // trace backwards, setting states as we go
            while (true) {
                // all the way out to edge lengths
                if (totalDistance == 0)
                    elevations.put(state.vertex, bestElevation);
                else {
                    double elevation = (bestElevation * state.distance + state.initialElevation * bestDistance) / totalDistance;
                    elevations.put(state.vertex, elevation);
                }
                if (state.backState == null)
                    break;
                bestDistance += state.backEdge.getDistance();
                state = state.backState;
                if (elevations.containsKey(state.vertex))
                    break;
            }
        }
    }
    // do actual assignments
    for (Vertex v : graph.getVertices()) {
        Double fromElevation = elevations.get(v);
        for (Edge e : v.getOutgoing()) {
            if (e instanceof StreetWithElevationEdge) {
                StreetWithElevationEdge edge = ((StreetWithElevationEdge) e);
                Double toElevation = elevations.get(edge.getToVertex());
                if (fromElevation == null || toElevation == null) {
                    if (!edge.isElevationFlattened() && !edge.isSlopeOverride())
                        log.warn("Unexpectedly missing elevation for edge " + edge);
                    continue;
                }
                if (edge.getElevationProfile() != null && edge.getElevationProfile().size() > 2) {
                    continue;
                }
                Coordinate[] coords = new Coordinate[2];
                coords[0] = new Coordinate(0, fromElevation);
                coords[1] = new Coordinate(edge.getDistance(), toElevation);
                PackedCoordinateSequence profile = new PackedCoordinateSequence.Double(coords);
                if (edge.setElevationProfile(profile, true)) {
                    log.trace(graph.addBuilderAnnotation(new ElevationFlattened(edge)));
                }
            }
        }
    }
}

Example 5 with BinHeap

use of org.opentripplanner.common.pqueue.BinHeap in project OpenTripPlanner by opentripplanner.

the class GenericDijkstra method getShortestPathTree.

public ShortestPathTree getShortestPathTree(State initialState) {
    Vertex target = null;
    if (options.rctx != null) {
        target = initialState.getOptions().rctx.target;
    }
    ShortestPathTree spt = new DominanceFunction.MinimumWeight().getNewShortestPathTree(options);
    BinHeap<State> queue = new BinHeap<State>(1000);
    spt.add(initialState);
    queue.insert(initialState, initialState.getWeight());
    while (!queue.empty()) {
        // Until the priority queue is empty:
        State u = queue.extract_min();
        Vertex u_vertex = u.getVertex();
        if (traverseVisitor != null) {
            traverseVisitor.visitVertex(u);
        }
        if (!spt.getStates(u_vertex).contains(u)) {
            continue;
        }
        if (verbose) {
            System.out.println("min," + u.getWeight());
            System.out.println(u_vertex);
        }
        if (searchTerminationStrategy != null && searchTerminationStrategy.shouldSearchTerminate(initialState.getVertex(), null, u, spt, options)) {
            break;
        }
        for (Edge edge : options.arriveBy ? u_vertex.getIncoming() : u_vertex.getOutgoing()) {
            if (skipEdgeStrategy != null && skipEdgeStrategy.shouldSkipEdge(initialState.getVertex(), null, u, edge, spt, options)) {
                continue;
            }
            // returning NULL), the iteration is over.
            for (State v = edge.traverse(u); v != null; v = v.getNextResult()) {
                if (skipTraverseResultStrategy != null && skipTraverseResultStrategy.shouldSkipTraversalResult(initialState.getVertex(), null, u, v, spt, options)) {
                    continue;
                }
                if (traverseVisitor != null) {
                    traverseVisitor.visitEdge(edge, v);
                }
                if (verbose) {
                    System.out.printf("  w = %f + %f = %f %s", u.getWeight(), v.getWeightDelta(), v.getWeight(), v.getVertex());
                }
                if (v.exceedsWeightLimit(options.maxWeight))
                    continue;
                if (spt.add(v)) {
                    double estimate = heuristic.estimateRemainingWeight(v);
                    queue.insert(v, v.getWeight() + estimate);
                    if (traverseVisitor != null)
                        traverseVisitor.visitEnqueue(v);
                }
            }
        }
    }
    return spt;
}