inline bool StallAtNode(const DataFacadeT &facade,
                         const NodeID node,
                         const EdgeWeight weight,
                         QueryHeap &query_heap) const
 {
     for (auto edge : facade.GetAdjacentEdgeRange(node))
     {
         const auto &data = facade.GetEdgeData(edge);
         const bool reverse_flag = ((!forward_direction) ? data.forward : data.backward);
         if (reverse_flag)
         {
             const NodeID to = facade.GetTarget(edge);
             const int edge_weight = data.weight;
             BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
             if (query_heap.WasInserted(to))
             {
                 if (query_heap.GetKey(to) + edge_weight < weight)
                 {
                     return true;
                 }
             }
         }
     }
     return false;
 }
    inline void RelaxOutgoingEdges(const DataFacadeT &facade,
                                   const NodeID node,
                                   const EdgeWeight weight,
                                   QueryHeap &query_heap) const
    {
        for (auto edge : facade.GetAdjacentEdgeRange(node))
        {
            const auto &data = facade.GetEdgeData(edge);
            const bool direction_flag = (forward_direction ? data.forward : data.backward);
            if (direction_flag)
            {
                const NodeID to = facade.GetTarget(edge);
                const int edge_weight = data.weight;

                BOOST_ASSERT_MSG(edge_weight > 0, "edge_weight invalid");
                const int to_weight = weight + edge_weight;

                // New Node discovered -> Add to Heap + Node Info Storage
                if (!query_heap.WasInserted(to))
                {
                    query_heap.Insert(to, to_weight, node);
                }
                // Found a shorter Path -> Update weight
                else if (to_weight < query_heap.GetKey(to))
                {
                    // new parent
                    query_heap.GetData(to).parent = node;
                    query_heap.DecreaseKey(to, to_weight);
                }
            }
        }
    }
