Preserve heap state in map matching

2018-04-27 05:36:52 +00:00
parent 89fabc1b9c
commit b630b4e32a
7 changed files with 185 additions and 98 deletions
@@ -44,50 +44,19 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta
 bool needsLoopForward(const PhantomNodes &phantoms);
 bool needsLoopBackwards(const PhantomNodes &phantoms);

-template <typename Heap>
-void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
+namespace detail
 {
-    const auto &source = nodes.source_phantom;
-    if (source.IsValidForwardSource())
-    {
-        forward_heap.Insert(source.forward_segment_id.id,
-                            -source.GetForwardWeightPlusOffset(),
-                            source.forward_segment_id.id);
-    }
-
-    if (source.IsValidReverseSource())
-    {
-        forward_heap.Insert(source.reverse_segment_id.id,
-                            -source.GetReverseWeightPlusOffset(),
-                            source.reverse_segment_id.id);
-    }
-
-    const auto &target = nodes.target_phantom;
-    if (target.IsValidForwardTarget())
-    {
-        reverse_heap.Insert(target.forward_segment_id.id,
-                            target.GetForwardWeightPlusOffset(),
-                            target.forward_segment_id.id);
-    }
-
-    if (target.IsValidReverseTarget())
-    {
-        reverse_heap.Insert(target.reverse_segment_id.id,
-                            target.GetReverseWeightPlusOffset(),
-                            target.reverse_segment_id.id);
-    }
-}
-
-template <typename ManyToManyQueryHeap>
-void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
+template <typename Algorithm>
+void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
+                        const PhantomNode &phantom_node)
 {
-    if (phantom_node.IsValidForwardSource())
+    if (phantom_node.IsValidForwardTarget())
    {
        heap.Insert(phantom_node.forward_segment_id.id,
                    -phantom_node.GetForwardWeightPlusOffset(),
                    {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
    }
-    if (phantom_node.IsValidReverseSource())
+    if (phantom_node.IsValidReverseTarget())
    {
        heap.Insert(phantom_node.reverse_segment_id.id,
                    -phantom_node.GetReverseWeightPlusOffset(),
@@ -95,8 +64,9 @@ void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no
    }
 }

-template <typename ManyToManyQueryHeap>
-void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
+template <typename Algorithm>
+void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
+                        const PhantomNode &phantom_node)
 {
    if (phantom_node.IsValidForwardTarget())
    {
@@ -112,6 +82,109 @@ void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no
    }
 }

+template <typename Algorithm>
+void insertSourceInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
+                        const PhantomNode &phantom_node)
+{
+    if (phantom_node.IsValidForwardSource())
+    {
+        heap.Insert(phantom_node.forward_segment_id.id,
+                    -phantom_node.GetForwardWeightPlusOffset(),
+                    phantom_node.forward_segment_id.id);
+    }
+    if (phantom_node.IsValidReverseSource())
+    {
+        heap.Insert(phantom_node.reverse_segment_id.id,
+                    -phantom_node.GetReverseWeightPlusOffset(),
+                    phantom_node.reverse_segment_id.id);
+    }
+}
+
+template <typename Algorithm>
+void insertTargetInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
+                        const PhantomNode &phantom_node)
+{
+    if (phantom_node.IsValidForwardTarget())
+    {
+        heap.Insert(phantom_node.forward_segment_id.id,
+                    phantom_node.GetForwardWeightPlusOffset(),
+                    phantom_node.forward_segment_id.id);
+    }
+    if (phantom_node.IsValidReverseTarget())
+    {
+        heap.Insert(phantom_node.reverse_segment_id.id,
+                    phantom_node.GetReverseWeightPlusOffset(),
+                    phantom_node.reverse_segment_id.id);
+    }
+}
+} // namespace detail
+
+inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
+}
+inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
+}
+inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
+}
+inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
+}
+inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
+}
+inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
+}
+inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
+}
+inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
+                               const PhantomNode &phantom_node)
+{
+    detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
+}
+
+template <typename Heap>
+void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
+{
+    insertSourceInHeap(forward_heap, nodes.source_phantom);
+    insertTargetInHeap(reverse_heap, nodes.target_phantom);
+}
+
+template <typename Algorithm>
+void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
+                        const PhantomNode &phantom_node)
+{
+    if (phantom_node.IsValidForwardSource())
+    {
+        heap.Insert(phantom_node.forward_segment_id.id,
+                    -phantom_node.GetForwardWeightPlusOffset(),
+                    {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
+    }
+    if (phantom_node.IsValidReverseSource())
+    {
+        heap.Insert(phantom_node.reverse_segment_id.id,
+                    -phantom_node.GetReverseWeightPlusOffset(),
+                    {phantom_node.reverse_segment_id.id, -phantom_node.GetReverseDuration()});
+    }
+}
+
 template <typename FacadeT>
 void annotatePath(const FacadeT &facade,
                  const PhantomNodes &phantom_node_pair,
@@ -321,10 +394,10 @@ void annotatePath(const FacadeT &facade,
    }
 }

-void adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
-                                      const PhantomNode &source_phantom,
-                                      const PhantomNode &target_phantom,
-                                      EdgeDistance &distance);
+EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
+                                              const PhantomNode &source_phantom,
+                                              const PhantomNode &target_phantom,
+                                              const EdgeDistance distance);

 template <typename AlgorithmT>
 InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,
