diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index e00f5c4fa..4d00bf918 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -44,50 +44,19 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta bool needsLoopForward(const PhantomNodes &phantoms); bool needsLoopBackwards(const PhantomNodes &phantoms); -template -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 -void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) +template +void insertSourceInHeap(typename SearchEngineData::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 -void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) +template +void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) { if (phantom_node.IsValidForwardTarget()) { @@ -112,6 +82,109 @@ void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no } } +template +void insertSourceInHeap(typename SearchEngineData::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 +void insertTargetInHeap(typename SearchEngineData::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::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} + +template +void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes) +{ + insertSourceInHeap(forward_heap, nodes.source_phantom); + insertTargetInHeap(reverse_heap, nodes.target_phantom); +} + +template +void insertSourceInHeap(typename SearchEngineData::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 void annotatePath(const FacadeT &facade, const PhantomNodes &phantom_node_pair, @@ -321,10 +394,10 @@ void annotatePath(const FacadeT &facade, } } -void adjustPathDistanceToPhantomNodes(const std::vector &path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - EdgeDistance &distance); +EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const EdgeDistance distance); template InternalRouteResult extractRoute(const DataFacade &facade, diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index d893369f5..a5141a6bc 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -97,7 +97,6 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, const std::vector &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 &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(), std::vector()); } 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 &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 unpacked_nodes; @@ -684,16 +685,18 @@ double getNetworkDistance(SearchEngineData &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 diff --git a/src/engine/routing_algorithms/many_to_many_ch.cpp b/src/engine/routing_algorithms/many_to_many_ch.cpp index 00449b9cb..496bada8f 100644 --- a/src/engine/routing_algorithms/many_to_many_ch.cpp +++ b/src/engine/routing_algorithms/many_to_many_ch.cpp @@ -242,7 +242,8 @@ void calculateDistances(typename SearchEngineData::ManyToManyQuer { EdgeDistance annotation = ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end()); - adjustPathDistanceToPhantomNodes(packed_leg, source_phantom, target_phantom, annotation); + annotation = adjustPathDistanceToPhantomNodes( + packed_leg, source_phantom, target_phantom, annotation); distances_table[row_index * number_of_targets + column_index] = annotation; } diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index 7d1b75901..daefc04c9 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -227,6 +227,9 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, { continue; } + forward_heap.Clear(); + const auto &source_phantom = prev_unbroken_timestamps_list[s].phantom_node; + insertSourceInHeap(forward_heap, source_phantom); for (const auto s_prime : util::irange(0UL, current_viterbi.size())) { @@ -237,14 +240,17 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, continue; } - double network_distance = - getNetworkDistance(engine_working_data, - facade, - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - weight_upper_bound); + reverse_heap.Clear(); + const auto &target_phantom = current_timestamps_list[s_prime].phantom_node; + insertTargetInHeap(reverse_heap, target_phantom); + + double network_distance = getNetworkDistance(engine_working_data, + facade, + forward_heap, + reverse_heap, + source_phantom, + target_phantom, + weight_upper_bound); // get distance diff between loc1/2 and locs/s_prime const auto d_t = std::abs(network_distance - haversine_distance); diff --git a/src/engine/routing_algorithms/routing_base.cpp b/src/engine/routing_algorithms/routing_base.cpp index 236e957fe..472a8cc9e 100644 --- a/src/engine/routing_algorithms/routing_base.cpp +++ b/src/engine/routing_algorithms/routing_base.cpp @@ -33,11 +33,12 @@ bool needsLoopBackwards(const PhantomNodes &phantoms) return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom); } -void adjustPathDistanceToPhantomNodes(const std::vector &path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - EdgeDistance &distance) +EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const EdgeDistance uncorrected_distance) { + EdgeDistance distance = uncorrected_distance; if (!path.empty()) { @@ -97,6 +98,12 @@ void adjustPathDistanceToPhantomNodes(const std::vector &path, distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); } } + + BOOST_ASSERT_MSG(distance >= 0 || distance > -1.0f, + "Distance correction generated negative number"); + // guard against underflow errors caused by rounding + distance = std::max(EdgeDistance{0}, distance); + return distance; } } // namespace routing_algorithms diff --git a/src/engine/routing_algorithms/routing_base_ch.cpp b/src/engine/routing_algorithms/routing_base_ch.cpp index f9f3485bb..d118f86f4 100644 --- a/src/engine/routing_algorithms/routing_base_ch.cpp +++ b/src/engine/routing_algorithms/routing_base_ch.cpp @@ -100,7 +100,7 @@ void search(SearchEngineData & /*engine_working_data*/, const PhantomNodes & /*phantom_nodes*/, const EdgeWeight weight_upper_bound) { - if (forward_heap.Empty() || reverse_heap.Empty()) + if (forward_heap.Empty() && reverse_heap.Empty()) { weight = INVALID_EDGE_WEIGHT; return; @@ -110,10 +110,14 @@ void search(SearchEngineData & /*engine_working_data*/, weight = weight_upper_bound; // get offset to account for offsets on phantom nodes on compressed edges - const auto min_edge_offset = std::min(0, forward_heap.MinKey()); - BOOST_ASSERT(min_edge_offset <= 0); + EdgeWeight min_edge_offset = 0; + if (forward_heap.Size() > 0) + { + min_edge_offset = std::min(min_edge_offset, forward_heap.MinKey()); + BOOST_ASSERT(min_edge_offset <= 0); + } // we only every insert negative offsets for nodes in the forward heap - BOOST_ASSERT(reverse_heap.MinKey() >= 0); + BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() >= 0); // run two-Target Dijkstra routing step. while (0 < (forward_heap.Size() + reverse_heap.Size())) @@ -176,11 +180,6 @@ double getNetworkDistance(SearchEngineData &engine_working_data, const PhantomNode &target_phantom, EdgeWeight weight_upper_bound) { - forward_heap.Clear(); - reverse_heap.Clear(); - - insertNodesInHeaps(forward_heap, reverse_heap, {source_phantom, target_phantom}); - EdgeWeight weight = INVALID_EDGE_WEIGHT; std::vector packed_path; search(engine_working_data, @@ -199,8 +198,6 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } - BOOST_ASSERT(nodes_number > 0); - EdgeDistance distance = 0; std::vector unpacked_nodes; @@ -208,24 +205,24 @@ double getNetworkDistance(SearchEngineData &engine_working_data, if (!packed_path.empty()) { unpacked_nodes.push_back(packed_path.front()); - unpackPath(facade, - packed_path.begin(), - packed_path.end(), - [&](std::pair &edge, const auto &) { - BOOST_ASSERT(edge.first == unpacked_nodes.back()); - unpacked_nodes.push_back(edge.second); - }); + unpackPath( + facade, packed_path.begin(), packed_path.end(), [&](const auto &edge, const auto &) { + BOOST_ASSERT(edge.first == unpacked_nodes.back()); + unpacked_nodes.push_back(edge.second); + }); - 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(unpacked_nodes, source_phantom, target_phantom, distance); + distance = + adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); - return distance / 10.; + return distance; } } // namespace ch diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index 4303375ff..eb852aa30 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -25,7 +25,7 @@ namespace // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) -const constexpr long double EARTH_RADIUS = 6372797.560856; +const constexpr double EARTH_RADIUS = 6372797.560856; class CheapRulerContainer {