Example #3
0
inline void UnpackCHPath(const DataFacadeT &facade,
                         BidirectionalIterator packed_path_begin,
                         BidirectionalIterator packed_path_end,
                         Callback &&callback)
{
    // make sure we have at least something to unpack
    if (packed_path_begin == packed_path_end)
        return;

    using EdgeData = typename DataFacadeT::EdgeData;

    std::stack<std::pair<NodeID, NodeID>> recursion_stack;

    // We have to push the path in reverse order onto the stack because it's LIFO.
    for (auto current = std::prev(packed_path_end); current != packed_path_begin;
         current = std::prev(current))
    {
        recursion_stack.emplace(*std::prev(current), *current);
    }

    std::pair<NodeID, NodeID> edge;
    while (!recursion_stack.empty())
    {
        edge = recursion_stack.top();
        recursion_stack.pop();

        // Look for an edge on the forward CH graph (.forward)
        EdgeID smaller_edge_id = facade.FindSmallestEdge(
            edge.first, edge.second, [](const EdgeData &data) { return data.forward; });

        // If we didn't find one there, the we might be looking at a part of the path that
        // was found using the backward search.  Here, we flip the node order (.second, .first)
        // and only consider edges with the `.backward` flag.
        if (SPECIAL_EDGEID == smaller_edge_id)
        {
            smaller_edge_id = facade.FindSmallestEdge(
                edge.second, edge.first, [](const EdgeData &data) { return data.backward; });
        }

        // If we didn't find anything *still*, then something is broken and someone has
        // called this function with bad values.
        BOOST_ASSERT_MSG(smaller_edge_id != SPECIAL_EDGEID, "Invalid smaller edge ID");

        const auto &data = facade.GetEdgeData(smaller_edge_id);
        BOOST_ASSERT_MSG(data.distance != std::numeric_limits<EdgeWeight>::max(),
                         "edge weight invalid");

        // If the edge is a shortcut, we need to add the two halfs to the stack.
        if (data.shortcut)
        { // unpack
            const NodeID middle_node_id = data.id;
            // Note the order here - we're adding these to a stack, so we
            // want the first->middle to get visited before middle->second
            recursion_stack.emplace(middle_node_id, edge.second);
            recursion_stack.emplace(edge.first, middle_node_id);
        }
        else
        {
            // We found an original edge, call our callback.
            std::forward<Callback>(callback)(edge, data);
        }
    }
}
Example #4
0
    // conduct T-Test
    bool ViaNodeCandidatePassesTTest(QueryHeap &existing_forward_heap,
                                     QueryHeap &existing_reverse_heap,
                                     QueryHeap &new_forward_heap,
                                     QueryHeap &new_reverse_heap,
                                     const RankedCandidateNode &candidate,
                                     const int length_of_shortest_path,
                                     int *length_of_via_path,
                                     NodeID *s_v_middle,
                                     NodeID *v_t_middle,
                                     const EdgeWeight min_edge_offset) const
    {
        new_forward_heap.Clear();
        new_reverse_heap.Clear();
        std::vector<NodeID> packed_s_v_path;
        std::vector<NodeID> packed_v_t_path;

        *s_v_middle = SPECIAL_NODEID;
        int upper_bound_s_v_path_length = INVALID_EDGE_WEIGHT;
        // compute path <s,..,v> by reusing forward search from s
        new_reverse_heap.Insert(candidate.node, 0, candidate.node);
        while (new_reverse_heap.Size() > 0)
        {
            super::RoutingStep(new_reverse_heap, existing_forward_heap, s_v_middle,
                               &upper_bound_s_v_path_length, min_edge_offset, false);
        }

        if (INVALID_EDGE_WEIGHT == upper_bound_s_v_path_length)
        {
            return false;
        }

        // compute path <v,..,t> by reusing backward search from t
        *v_t_middle = SPECIAL_NODEID;
        int upper_bound_of_v_t_path_length = INVALID_EDGE_WEIGHT;
        new_forward_heap.Insert(candidate.node, 0, candidate.node);
        while (new_forward_heap.Size() > 0)
        {
            super::RoutingStep(new_forward_heap, existing_reverse_heap, v_t_middle,
                               &upper_bound_of_v_t_path_length, min_edge_offset, true);
        }

        if (INVALID_EDGE_WEIGHT == upper_bound_of_v_t_path_length)
        {
            return false;
        }

        *length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;

        // retrieve packed paths
        super::RetrievePackedPathFromHeap(existing_forward_heap, new_reverse_heap, *s_v_middle,
                                          packed_s_v_path);

        super::RetrievePackedPathFromHeap(new_forward_heap, existing_reverse_heap, *v_t_middle,
                                          packed_v_t_path);

        NodeID s_P = *s_v_middle, t_P = *v_t_middle;
        if (SPECIAL_NODEID == s_P)
        {
            return false;
        }

        if (SPECIAL_NODEID == t_P)
        {
            return false;
        }
        const int T_threshold = static_cast<int>(VIAPATH_EPSILON * length_of_shortest_path);
        int unpacked_until_distance = 0;

        std::stack<SearchSpaceEdge> unpack_stack;
        // Traverse path s-->v
        for (std::size_t i = packed_s_v_path.size() - 1; (i > 0) && unpack_stack.empty(); --i)
        {
            const EdgeID current_edge_id =
                facade->FindEdgeInEitherDirection(packed_s_v_path[i - 1], packed_s_v_path[i]);
            const int length_of_current_edge = facade->GetEdgeData(current_edge_id).distance;
            if ((length_of_current_edge + unpacked_until_distance) >= T_threshold)
            {
                unpack_stack.emplace(packed_s_v_path[i - 1], packed_s_v_path[i]);
            }
            else
            {
                unpacked_until_distance += length_of_current_edge;
                s_P = packed_s_v_path[i - 1];
            }
        }

        while (!unpack_stack.empty())
        {
            const SearchSpaceEdge via_path_edge = unpack_stack.top();
            unpack_stack.pop();
            EdgeID edge_in_via_path_id =
                facade->FindEdgeInEitherDirection(via_path_edge.first, via_path_edge.second);

            if (SPECIAL_EDGEID == edge_in_via_path_id)
            {
                return false;
            }

            const EdgeData &current_edge_data = facade->GetEdgeData(edge_in_via_path_id);
            const bool current_edge_is_shortcut = current_edge_data.shortcut;
            if (current_edge_is_shortcut)
            {
                const NodeID via_path_middle_node_id = current_edge_data.id;
                const EdgeID second_segment_edge_id = facade->FindEdgeInEitherDirection(
                        via_path_middle_node_id, via_path_edge.second);
                const int second_segment_length =
                    facade->GetEdgeData(second_segment_edge_id).distance;
                // attention: !unpacking in reverse!
                // Check if second segment is the one to go over treshold? if yes add second segment
                // to stack, else push first segment to stack and add distance of second one.
                if (unpacked_until_distance + second_segment_length >= T_threshold)
                {
                    unpack_stack.emplace(via_path_middle_node_id, via_path_edge.second);
                }
                else
                {
                    unpacked_until_distance += second_segment_length;
                    unpack_stack.emplace(via_path_edge.first, via_path_middle_node_id);
                }
            }
            else
            {
                // edge is not a shortcut, set the start node for T-Test to end of edge.
                unpacked_until_distance += current_edge_data.distance;
                s_P = via_path_edge.first;
            }
        }

        int t_test_path_length = unpacked_until_distance;
        unpacked_until_distance = 0;
        // Traverse path s-->v
        BOOST_ASSERT(!packed_v_t_path.empty());
        for (unsigned i = 0, packed_path_length = static_cast<unsigned>(packed_v_t_path.size() - 1);
                (i < packed_path_length) && unpack_stack.empty(); ++i)
        {
            const EdgeID edgeID =
                facade->FindEdgeInEitherDirection(packed_v_t_path[i], packed_v_t_path[i + 1]);
            int length_of_current_edge = facade->GetEdgeData(edgeID).distance;
            if (length_of_current_edge + unpacked_until_distance >= T_threshold)
            {
                unpack_stack.emplace(packed_v_t_path[i], packed_v_t_path[i + 1]);
            }
            else
            {
                unpacked_until_distance += length_of_current_edge;
                t_P = packed_v_t_path[i + 1];
            }
        }

        while (!unpack_stack.empty())
        {
            const SearchSpaceEdge via_path_edge = unpack_stack.top();
            unpack_stack.pop();
            EdgeID edge_in_via_path_id =
                facade->FindEdgeInEitherDirection(via_path_edge.first, via_path_edge.second);
            if (SPECIAL_EDGEID == edge_in_via_path_id)
            {
                return false;
            }

            const EdgeData &current_edge_data = facade->GetEdgeData(edge_in_via_path_id);
            const bool IsViaEdgeShortCut = current_edge_data.shortcut;
            if (IsViaEdgeShortCut)
            {
                const NodeID middleOfViaPath = current_edge_data.id;
                EdgeID edgeIDOfFirstSegment =
                    facade->FindEdgeInEitherDirection(via_path_edge.first, middleOfViaPath);
                int lengthOfFirstSegment = facade->GetEdgeData(edgeIDOfFirstSegment).distance;
                // Check if first segment is the one to go over treshold? if yes first segment to
                // stack, else push second segment to stack and add distance of first one.
                if (unpacked_until_distance + lengthOfFirstSegment >= T_threshold)
                {
                    unpack_stack.emplace(via_path_edge.first, middleOfViaPath);
                }
                else
                {
                    unpacked_until_distance += lengthOfFirstSegment;
                    unpack_stack.emplace(middleOfViaPath, via_path_edge.second);
                }
            }
            else
            {
                // edge is not a shortcut, set the start node for T-Test to end of edge.
                unpacked_until_distance += current_edge_data.distance;
                t_P = via_path_edge.second;
            }
        }

        t_test_path_length += unpacked_until_distance;
        // Run actual T-Test query and compare if distances equal.
        engine_working_data.InitializeOrClearThirdThreadLocalStorage(
            super::facade->GetNumberOfNodes());

        QueryHeap &forward_heap3 = *engine_working_data.forward_heap_3;
        QueryHeap &reverse_heap3 = *engine_working_data.reverse_heap_3;
        int upper_bound = INVALID_EDGE_WEIGHT;
        NodeID middle = SPECIAL_NODEID;

        forward_heap3.Insert(s_P, 0, s_P);
        reverse_heap3.Insert(t_P, 0, t_P);
        // exploration from s and t until deletemin/(1+epsilon) > _lengt_oO_sShortest_path
        while ((forward_heap3.Size() + reverse_heap3.Size()) > 0)
        {
            if (!forward_heap3.Empty())
            {
                super::RoutingStep(forward_heap3, reverse_heap3, &middle, &upper_bound,
                                   min_edge_offset, true);
            }
            if (!reverse_heap3.Empty())
            {
                super::RoutingStep(reverse_heap3, forward_heap3, &middle, &upper_bound,
                                   min_edge_offset, false);
            }
        }
        return (upper_bound <= t_test_path_length);
    }
