Exemplo n.º 1
0
InternalRouteResult directShortestPathSearch(
    SearchEngineData<mld::Algorithm> &engine_working_data,
    const datafacade::ContiguousInternalMemoryDataFacade<mld::Algorithm> &facade,
    const PhantomNodes &phantom_nodes)
{
    engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());
    auto &forward_heap = *engine_working_data.forward_heap_1;
    auto &reverse_heap = *engine_working_data.reverse_heap_1;
    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);

    // TODO: when structured bindings will be allowed change to
    // auto [weight, source_node, target_node, unpacked_edges] = ...
    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    std::vector<NodeID> unpacked_nodes;
    std::vector<EdgeID> unpacked_edges;
    std::tie(weight, unpacked_nodes, unpacked_edges) = mld::search(engine_working_data,
                                                                   facade,
                                                                   forward_heap,
                                                                   reverse_heap,
                                                                   DO_NOT_FORCE_LOOPS,
                                                                   DO_NOT_FORCE_LOOPS,
                                                                   INVALID_EDGE_WEIGHT,
                                                                   phantom_nodes);

    return extractRoute(facade, weight, phantom_nodes, unpacked_nodes, unpacked_edges);
}
Exemplo n.º 2
0
InternalRouteResult
directShortestPathSearch(SearchEngineData<Algorithm> &engine_working_data,
                         const datafacade::ContiguousInternalMemoryDataFacade<Algorithm> &facade,
                         const PhantomNodes &phantom_nodes)
{
    engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());
    auto &forward_heap = *engine_working_data.forward_heap_1;
    auto &reverse_heap = *engine_working_data.reverse_heap_1;
    forward_heap.Clear();
    reverse_heap.Clear();

    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    std::vector<NodeID> packed_leg;
    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);

    search(engine_working_data,
           facade,
           forward_heap,
           reverse_heap,
           weight,
           packed_leg,
           DO_NOT_FORCE_LOOPS,
           DO_NOT_FORCE_LOOPS,
           phantom_nodes);

    std::vector<NodeID> unpacked_nodes;
    std::vector<EdgeID> unpacked_edges;

    if (!packed_leg.empty())
    {
        unpacked_nodes.reserve(packed_leg.size());
        unpacked_edges.reserve(packed_leg.size());
        unpacked_nodes.push_back(packed_leg.front());
        ch::unpackPath(facade,
                       packed_leg.begin(),
                       packed_leg.end(),
                       [&unpacked_nodes, &unpacked_edges](std::pair<NodeID, NodeID> &edge,
                                                          const auto &edge_id) {
                           BOOST_ASSERT(edge.first == unpacked_nodes.back());
                           unpacked_nodes.push_back(edge.second);
                           unpacked_edges.push_back(edge_id);
                       });
    }

    return extractRoute(facade, weight, phantom_nodes, unpacked_nodes, unpacked_edges);
}
Exemplo n.º 3
0
std::vector<TurnData>
getTileTurns(const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CH> &facade,
             const std::vector<RTreeLeaf> &edges,
             const std::vector<std::size_t> &sorted_edge_indexes)
{
    std::vector<TurnData> all_turn_data;

    // Struct to hold info on all the EdgeBasedNodes that are visible in our tile
    // When we create these, we insure that (source, target) and packed_geometry_id
    // are all pointed in the same direction.
    struct EdgeBasedNodeInfo
    {
        bool is_geometry_forward; // Is the geometry forward or reverse?
        unsigned packed_geometry_id;
    };
    // Lookup table for edge-based-nodes
    std::unordered_map<NodeID, EdgeBasedNodeInfo> edge_based_node_info;

    struct SegmentData
    {
        NodeID target_node;
        EdgeID edge_based_node_id;
    };

    std::unordered_map<NodeID, std::vector<SegmentData>> directed_graph;
    // Reserve enough space for unique edge-based-nodes on every edge.
    // Only a tile with all unique edges will use this much, but
    // it saves us a bunch of re-allocations during iteration.
    directed_graph.reserve(edges.size() * 2);

    // Build an adjacency list for all the road segments visible in
    // the tile
    for (const auto &edge_index : sorted_edge_indexes)
    {
        const auto &edge = edges[edge_index];
        if (edge.forward_segment_id.enabled)
        {
            // operator[] will construct an empty vector at [edge.u] if there is no value.
            directed_graph[edge.u].push_back({edge.v, edge.forward_segment_id.id});
            if (edge_based_node_info.count(edge.forward_segment_id.id) == 0)
            {
                edge_based_node_info[edge.forward_segment_id.id] = {true, edge.packed_geometry_id};
            }
            else
            {
                BOOST_ASSERT(edge_based_node_info[edge.forward_segment_id.id].is_geometry_forward ==
                             true);
                BOOST_ASSERT(edge_based_node_info[edge.forward_segment_id.id].packed_geometry_id ==
                             edge.packed_geometry_id);
            }
        }
        if (edge.reverse_segment_id.enabled)
        {
            directed_graph[edge.v].push_back({edge.u, edge.reverse_segment_id.id});
            if (edge_based_node_info.count(edge.reverse_segment_id.id) == 0)
            {
                edge_based_node_info[edge.reverse_segment_id.id] = {false, edge.packed_geometry_id};
            }
            else
            {
                BOOST_ASSERT(edge_based_node_info[edge.reverse_segment_id.id].is_geometry_forward ==
                             false);
                BOOST_ASSERT(edge_based_node_info[edge.reverse_segment_id.id].packed_geometry_id ==
                             edge.packed_geometry_id);
            }
        }
    }

    // Given a turn:
    //     u---v
    //         |
    //         w
    //  uv is the "approach"
    //  vw is the "exit"
    std::vector<contractor::QueryEdge::EdgeData> unpacked_shortcut;
    std::vector<EdgeWeight> approach_weight_vector;
    std::vector<EdgeWeight> approach_duration_vector;

    // Make sure we traverse the startnodes in a consistent order
    // to ensure identical PBF encoding on all platforms.
    std::vector<NodeID> sorted_startnodes;
    sorted_startnodes.reserve(directed_graph.size());
    for (const auto &startnode : directed_graph)
        sorted_startnodes.push_back(startnode.first);
    std::sort(sorted_startnodes.begin(), sorted_startnodes.end());

    // Look at every node in the directed graph we created
    for (const auto &startnode : sorted_startnodes)
    {
        const auto &nodedata = directed_graph[startnode];
        // For all the outgoing edges from the node
        for (const auto &approachedge : nodedata)
        {
            // If the target of this edge doesn't exist in our directed
            // graph, it's probably outside the tile, so we can skip it
            if (directed_graph.count(approachedge.target_node) == 0)
                continue;

            // For each of the outgoing edges from our target coordinate
            for (const auto &exit_edge : directed_graph[approachedge.target_node])
            {
                // If the next edge has the same edge_based_node_id, then it's
                // not a turn, so skip it
                if (approachedge.edge_based_node_id == exit_edge.edge_based_node_id)
                    continue;

                // Skip u-turns
                if (startnode == exit_edge.target_node)
                    continue;

                // Find the connection between our source road and the target node
                // Since we only want to find direct edges, we cannot check shortcut edges here.
                // Otherwise we might find a forward edge even though a shorter backward edge
                // exists (due to oneways).
                //
                // a > - > - > - b
                // |             |
                // |------ c ----|
                //
                // would offer a backward edge at `b` to `a` (due to the oneway from a to b)
                // but could also offer a shortcut (b-c-a) from `b` to `a` which is longer.
                EdgeID smaller_edge_id =
                    facade.FindSmallestEdge(approachedge.edge_based_node_id,
                                            exit_edge.edge_based_node_id,
                                            [](const contractor::QueryEdge::EdgeData &data) {
                                                return data.forward && !data.shortcut;
                                            });

                // Depending on how the graph is constructed, we might have to look for
                // a backwards edge instead.  They're equivalent, just one is available for
                // a forward routing search, and one is used for the backwards dijkstra
                // steps.  Their weight should be the same, we can use either one.
                // If we didn't find a forward edge, try for a backward one
                if (SPECIAL_EDGEID == smaller_edge_id)
                {
                    smaller_edge_id =
                        facade.FindSmallestEdge(exit_edge.edge_based_node_id,
                                                approachedge.edge_based_node_id,
                                                [](const contractor::QueryEdge::EdgeData &data) {
                                                    return data.backward && !data.shortcut;
                                                });
                }

                // If no edge was found, it means that there's no connection between these
                // nodes, due to oneways or turn restrictions. Given the edge-based-nodes
                // that we're examining here, we *should* only find directly-connected
                // edges, not shortcuts
                if (smaller_edge_id != SPECIAL_EDGEID)
                {
                    const auto &data = facade.GetEdgeData(smaller_edge_id);
                    BOOST_ASSERT_MSG(!data.shortcut, "Connecting edge must not be a shortcut");

                    // Now, calculate the sum of the weight of all the segments.
                    if (edge_based_node_info[approachedge.edge_based_node_id].is_geometry_forward)
                    {
                        approach_weight_vector = facade.GetUncompressedForwardWeights(
                            edge_based_node_info[approachedge.edge_based_node_id]
                                .packed_geometry_id);
                        approach_duration_vector = facade.GetUncompressedForwardDurations(
                            edge_based_node_info[approachedge.edge_based_node_id]
                                .packed_geometry_id);
                    }
                    else
                    {
                        approach_weight_vector = facade.GetUncompressedReverseWeights(
                            edge_based_node_info[approachedge.edge_based_node_id]
                                .packed_geometry_id);
                        approach_duration_vector = facade.GetUncompressedReverseDurations(
                            edge_based_node_info[approachedge.edge_based_node_id]
                                .packed_geometry_id);
                    }
                    const auto sum_node_weight = std::accumulate(approach_weight_vector.begin(),
                                                                 approach_weight_vector.end(),
                                                                 EdgeWeight{0});
                    const auto sum_node_duration = std::accumulate(approach_duration_vector.begin(),
                                                                   approach_duration_vector.end(),
                                                                   EdgeWeight{0});

                    // The edge.weight is the whole edge weight, which includes the turn
                    // cost.
                    // The turn cost is the edge.weight minus the sum of the individual road
                    // segment weights.  This might not be 100% accurate, because some
                    // intersections include stop signs, traffic signals and other
                    // penalties, but at this stage, we can't divide those out, so we just
                    // treat the whole lot as the "turn cost" that we'll stick on the map.
                    const auto turn_weight = data.weight - sum_node_weight;
                    const auto turn_duration = data.duration - sum_node_duration;

                    // Find the three nodes that make up the turn movement)
                    const auto node_from = startnode;
                    const auto node_via = approachedge.target_node;
                    const auto node_to = exit_edge.target_node;

                    const auto coord_from = facade.GetCoordinateOfNode(node_from);
                    const auto coord_via = facade.GetCoordinateOfNode(node_via);
                    const auto coord_to = facade.GetCoordinateOfNode(node_to);

                    // Calculate the bearing that we approach the intersection at
                    const auto angle_in = static_cast<int>(
                        util::coordinate_calculation::bearing(coord_from, coord_via));

                    const auto exit_bearing = static_cast<int>(
                        util::coordinate_calculation::bearing(coord_via, coord_to));

                    // Figure out the angle of the turn
                    auto turn_angle = exit_bearing - angle_in;
                    while (turn_angle > 180)
                    {
                        turn_angle -= 360;
                    }
                    while (turn_angle < -180)
                    {
                        turn_angle += 360;
                    }

                    // Save everything we need to later add all the points to the tile.
                    // We need the coordinate of the intersection, the angle in, the turn
                    // angle and the turn cost.
                    all_turn_data.push_back(
                        TurnData{coord_via, angle_in, turn_angle, turn_weight, turn_duration});
                }
            }
        }
    }

    return all_turn_data;
}
Exemplo n.º 4
0
InternalRouteResult
shortestPathSearch(SearchEngineData<Algorithm> &engine_working_data,
                   const datafacade::ContiguousInternalMemoryDataFacade<Algorithm> &facade,
                   const std::vector<PhantomNodes> &phantom_nodes_vector,
                   const boost::optional<bool> continue_straight_at_waypoint)
{
    InternalRouteResult raw_route_data;
    raw_route_data.segment_end_coordinates = phantom_nodes_vector;
    const bool allow_uturn_at_waypoint =
        !(continue_straight_at_waypoint ? *continue_straight_at_waypoint
                                        : facade.GetContinueStraightDefault());

    engine_working_data.InitializeOrClearFirstThreadLocalStorage(facade.GetNumberOfNodes());

    auto &forward_heap = *engine_working_data.forward_heap_1;
    auto &reverse_heap = *engine_working_data.reverse_heap_1;

    int total_weight_to_forward = 0;
    int total_weight_to_reverse = 0;
    bool search_from_forward_node =
        phantom_nodes_vector.front().source_phantom.IsValidForwardSource();
    bool search_from_reverse_node =
        phantom_nodes_vector.front().source_phantom.IsValidReverseSource();

    std::vector<NodeID> prev_packed_leg_to_forward;
    std::vector<NodeID> prev_packed_leg_to_reverse;

    std::vector<NodeID> total_packed_path_to_forward;
    std::vector<std::size_t> packed_leg_to_forward_begin;
    std::vector<NodeID> total_packed_path_to_reverse;
    std::vector<std::size_t> packed_leg_to_reverse_begin;

    std::size_t current_leg = 0;
    // this implements a dynamic program that finds the shortest route through
    // a list of vias
    for (const auto &phantom_node_pair : phantom_nodes_vector)
    {
        int new_total_weight_to_forward = INVALID_EDGE_WEIGHT;
        int new_total_weight_to_reverse = INVALID_EDGE_WEIGHT;

        std::vector<NodeID> packed_leg_to_forward;
        std::vector<NodeID> packed_leg_to_reverse;

        const auto &source_phantom = phantom_node_pair.source_phantom;
        const auto &target_phantom = phantom_node_pair.target_phantom;

        bool search_to_forward_node = target_phantom.IsValidForwardTarget();
        bool search_to_reverse_node = target_phantom.IsValidReverseTarget();

        BOOST_ASSERT(!search_from_forward_node || source_phantom.IsValidForwardSource());
        BOOST_ASSERT(!search_from_reverse_node || source_phantom.IsValidReverseSource());

        if (search_to_reverse_node || search_to_forward_node)
        {
            if (allow_uturn_at_waypoint)
            {
                searchWithUTurn(engine_working_data,
                                facade,
                                forward_heap,
                                reverse_heap,
                                search_from_forward_node,
                                search_from_reverse_node,
                                search_to_forward_node,
                                search_to_reverse_node,
                                source_phantom,
                                target_phantom,
                                total_weight_to_forward,
                                total_weight_to_reverse,
                                new_total_weight_to_forward,
                                packed_leg_to_forward);
                // if only the reverse node is valid (e.g. when using the match plugin) we
                // actually need to move
                if (!target_phantom.IsValidForwardTarget())
                {
                    BOOST_ASSERT(target_phantom.IsValidReverseTarget());
                    new_total_weight_to_reverse = new_total_weight_to_forward;
                    packed_leg_to_reverse = std::move(packed_leg_to_forward);
                    new_total_weight_to_forward = INVALID_EDGE_WEIGHT;

                    // (*)
                    //
                    //   Below we have to check if new_total_weight_to_forward is invalid.
                    //   This prevents use-after-move on packed_leg_to_forward.
                }
                else if (target_phantom.IsValidReverseTarget())
                {
                    new_total_weight_to_reverse = new_total_weight_to_forward;
                    packed_leg_to_reverse = packed_leg_to_forward;
                }
            }
            else
            {
                search(engine_working_data,
                       facade,
                       forward_heap,
                       reverse_heap,
                       search_from_forward_node,
                       search_from_reverse_node,
                       search_to_forward_node,
                       search_to_reverse_node,
                       source_phantom,
                       target_phantom,
                       total_weight_to_forward,
                       total_weight_to_reverse,
                       new_total_weight_to_forward,
                       new_total_weight_to_reverse,
                       packed_leg_to_forward,
                       packed_leg_to_reverse);
            }
        }

        // Note: To make sure we do not access the moved-from packed_leg_to_forward
        // we guard its access by a check for invalid edge weight. See  (*) above.

        // No path found for both target nodes?
        if ((INVALID_EDGE_WEIGHT == new_total_weight_to_forward) &&
            (INVALID_EDGE_WEIGHT == new_total_weight_to_reverse))
        {
            return raw_route_data;
        }

        // we need to figure out how the new legs connect to the previous ones
        if (current_leg > 0)
        {
            bool forward_to_forward =
                (new_total_weight_to_forward != INVALID_EDGE_WEIGHT) &&
                packed_leg_to_forward.front() == source_phantom.forward_segment_id.id;
            bool reverse_to_forward =
                (new_total_weight_to_forward != INVALID_EDGE_WEIGHT) &&
                packed_leg_to_forward.front() == source_phantom.reverse_segment_id.id;
            bool forward_to_reverse =
                (new_total_weight_to_reverse != INVALID_EDGE_WEIGHT) &&
                packed_leg_to_reverse.front() == source_phantom.forward_segment_id.id;
            bool reverse_to_reverse =
                (new_total_weight_to_reverse != INVALID_EDGE_WEIGHT) &&
                packed_leg_to_reverse.front() == source_phantom.reverse_segment_id.id;

            BOOST_ASSERT(!forward_to_forward || !reverse_to_forward);
            BOOST_ASSERT(!forward_to_reverse || !reverse_to_reverse);

            // in this case we always need to copy
            if (forward_to_forward && forward_to_reverse)
            {
                // in this case we copy the path leading to the source forward node
                // and change the case
                total_packed_path_to_reverse = total_packed_path_to_forward;
                packed_leg_to_reverse_begin = packed_leg_to_forward_begin;
                forward_to_reverse = false;
                reverse_to_reverse = true;
            }
            else if (reverse_to_forward && reverse_to_reverse)
            {
                total_packed_path_to_forward = total_packed_path_to_reverse;
                packed_leg_to_forward_begin = packed_leg_to_reverse_begin;
                reverse_to_forward = false;
                forward_to_forward = true;
            }
            BOOST_ASSERT(!forward_to_forward || !forward_to_reverse);
            BOOST_ASSERT(!reverse_to_forward || !reverse_to_reverse);

            // in this case we just need to swap to regain the correct mapping
            if (reverse_to_forward || forward_to_reverse)
            {
                total_packed_path_to_forward.swap(total_packed_path_to_reverse);
                packed_leg_to_forward_begin.swap(packed_leg_to_reverse_begin);
            }
        }

        if (new_total_weight_to_forward != INVALID_EDGE_WEIGHT)
        {
            BOOST_ASSERT(target_phantom.IsValidForwardTarget());

            packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
            total_packed_path_to_forward.insert(total_packed_path_to_forward.end(),
                                                packed_leg_to_forward.begin(),
                                                packed_leg_to_forward.end());
            search_from_forward_node = true;
        }
        else
        {
            total_packed_path_to_forward.clear();
            packed_leg_to_forward_begin.clear();
            search_from_forward_node = false;
        }

        if (new_total_weight_to_reverse != INVALID_EDGE_WEIGHT)
        {
            BOOST_ASSERT(target_phantom.IsValidReverseTarget());

            packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
            total_packed_path_to_reverse.insert(total_packed_path_to_reverse.end(),
                                                packed_leg_to_reverse.begin(),
                                                packed_leg_to_reverse.end());
            search_from_reverse_node = true;
        }
        else
        {
            total_packed_path_to_reverse.clear();
            packed_leg_to_reverse_begin.clear();
            search_from_reverse_node = false;
        }

        prev_packed_leg_to_forward = std::move(packed_leg_to_forward);
        prev_packed_leg_to_reverse = std::move(packed_leg_to_reverse);

        total_weight_to_forward = new_total_weight_to_forward;
        total_weight_to_reverse = new_total_weight_to_reverse;

        ++current_leg;
    }

    BOOST_ASSERT(total_weight_to_forward != INVALID_EDGE_WEIGHT ||
                 total_weight_to_reverse != INVALID_EDGE_WEIGHT);

    // We make sure the fastest route is always in packed_legs_to_forward
    if (total_weight_to_forward < total_weight_to_reverse ||
        (total_weight_to_forward == total_weight_to_reverse &&
         total_packed_path_to_forward.size() < total_packed_path_to_reverse.size()))
    {
        // insert sentinel
        packed_leg_to_forward_begin.push_back(total_packed_path_to_forward.size());
        BOOST_ASSERT(packed_leg_to_forward_begin.size() == phantom_nodes_vector.size() + 1);

        unpackLegs(facade,
                   phantom_nodes_vector,
                   total_packed_path_to_forward,
                   packed_leg_to_forward_begin,
                   total_weight_to_forward,
                   raw_route_data);
    }
    else
    {
        // insert sentinel
        packed_leg_to_reverse_begin.push_back(total_packed_path_to_reverse.size());
        BOOST_ASSERT(packed_leg_to_reverse_begin.size() == phantom_nodes_vector.size() + 1);

        unpackLegs(facade,
                   phantom_nodes_vector,
                   total_packed_path_to_reverse,
                   packed_leg_to_reverse_begin,
                   total_weight_to_reverse,
                   raw_route_data);
    }

    return raw_route_data;
}
Exemplo n.º 5
0
std::vector<EdgeWeight>
manyToManySearch(SearchEngineData &engine_working_data,
                 const datafacade::ContiguousInternalMemoryDataFacade<algorithm::CH> &facade,
                 const std::vector<PhantomNode> &phantom_nodes,
                 const std::vector<std::size_t> &source_indices,
                 const std::vector<std::size_t> &target_indices)
{
    const auto number_of_sources =
        source_indices.empty() ? phantom_nodes.size() : source_indices.size();
    const auto number_of_targets =
        target_indices.empty() ? phantom_nodes.size() : target_indices.size();
    const auto number_of_entries = number_of_sources * number_of_targets;

    std::vector<EdgeWeight> weights_table(number_of_entries, INVALID_EDGE_WEIGHT);
    std::vector<EdgeWeight> durations_table(number_of_entries, MAXIMAL_EDGE_DURATION);

    engine_working_data.InitializeOrClearManyToManyThreadLocalStorage(facade.GetNumberOfNodes());

    auto &query_heap = *(engine_working_data.many_to_many_heap);

    SearchSpaceWithBuckets search_space_with_buckets;

    unsigned column_idx = 0;
    const auto search_target_phantom = [&](const PhantomNode &phantom) {
        // clear heap and insert target nodes
        query_heap.Clear();
        insertNodesInHeap<REVERSE_DIRECTION>(query_heap, phantom);

        // explore search space
        while (!query_heap.Empty())
        {
            backwardRoutingStep(facade, column_idx, query_heap, search_space_with_buckets);
        }
        ++column_idx;
    };

    // for each source do forward search
    unsigned row_idx = 0;
    const auto search_source_phantom = [&](const PhantomNode &phantom) {
        // clear heap and insert source nodes
        query_heap.Clear();
        insertNodesInHeap<FORWARD_DIRECTION>(query_heap, phantom);

        // explore search space
        while (!query_heap.Empty())
        {
            forwardRoutingStep(facade,
                               row_idx,
                               number_of_targets,
                               query_heap,
                               search_space_with_buckets,
                               weights_table,
                               durations_table);
        }
        ++row_idx;
    };

    if (target_indices.empty())
    {
        for (const auto &phantom : phantom_nodes)
        {
            search_target_phantom(phantom);
        }
    }
    else
    {
        for (const auto index : target_indices)
        {
            const auto &phantom = phantom_nodes[index];
            search_target_phantom(phantom);
        }
    }

    if (source_indices.empty())
    {
        for (const auto &phantom : phantom_nodes)
        {
            search_source_phantom(phantom);
        }
    }
    else
    {
        for (const auto index : source_indices)
        {
            const auto &phantom = phantom_nodes[index];
            search_source_phantom(phantom);
        }
    }

    return durations_table;
}