@@ -97,7 +97,6 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
                                 const std::vector<std::size_t> &phantom_indices)
 {
    auto min_level = [&partition, node](const PhantomNode &phantom_node) {
-
        const auto &forward_segment = phantom_node.forward_segment_id;
        const auto forward_level =
            forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id)
@@ -391,21 +390,27 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
                    EdgeWeight weight_upper_bound,
                    Args... args)
 {
-    if (forward_heap.Empty() || reverse_heap.Empty())
+    if (forward_heap.Empty() && reverse_heap.Empty())
    {
        return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
    }

    const auto &partition = facade.GetMultiLevelPartition();

-    BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
-    BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
+    BOOST_ASSERT(forward_heap.Empty() || forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
+    BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);

    // run two-Target Dijkstra routing step.
    NodeID middle = SPECIAL_NODEID;
    EdgeWeight weight = weight_upper_bound;
-    EdgeWeight forward_heap_min = forward_heap.MinKey();
-    EdgeWeight reverse_heap_min = reverse_heap.MinKey();
+
+    EdgeWeight forward_heap_min = 0;
+    if (!forward_heap.Empty())
+        forward_heap_min = forward_heap.MinKey();
+    EdgeWeight reverse_heap_min = 0;
+    if (!reverse_heap.Empty())
+        reverse_heap_min = reverse_heap.MinKey();
+
    while (forward_heap.Size() + reverse_heap.Size() > 0 &&
           forward_heap_min + reverse_heap_min < weight)
    {
@@ -657,11 +662,7 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                          const PhantomNode &target_phantom,
                          EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
 {
-    forward_heap.Clear();
-    reverse_heap.Clear();
-
    const PhantomNodes phantom_nodes{source_phantom, target_phantom};
-    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);

    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    std::vector<NodeID> unpacked_nodes;
@@ -684,16 +685,18 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,

    if (!unpacked_nodes.empty())
    {
-        for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); node_iter++)
-        {
-            distance += computeEdgeDistance(facade, *node_iter);
-        }
+        distance = std::accumulate(unpacked_nodes.begin(),
+                                   std::prev(unpacked_nodes.end()),
+                                   EdgeDistance{0},
+                                   [&](const EdgeDistance distance, const auto node_id) {
+                                       return distance + computeEdgeDistance(facade, node_id);
+                                   });
    }

-    adjustPathDistanceToPhantomNodes(
+    distance = adjustPathDistanceToPhantomNodes(
        unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance);

-    return distance / 10.;
+    return distance;
 }

 } // namespace mld