Example #5
0
    void AlternativeRoutingStep(QueryHeap &heap1,
                                QueryHeap &heap2,
                                NodeID *middle_node,
                                int *upper_bound_to_shortest_path_distance,
                                std::vector<NodeID> &search_space_intersection,
                                std::vector<SearchSpaceEdge> &search_space,
                                const EdgeWeight min_edge_offset) const
    {
        QueryHeap &forward_heap = (is_forward_directed ? heap1 : heap2);
        QueryHeap &reverse_heap = (is_forward_directed ? heap2 : heap1);

        const NodeID node = forward_heap.DeleteMin();
        const int distance = forward_heap.GetKey(node);
        // const NodeID parentnode = forward_heap.GetData(node).parent;
        // SimpleLogger().Write() << (is_forward_directed ? "[fwd] " : "[rev] ") << "settled edge ("
        // << parentnode << "," << node << "), dist: " << distance;

        const int scaled_distance =
            static_cast<int>((distance + min_edge_offset) / (1. + VIAPATH_EPSILON));
        if ((INVALID_EDGE_WEIGHT != *upper_bound_to_shortest_path_distance) &&
                (scaled_distance > *upper_bound_to_shortest_path_distance))
        {
            forward_heap.DeleteAll();
            return;
        }

        search_space.emplace_back(forward_heap.GetData(node).parent, node);

        if (reverse_heap.WasInserted(node))
        {
            search_space_intersection.emplace_back(node);
            const int new_distance = reverse_heap.GetKey(node) + distance;
            if (new_distance < *upper_bound_to_shortest_path_distance)
            {
                if (new_distance >= 0)
                {
                    *middle_node = node;
                    *upper_bound_to_shortest_path_distance = new_distance;
                    //     SimpleLogger().Write() << "accepted middle_node " << *middle_node << " at
                    //     distance " << new_distance;
                    // } else {
                    //     SimpleLogger().Write() << "discarded middle_node " << *middle_node << "
                    //     at distance " << new_distance;
                }
            }
        }

        for (auto edge : facade->GetAdjacentEdgeRange(node))
        {
            const EdgeData &data = facade->GetEdgeData(edge);
            const bool edge_is_forward_directed =
                (is_forward_directed ? data.forward : data.backward);
            if (edge_is_forward_directed)
            {

                const NodeID to = facade->GetTarget(edge);
                const int edge_weight = data.distance;

                BOOST_ASSERT(edge_weight > 0);
                const int to_distance = distance + edge_weight;

                // New Node discovered -> Add to Heap + Node Info Storage
                if (!forward_heap.WasInserted(to))
                {
                    forward_heap.Insert(to, to_distance, node);
                }
                // Found a shorter Path -> Update distance
                else if (to_distance < forward_heap.GetKey(to))
                {
                    // new parent
                    forward_heap.GetData(to).parent = node;
                    // decreased distance
                    forward_heap.DecreaseKey(to, to_distance);
                }
            }
        }
    }
