diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 4d00bf918..e00f5c4fa 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -44,19 +44,50 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta bool needsLoopForward(const PhantomNodes &phantoms); bool needsLoopBackwards(const PhantomNodes &phantoms); -namespace detail +template +void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes) { -template -void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, - const PhantomNode &phantom_node) + 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) { - if (phantom_node.IsValidForwardTarget()) + 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.IsValidReverseTarget()) + if (phantom_node.IsValidReverseSource()) { heap.Insert(phantom_node.reverse_segment_id.id, -phantom_node.GetReverseWeightPlusOffset(), @@ -64,9 +95,8 @@ void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHea } } -template -void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, - const PhantomNode &phantom_node) +template +void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) { if (phantom_node.IsValidForwardTarget()) { @@ -82,109 +112,6 @@ void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHea } } -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, @@ -394,10 +321,10 @@ void annotatePath(const FacadeT &facade, } } -EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - const EdgeDistance distance); +void adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + 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 a5141a6bc..d893369f5 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -97,6 +97,7 @@ 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) @@ -390,27 +391,21 @@ 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 = 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(); - + EdgeWeight forward_heap_min = forward_heap.MinKey(); + EdgeWeight reverse_heap_min = reverse_heap.MinKey(); while (forward_heap.Size() + reverse_heap.Size() > 0 && forward_heap_min + reverse_heap_min < weight) { @@ -662,7 +657,11 @@ 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; @@ -685,18 +684,16 @@ double getNetworkDistance(SearchEngineData &engine_working_data, if (!unpacked_nodes.empty()) { - 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); - }); + for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); node_iter++) + { + distance += computeEdgeDistance(facade, *node_iter); + } } - distance = adjustPathDistanceToPhantomNodes( + adjustPathDistanceToPhantomNodes( unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance); - return distance; + return distance / 10.; } } // 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 496bada8f..00449b9cb 100644 --- a/src/engine/routing_algorithms/many_to_many_ch.cpp +++ b/src/engine/routing_algorithms/many_to_many_ch.cpp @@ -242,8 +242,7 @@ void calculateDistances(typename SearchEngineData::ManyToManyQuer { EdgeDistance annotation = ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end()); - annotation = adjustPathDistanceToPhantomNodes( - packed_leg, source_phantom, target_phantom, 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 daefc04c9..7d1b75901 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -227,9 +227,6 @@ 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())) { @@ -240,17 +237,14 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, continue; } - 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); + 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); // 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 472a8cc9e..236e957fe 100644 --- a/src/engine/routing_algorithms/routing_base.cpp +++ b/src/engine/routing_algorithms/routing_base.cpp @@ -33,12 +33,11 @@ bool needsLoopBackwards(const PhantomNodes &phantoms) return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom); } -EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom, - const EdgeDistance uncorrected_distance) +void adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + EdgeDistance &distance) { - EdgeDistance distance = uncorrected_distance; if (!path.empty()) { @@ -98,12 +97,6 @@ EdgeDistance 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 d118f86f4..f9f3485bb 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,14 +110,10 @@ void search(SearchEngineData & /*engine_working_data*/, weight = weight_upper_bound; // get offset to account for offsets on phantom nodes on compressed edges - 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); - } + const auto min_edge_offset = std::min(0, 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.Empty() || reverse_heap.MinKey() >= 0); + BOOST_ASSERT(reverse_heap.MinKey() >= 0); // run two-Target Dijkstra routing step. while (0 < (forward_heap.Size() + reverse_heap.Size())) @@ -180,6 +176,11 @@ 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, @@ -198,6 +199,8 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } + BOOST_ASSERT(nodes_number > 0); + EdgeDistance distance = 0; std::vector unpacked_nodes; @@ -205,24 +208,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(), [&](const auto &edge, const auto &) { - BOOST_ASSERT(edge.first == unpacked_nodes.back()); - unpacked_nodes.push_back(edge.second); - }); + 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); + }); - 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); - }); + for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); + node_iter++) + { + distance += computeEdgeDistance(facade, *node_iter); + } } - distance = - adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); + adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); - return distance; + return distance / 10.; } } // namespace ch diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index eb852aa30..4303375ff 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 double EARTH_RADIUS = 6372797.560856; +const constexpr long double EARTH_RADIUS = 6372797.560856; class CheapRulerContainer {