From 76d64f27cc68341f557894c1596aa52529ad223b Mon Sep 17 00:00:00 2001 From: Moritz Kobitzsch Date: Wed, 30 Mar 2016 13:14:48 +0200 Subject: [PATCH] fix same segment routes --- include/engine/guidance/assemble_leg.hpp | 2 +- .../routing_algorithms/routing_base.hpp | 36 ++++--- src/engine/guidance/post_processing.cpp | 101 ++++++++++++------ 3 files changed, 88 insertions(+), 51 deletions(-) diff --git a/include/engine/guidance/assemble_leg.hpp b/include/engine/guidance/assemble_leg.hpp index e1f846bb4..2470f873b 100644 --- a/include/engine/guidance/assemble_leg.hpp +++ b/include/engine/guidance/assemble_leg.hpp @@ -118,7 +118,7 @@ RouteLeg assembleLeg(const DataFacadeT &facade, // s // | - // Given a route a---b---c where there is a right turn at b. + // Given a route a---b---c where there is a right turn at c. // | // d // |--t diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 023ec7a67..04fc925f2 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -89,11 +89,10 @@ template class BasicRoutingInterface const std::int32_t new_distance = reverse_heap.GetKey(node) + distance; if (new_distance < upper_bound) { + // if loops are forced, they are so at the source if (new_distance >= 0 && - (!force_loop_forward || - forward_heap.GetData(node).parent != - node) // if loops are forced, they are so at the source - && (!force_loop_reverse || reverse_heap.GetData(node).parent != node)) + (!force_loop_forward || forward_heap.GetData(node).parent != node) && + (!force_loop_reverse || reverse_heap.GetData(node).parent != node)) { middle_node_id = node; upper_bound = new_distance; @@ -320,8 +319,11 @@ template class BasicRoutingInterface for (std::size_t i = start_index; i < end_index; ++i) { unpacked_path.push_back( - PathData{id_vector[i], name_index, weight_vector[i], - extractor::guidance::TurnInstruction::NO_TURN(), travel_mode}); + PathData{id_vector[i], + name_index, + weight_vector[i], + extractor::guidance::TurnInstruction::NO_TURN(), + travel_mode}); } BOOST_ASSERT(unpacked_path.size() > 0); unpacked_path.back().turn_instruction = turn_instruction; @@ -339,6 +341,7 @@ template class BasicRoutingInterface // However the first segment duration needs to be adjusted to the fact that the // source phantom is in the middle of the segment. // We do this by subtracting v--s from the duration. + BOOST_ASSERT(unpacked_path.front().duration_until_turn >= source_weight); unpacked_path.front().duration_until_turn -= source_weight; } @@ -369,8 +372,10 @@ template class BasicRoutingInterface } std::size_t end_index = phantom_node_pair.target_phantom.fwd_segment_position; + const std::size_t delta = target_traversed_in_reverse ? 1 : 0; if (target_traversed_in_reverse) { + start_index += 1; std::reverse(id_vector.begin(), id_vector.end()); std::reverse(weight_vector.begin(), weight_vector.end()); end_index = id_vector.size() - phantom_node_pair.target_phantom.fwd_segment_position; @@ -392,8 +397,10 @@ template class BasicRoutingInterface { BOOST_ASSERT(i < id_vector.size()); BOOST_ASSERT(phantom_node_pair.target_phantom.forward_travel_mode > 0); - unpacked_path.emplace_back( - PathData{id_vector[i], phantom_node_pair.target_phantom.name_id, weight_vector[i], + unpacked_path.push_back( + PathData{id_vector[i], + phantom_node_pair.target_phantom.name_id, + weight_vector[i - delta], extractor::guidance::TurnInstruction::NO_TURN(), target_traversed_in_reverse ? phantom_node_pair.target_phantom.backward_travel_mode @@ -402,9 +409,9 @@ template class BasicRoutingInterface if (is_local_path && unpacked_path.size() > 0) { - auto source_weight = start_traversed_in_reverse - ? phantom_node_pair.source_phantom.reverse_weight - : phantom_node_pair.source_phantom.forward_weight; + const auto source_weight = start_traversed_in_reverse + ? phantom_node_pair.source_phantom.reverse_weight + : phantom_node_pair.source_phantom.forward_weight; // The above code will create segments for (v, w), (w,x), (x, y) and (y, Z). // However the first segment duration needs to be adjusted to the fact that the source // phantom is in the middle of the segment. We do this by subtracting v--s from the @@ -423,9 +430,6 @@ template class BasicRoutingInterface const std::size_t last_index = unpacked_path.size() - 1; const std::size_t second_to_last_index = last_index - 1; - // looks like a trivially true check but tests for underflow - BOOST_ASSERT(last_index > second_to_last_index); - if (unpacked_path[last_index].turn_via_node == unpacked_path[second_to_last_index].turn_via_node) { @@ -651,8 +655,8 @@ template class BasicRoutingInterface } // TODO check if unordered_set might be faster // sort by id and increasing by distance - auto entry_point_comparator = [](const std::pair &lhs, - const std::pair &rhs) + auto entry_point_comparator = + [](const std::pair &lhs, const std::pair &rhs) { return lhs.first < rhs.first || (lhs.first == rhs.first && lhs.second < rhs.second); }; diff --git a/src/engine/guidance/post_processing.cpp b/src/engine/guidance/post_processing.cpp index 4e7a3d642..b2bf74454 100644 --- a/src/engine/guidance/post_processing.cpp +++ b/src/engine/guidance/post_processing.cpp @@ -293,10 +293,6 @@ std::vector postProcess(std::vector steps) void trimShortSegments(std::vector &steps, LegGeometry &geometry) { -#if OSRM_POST_PROCESSING_PRINT_DEBUG - std::cout << "[Pre-Trimming]" << std::endl; - print(steps); -#endif // Doing this step in post-processing provides a few challenges we cannot overcome. // The removal of an initial step imposes some copy overhead in the steps, moving all later // steps to the front. @@ -305,7 +301,7 @@ void trimShortSegments(std::vector &steps, LegGeometry &geometry) // few seconds of inaccuracy at both ends. This is acceptable, however, since the turn should // usually not be as relevant. - if (steps.size() <= 2) + if (steps.size() < 2) return; // if phantom node is located at the connection of two segments, either one can be selected as @@ -321,37 +317,59 @@ void trimShortSegments(std::vector &steps, LegGeometry &geometry) // We have to be careful though, since routing that starts in a roundabout has a valid. // To catch these cases correctly, we have to perform trimming prior to the post-processing - if (steps.front().distance <= std::numeric_limits::epsilon()) + BOOST_ASSERT(geometry.locations.size() >= steps.size()); + const bool zero_length_step = steps.front().distance <= std::numeric_limits::epsilon(); + const bool duplicated_coordinate = geometry.locations[0] == geometry.locations[1]; + if (zero_length_step || duplicated_coordinate) { - // We have to adjust the first step both for its name and the bearings - const auto ¤t_depart = steps.front(); - auto &designated_depart = *(steps.begin() + 1); - - // FIXME this is required to be consistent with the route durations. The initial turn is not - // actually part of the route, though - designated_depart.duration += current_depart.duration; - + // fixup the coordinate geometry.locations.erase(geometry.locations.begin()); - BOOST_ASSERT(geometry.segment_offsets[1] == 1); - // geometry offsets have to be adjusted. Move all offsets to the front and reduce by one. - std::transform(geometry.segment_offsets.begin() + 1, geometry.segment_offsets.end(), - geometry.segment_offsets.begin(), [](const std::size_t val) - { - return val - 1; - }); - geometry.segment_offsets.pop_back(); - // remove the initial distance value geometry.segment_distances.erase(geometry.segment_distances.begin()); - // update initial turn direction/bearings. Due to the duplicated first coordinate, the - // initial bearing is invalid - designated_depart.maneuver = detail::stepManeuverFromGeometry( - TurnInstruction::NO_TURN(), WaypointType::Depart, geometry); + // We have to adjust the first step both for its name and the bearings + if (zero_length_step) + { + // move offsets to front + BOOST_ASSERT(geometry.segment_offsets[1] == 1); + // geometry offsets have to be adjusted. Move all offsets to the front and reduce by + // one. (This is an inplace forward one and reduce by one) + std::transform(geometry.segment_offsets.begin() + 1, geometry.segment_offsets.end(), + geometry.segment_offsets.begin(), [](const std::size_t val) + { + return val - 1; + }); - // finally remove the initial (now duplicated move) - steps.erase(steps.begin()); + geometry.segment_offsets.pop_back(); + const auto ¤t_depart = steps.front(); + auto &designated_depart = *(steps.begin() + 1); + + // FIXME this is required to be consistent with the route durations. The initial turn is + // not actually part of the route, though + designated_depart.duration += current_depart.duration; + + // update initial turn direction/bearings. Due to the duplicated first coordinate, the + // initial bearing is invalid + designated_depart.maneuver = detail::stepManeuverFromGeometry( + TurnInstruction::NO_TURN(), WaypointType::Depart, geometry); + + // finally remove the initial (now duplicated move) + steps.erase(steps.begin()); + } + else + { + steps.front().geometry_begin = 1; + // reduce all offsets by one (inplace) + std::transform(geometry.segment_offsets.begin(), geometry.segment_offsets.end(), + geometry.segment_offsets.begin(), [](const std::size_t val) + { + return val - 1; + }); + + steps.front().maneuver = detail::stepManeuverFromGeometry( + TurnInstruction::NO_TURN(), WaypointType::Depart, geometry); + } // and update the leg geometry indices for the removed entry std::for_each(steps.begin(), steps.end(), [](RouteStep &step) @@ -362,9 +380,10 @@ void trimShortSegments(std::vector &steps, LegGeometry &geometry) } // make sure we still have enough segments - if (steps.size() <= 2) + if (steps.size() < 2) return; + BOOST_ASSERT(geometry.locations.size() >= steps.size()); auto &next_to_last_step = *(steps.end() - 2); // in the end, the situation with the roundabout cannot occur. As a result, we can remove all // zero-length instructions @@ -380,6 +399,21 @@ void trimShortSegments(std::vector &steps, LegGeometry &geometry) steps.pop_back(); // the geometry indices of the last step are already correct; } + else if (geometry.locations[geometry.locations.size() - 2] == + geometry.locations[geometry.locations.size() - 1]) + { + // correct steps but duplicated coordinate in the end. + // This can happen if the last coordinate snaps to a node in the unpacked geometry + geometry.locations.pop_back(); + geometry.segment_offsets.back()--; + BOOST_ASSERT(next_to_last_step.geometry_end == steps.back().geometry_begin); + BOOST_ASSERT(next_to_last_step.geometry_begin < next_to_last_step.geometry_end); + next_to_last_step.geometry_end--; + steps.back().geometry_begin--; + steps.back().geometry_end--; + steps.back().maneuver = detail::stepManeuverFromGeometry(TurnInstruction::NO_TURN(), + WaypointType::Arrive, geometry); + } } // assign relative locations to depart/arrive instructions @@ -399,8 +433,7 @@ std::vector assignRelativeLocations(std::vector steps, distance_to_start >= MINIMAL_RELATIVE_DISTANCE && distance_to_start <= MAXIMAL_RELATIVE_DISTANCE ? angleToDirectionModifier(util::coordinate_calculation::computeAngle( - source_node.input_location, leg_geometry.locations.at(0), - leg_geometry.locations.at(1))) + source_node.input_location, leg_geometry.locations[0], leg_geometry.locations[1])) : extractor::guidance::DirectionModifier::UTurn; steps.front().maneuver.instruction.direction_modifier = initial_modifier; @@ -411,8 +444,8 @@ std::vector assignRelativeLocations(std::vector steps, distance_from_end >= MINIMAL_RELATIVE_DISTANCE && distance_from_end <= MAXIMAL_RELATIVE_DISTANCE ? angleToDirectionModifier(util::coordinate_calculation::computeAngle( - leg_geometry.locations.at(leg_geometry.locations.size() - 2), - leg_geometry.locations.at(leg_geometry.locations.size() - 1), + leg_geometry.locations[leg_geometry.locations.size() - 2], + leg_geometry.locations[leg_geometry.locations.size() - 1], target_node.input_location)) : extractor::guidance::DirectionModifier::UTurn;