Example #6
0
    // TODO: reorder parameters
    // compute and unpack <s,..,v> and <v,..,t> by exploring search spaces
    // from v and intersecting against queues. only half-searches have to be
    // done at this stage
    void ComputeLengthAndSharingOfViaPath(const NodeID via_node,
                                          int *real_length_of_via_path,
                                          int *sharing_of_via_path,
                                          const std::vector<NodeID> &packed_shortest_path,
                                          const EdgeWeight min_edge_offset)
    {
        engine_working_data.InitializeOrClearSecondThreadLocalStorage(
            super::facade->GetNumberOfNodes());

        QueryHeap &existing_forward_heap = *engine_working_data.forward_heap_1;
        QueryHeap &existing_reverse_heap = *engine_working_data.reverse_heap_1;
        QueryHeap &new_forward_heap = *engine_working_data.forward_heap_2;
        QueryHeap &new_reverse_heap = *engine_working_data.reverse_heap_2;

        std::vector<NodeID> packed_s_v_path;
        std::vector<NodeID> packed_v_t_path;

        std::vector<NodeID> partially_unpacked_shortest_path;
        std::vector<NodeID> partially_unpacked_via_path;

        NodeID s_v_middle = SPECIAL_NODEID;
        int upper_bound_s_v_path_length = INVALID_EDGE_WEIGHT;
        new_reverse_heap.Insert(via_node, 0, via_node);
        // compute path <s,..,v> by reusing forward search from s
        while (!new_reverse_heap.Empty())
        {
            super::RoutingStep(new_reverse_heap, existing_forward_heap, &s_v_middle,
                               &upper_bound_s_v_path_length, min_edge_offset, false);
        }
        // compute path <v,..,t> by reusing backward search from node t
        NodeID v_t_middle = SPECIAL_NODEID;
        int upper_bound_of_v_t_path_length = INVALID_EDGE_WEIGHT;
        new_forward_heap.Insert(via_node, 0, via_node);
        while (!new_forward_heap.Empty())
        {
            super::RoutingStep(new_forward_heap, existing_reverse_heap, &v_t_middle,
                               &upper_bound_of_v_t_path_length, min_edge_offset, true);
        }
        *real_length_of_via_path = upper_bound_s_v_path_length + upper_bound_of_v_t_path_length;

        if (SPECIAL_NODEID == s_v_middle || SPECIAL_NODEID == v_t_middle)
        {
            return;
        }

        // retrieve packed paths
        super::RetrievePackedPathFromHeap(existing_forward_heap, new_reverse_heap, s_v_middle,
                                          packed_s_v_path);
        super::RetrievePackedPathFromHeap(new_forward_heap, existing_reverse_heap, v_t_middle,
                                          packed_v_t_path);

        // partial unpacking, compute sharing
        // First partially unpack s-->v until paths deviate, note length of common path.
        const int64_t s_v_min_path_size =
            std::min(packed_s_v_path.size(), packed_shortest_path.size()) - 1;
        for (const int64_t current_node : osrm::irange<int64_t>(0, s_v_min_path_size))
        {
            if (packed_s_v_path[current_node] == packed_shortest_path[current_node] &&
                    packed_s_v_path[current_node + 1] == packed_shortest_path[current_node + 1])
            {
                EdgeID edgeID = facade->FindEdgeInEitherDirection(
                                    packed_s_v_path[current_node], packed_s_v_path[current_node + 1]);
                *sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
            }
            else
            {
                if (packed_s_v_path[current_node] == packed_shortest_path[current_node])
                {
                    super::UnpackEdge(packed_s_v_path[current_node],
                                      packed_s_v_path[current_node + 1],
                                      partially_unpacked_via_path);
                    super::UnpackEdge(packed_shortest_path[current_node],
                                      packed_shortest_path[current_node + 1],
                                      partially_unpacked_shortest_path);
                    break;
                }
            }
        }
        // traverse partially unpacked edge and note common prefix
        const int64_t packed_path_length =
            std::min(partially_unpacked_via_path.size(), partially_unpacked_shortest_path.size()) -
            1;
        for (int64_t current_node = 0; (current_node < packed_path_length) &&
                (partially_unpacked_via_path[current_node] ==
                 partially_unpacked_shortest_path[current_node] &&
                 partially_unpacked_via_path[current_node + 1] ==
                 partially_unpacked_shortest_path[current_node + 1]);
                ++current_node)
        {
            EdgeID selected_edge =
                facade->FindEdgeInEitherDirection(partially_unpacked_via_path[current_node],
                                                  partially_unpacked_via_path[current_node + 1]);
            *sharing_of_via_path += facade->GetEdgeData(selected_edge).distance;
        }

        // Second, partially unpack v-->t in reverse order until paths deviate and note lengths
        int64_t via_path_index = packed_v_t_path.size() - 1;
        int64_t shortest_path_index = packed_shortest_path.size() - 1;
        for (; via_path_index > 0 && shortest_path_index > 0;
                --via_path_index, --shortest_path_index)
        {
            if (packed_v_t_path[via_path_index - 1] ==
                    packed_shortest_path[shortest_path_index - 1] &&
                    packed_v_t_path[via_path_index] == packed_shortest_path[shortest_path_index])
            {
                EdgeID edgeID = facade->FindEdgeInEitherDirection(
                                    packed_v_t_path[via_path_index - 1], packed_v_t_path[via_path_index]);
                *sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
            }
            else
            {
                if (packed_v_t_path[via_path_index] == packed_shortest_path[shortest_path_index])
                {
                    super::UnpackEdge(packed_v_t_path[via_path_index - 1],
                                      packed_v_t_path[via_path_index], partially_unpacked_via_path);
                    super::UnpackEdge(packed_shortest_path[shortest_path_index - 1],
                                      packed_shortest_path[shortest_path_index],
                                      partially_unpacked_shortest_path);
                    break;
                }
            }
        }

        via_path_index = partially_unpacked_via_path.size() - 1;
        shortest_path_index = partially_unpacked_shortest_path.size() - 1;
        for (; via_path_index > 0 && shortest_path_index > 0;
                --via_path_index, --shortest_path_index)
        {
            if (partially_unpacked_via_path[via_path_index - 1] ==
                    partially_unpacked_shortest_path[shortest_path_index - 1] &&
                    partially_unpacked_via_path[via_path_index] ==
                    partially_unpacked_shortest_path[shortest_path_index])
            {
                EdgeID edgeID = facade->FindEdgeInEitherDirection(
                                    partially_unpacked_via_path[via_path_index - 1],
                                    partially_unpacked_via_path[via_path_index]);
                *sharing_of_via_path += facade->GetEdgeData(edgeID).distance;
            }
            else
            {
                break;
            }
        }
        // finished partial unpacking spree! Amount of sharing is stored to appropriate pointer
        // variable
    }