diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index f1a89f195..e00f5c4fa 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -321,24 +321,10 @@ void annotatePath(const FacadeT &facade, } } -template -double getPathDistance(const DataFacade &facade, - const std::vector &unpacked_path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom) -{ - double distance = 0; - auto prev_coordinate = source_phantom.location; - for (const auto &p : unpacked_path) - { - const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); - distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, current_coordinate); - prev_coordinate = current_coordinate; - } - distance += util::coordinate_calculation::fccApproximateDistance(prev_coordinate, target_phantom.location); - - return 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 ce9b23d70..d893369f5 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -120,7 +120,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, } return result; } -} +} // namespace // Heaps only record for each node its predecessor ("parent") on the shortest path. // For re-constructing the actual path we need to trace back all parent "pointers". @@ -680,11 +680,20 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } - std::vector unpacked_path; + EdgeDistance distance = 0; - annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path); + 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); + } + } - return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); + adjustPathDistanceToPhantomNodes( + unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, 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 8ee02b5ec..00449b9cb 100644 --- a/src/engine/routing_algorithms/many_to_many_ch.cpp +++ b/src/engine/routing_algorithms/many_to_many_ch.cpp @@ -240,74 +240,11 @@ void calculateDistances(typename SearchEngineData::ManyToManyQuer } if (!packed_leg.empty()) { - auto annotation = + EdgeDistance annotation = ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end()); + adjustPathDistanceToPhantomNodes(packed_leg, source_phantom, target_phantom, annotation); distances_table[row_index * number_of_targets + column_index] = annotation; - - // check the direction of travel to figure out how to calculate the offset to/from - // the source/target - if (source_phantom.forward_segment_id.id == packed_leg.front()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // -->s <-- subtract offset to start at source - // ......... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = source_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] -= offset; - } - else if (source_phantom.reverse_segment_id.id == packed_leg.front()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // s<------- <-- subtract offset to start at source - // ... <-- want this distance - // entry 0---1---2---3 <-- 3 is exit node - EdgeDistance offset = source_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] -= offset; - } - if (target_phantom.forward_segment_id.id == packed_leg.back()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // ++>t <-- add offset to get to target - // ................ <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = target_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] += offset; - } - else if (target_phantom.reverse_segment_id.id == packed_leg.back()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // <++t <-- add offset to get from target - // ................ <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = target_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] += offset; - } - } - else - { - // there is no shortcut to unpack. source and target are on the same EBG Node. - // if the offset of the target is greater than the offset of the source, subtract it - if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance()) - { - // --------->t <-- offsets - // ->s <-- subtract source offset from target offset - // ......... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = - target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] = offset; - } - else - { - // s<--- <-- offsets - // t<--------- <-- subtract source offset from target offset - // ...... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = - target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] = offset; - } } packed_leg.clear(); } diff --git a/src/engine/routing_algorithms/routing_base.cpp b/src/engine/routing_algorithms/routing_base.cpp index fb01472bb..236e957fe 100644 --- a/src/engine/routing_algorithms/routing_base.cpp +++ b/src/engine/routing_algorithms/routing_base.cpp @@ -33,6 +33,72 @@ 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) +{ + if (!path.empty()) + { + + // check the direction of travel to figure out how to calculate the offset to/from + // the source/target + if (source_phantom.forward_segment_id.id == path.front()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // -->s <-- subtract offset to start at source + // ......... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance -= source_phantom.GetForwardDistance(); + } + else if (source_phantom.reverse_segment_id.id == path.front()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // s<------- <-- subtract offset to start at source + // ... <-- want this distance + // entry 0---1---2---3 <-- 3 is exit node + distance -= source_phantom.GetReverseDistance(); + } + if (target_phantom.forward_segment_id.id == path.back()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // ++>t <-- add offset to get to target + // ................ <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance += target_phantom.GetForwardDistance(); + } + else if (target_phantom.reverse_segment_id.id == path.back()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // <++t <-- add offset to get from target + // ................ <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance += target_phantom.GetReverseDistance(); + } + } + else + { + // there is no shortcut to unpack. source and target are on the same EBG Node. + // if the offset of the target is greater than the offset of the source, subtract it + if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance()) + { + // --------->t <-- offsets + // ->s <-- subtract source offset from target offset + // ......... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance = target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance(); + } + else + { + // s<--- <-- offsets + // t<--------- <-- subtract source offset from target offset + // ...... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); + } + } +} + } // namespace routing_algorithms } // namespace engine } // namespace osrm diff --git a/src/engine/routing_algorithms/routing_base_ch.cpp b/src/engine/routing_algorithms/routing_base_ch.cpp index 695ebe923..f9f3485bb 100644 --- a/src/engine/routing_algorithms/routing_base_ch.cpp +++ b/src/engine/routing_algorithms/routing_base_ch.cpp @@ -199,14 +199,33 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } - std::vector unpacked_path; - unpackPath(facade, - packed_path.begin(), - packed_path.end(), - {source_phantom, target_phantom}, - unpacked_path); + BOOST_ASSERT(nodes_number > 0); - return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); + EdgeDistance distance = 0; + + std::vector unpacked_nodes; + unpacked_nodes.reserve(packed_path.size()); + 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); + }); + + for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); + node_iter++) + { + distance += computeEdgeDistance(facade, *node_iter); + } + } + + adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); + + return distance / 10.; } } // namespace ch