From 80c55119d25bf85e3b9c6546848579c6a030ba67 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Mon, 28 Aug 2017 19:03:51 +0300 Subject: [PATCH] Add bearing for the phantom node --- features/testbot/bearing_param.feature | 4 +-- include/engine/geospatial_query.hpp | 37 ++++++++++++---------- include/engine/guidance/assemble_steps.hpp | 13 +++++--- include/engine/phantom_node.hpp | 33 ++++++++++++------- src/engine/guidance/assemble_steps.cpp | 20 ++++++++++-- 5 files changed, 71 insertions(+), 36 deletions(-) diff --git a/features/testbot/bearing_param.feature b/features/testbot/bearing_param.feature index 4c313eed4..1d286ca49 100644 --- a/features/testbot/bearing_param.feature +++ b/features/testbot/bearing_param.feature @@ -67,8 +67,8 @@ Feature: Bearing parameter | from | to | bearings | route | bearing | | 0 | b | 10 10 | bc,bc | 0->0,0->0 | | 0 | b | 90 90 | ab,ab | 0->90,90->0 | - | 0 | b | 170 170 | da,da | 0->0,0->0 | - | 0 | b | 189 189 | da,da | 0->0,0->0 | + | 0 | b | 170 170 | da,da | 0->180,180->0 | + | 0 | b | 189 189 | da,da | 0->180,180->0 | | 0 | 1 | 90 270 | ab,cd,cd | 0->90,90->0,270->0 | | 1 | 2 | 10 10 | bc,bc | 0->0,0->0 | | 1 | 2 | 90 90 | ab,cd,ab,ab | 0->90,90->0,270->180,90->0 | diff --git a/include/engine/geospatial_query.hpp b/include/engine/geospatial_query.hpp index 3cf51d3c7..70c984bc9 100644 --- a/include/engine/geospatial_query.hpp +++ b/include/engine/geospatial_query.hpp @@ -502,23 +502,26 @@ template class GeospatialQuery bool is_reverse_valid_target = areSegmentsValid( reverse_weight_vector.begin(), reverse_weight_vector.end() - data.fwd_segment_position); - auto transformed = PhantomNodeWithDistance{PhantomNode{data, - component_id, - forward_weight, - reverse_weight, - forward_weight_offset, - reverse_weight_offset, - forward_duration, - reverse_duration, - forward_duration_offset, - reverse_duration_offset, - is_forward_valid_source, - is_forward_valid_target, - is_reverse_valid_source, - is_reverse_valid_target, - point_on_segment, - input_coordinate}, - current_perpendicular_distance}; + auto transformed = PhantomNodeWithDistance{ + PhantomNode{data, + component_id, + forward_weight, + reverse_weight, + forward_weight_offset, + reverse_weight_offset, + forward_duration, + reverse_duration, + forward_duration_offset, + reverse_duration_offset, + is_forward_valid_source, + is_forward_valid_target, + is_reverse_valid_source, + is_reverse_valid_target, + point_on_segment, + input_coordinate, + static_cast(util::coordinate_calculation::bearing( + coordinates[data.u], coordinates[data.v]))}, + current_perpendicular_distance}; return transformed; } diff --git a/include/engine/guidance/assemble_steps.hpp b/include/engine/guidance/assemble_steps.hpp index 054fc798e..f2655958c 100644 --- a/include/engine/guidance/assemble_steps.hpp +++ b/include/engine/guidance/assemble_steps.hpp @@ -29,8 +29,12 @@ namespace guidance { namespace detail { -std::pair getDepartBearings(const LegGeometry &leg_geometry); -std::pair getArriveBearings(const LegGeometry &leg_geometry); +std::pair getDepartBearings(const LegGeometry &leg_geometry, + const PhantomNode &source_node, + const bool traversed_in_reverse); +std::pair getArriveBearings(const LegGeometry &leg_geometry, + const PhantomNode &target_node, + const bool traversed_in_reverse); } // ns detail inline std::vector assembleSteps(const datafacade::BaseDataFacade &facade, @@ -72,7 +76,8 @@ inline std::vector assembleSteps(const datafacade::BaseDataFacade &fa std::size_t segment_index = 0; BOOST_ASSERT(leg_geometry.locations.size() >= 2); - auto bearings = detail::getDepartBearings(leg_geometry); + auto bearings = + detail::getDepartBearings(leg_geometry, source_node, source_traversed_in_reverse); StepManeuver maneuver{source_node.location, bearings.first, @@ -260,7 +265,7 @@ inline std::vector assembleSteps(const datafacade::BaseDataFacade &fa } BOOST_ASSERT(segment_index == number_of_segments - 1); - bearings = detail::getArriveBearings(leg_geometry); + bearings = detail::getArriveBearings(leg_geometry, target_node, target_traversed_in_reverse); intersection = { target_node.location, diff --git a/include/engine/phantom_node.hpp b/include/engine/phantom_node.hpp index 8a7485e7d..c756795e4 100644 --- a/include/engine/phantom_node.hpp +++ b/include/engine/phantom_node.hpp @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "extractor/travel_mode.hpp" #include "util/typedefs.hpp" +#include "util/bearing.hpp" #include "util/coordinate.hpp" #include @@ -47,13 +48,14 @@ namespace engine struct PhantomNode { PhantomNode() - : forward_segment_id{SPECIAL_SEGMENTID, false}, - reverse_segment_id{SPECIAL_SEGMENTID, false}, forward_weight(INVALID_EDGE_WEIGHT), - reverse_weight(INVALID_EDGE_WEIGHT), forward_weight_offset(0), reverse_weight_offset(0), + : forward_segment_id{SPECIAL_SEGMENTID, false}, reverse_segment_id{SPECIAL_SEGMENTID, + false}, + forward_weight(INVALID_EDGE_WEIGHT), reverse_weight(INVALID_EDGE_WEIGHT), + forward_weight_offset(0), reverse_weight_offset(0), forward_duration(MAXIMAL_EDGE_DURATION), reverse_duration(MAXIMAL_EDGE_DURATION), - forward_duration_offset(0), reverse_duration_offset(0), fwd_segment_position(0), - is_valid_forward_source{false}, is_valid_forward_target{false}, - is_valid_reverse_source{false}, is_valid_reverse_target{false} + forward_duration_offset(0), reverse_duration_offset(0), + fwd_segment_position(0), is_valid_forward_source{false}, is_valid_forward_target{false}, + is_valid_reverse_source{false}, is_valid_reverse_target{false}, bearing(0) { } @@ -85,8 +87,9 @@ struct PhantomNode bool IsValid(const unsigned number_of_nodes) const { - return location.IsValid() && ((forward_segment_id.id < number_of_nodes) || - (reverse_segment_id.id < number_of_nodes)) && + return location.IsValid() && + ((forward_segment_id.id < number_of_nodes) || + (reverse_segment_id.id < number_of_nodes)) && ((forward_weight != INVALID_EDGE_WEIGHT) || (reverse_weight != INVALID_EDGE_WEIGHT)) && ((forward_duration != MAXIMAL_EDGE_DURATION) || @@ -117,6 +120,12 @@ struct PhantomNode { return reverse_segment_id.enabled && is_valid_reverse_target; } + short GetBearing(const bool traversed_in_reverse) const + { + if (traversed_in_reverse) + return util::bearing::reverse(bearing); + return bearing; + } bool operator==(const PhantomNode &other) const { return location == other.location; } @@ -136,7 +145,8 @@ struct PhantomNode bool is_valid_reverse_source, bool is_valid_reverse_target, const util::Coordinate location, - const util::Coordinate input_location) + const util::Coordinate input_location, + const unsigned short bearing) : forward_segment_id{other.forward_segment_id}, reverse_segment_id{other.reverse_segment_id}, forward_weight{forward_weight}, reverse_weight{reverse_weight}, forward_weight_offset{forward_weight_offset}, @@ -148,7 +158,7 @@ struct PhantomNode is_valid_forward_source{is_valid_forward_source}, is_valid_forward_target{is_valid_forward_target}, is_valid_reverse_source{is_valid_reverse_source}, - is_valid_reverse_target{is_valid_reverse_target} + is_valid_reverse_target{is_valid_reverse_target}, bearing{bearing} { } @@ -173,7 +183,8 @@ struct PhantomNode unsigned short is_valid_forward_target : 1; unsigned short is_valid_reverse_source : 1; unsigned short is_valid_reverse_target : 1; - unsigned short : 12; // Unused padding out to 16 bits (2 bytes) + unsigned short bearing : 9; + unsigned short : 3; // Unused padding out to 16 bits (2 bytes) }; static_assert(sizeof(PhantomNode) == 64, "PhantomNode has more padding then expected"); diff --git a/src/engine/guidance/assemble_steps.cpp b/src/engine/guidance/assemble_steps.cpp index 308b68a77..3b0e601cf 100644 --- a/src/engine/guidance/assemble_steps.cpp +++ b/src/engine/guidance/assemble_steps.cpp @@ -2,6 +2,9 @@ #include +#include "util/bearing.hpp" +#include "util/log.hpp" + #include #include @@ -14,21 +17,34 @@ namespace guidance namespace detail { -std::pair getDepartBearings(const LegGeometry &leg_geometry) +std::pair getDepartBearings(const LegGeometry &leg_geometry, + const PhantomNode &source_node, + const bool traversed_in_reverse) { BOOST_ASSERT(leg_geometry.locations.size() >= 2); const auto turn_coordinate = leg_geometry.locations.front(); const auto post_turn_coordinate = *(leg_geometry.locations.begin() + 1); + + if (turn_coordinate == post_turn_coordinate) + { + return std::make_pair(0, source_node.GetBearing(traversed_in_reverse)); + } return std::make_pair( 0, std::round(util::coordinate_calculation::bearing(turn_coordinate, post_turn_coordinate))); } -std::pair getArriveBearings(const LegGeometry &leg_geometry) +std::pair getArriveBearings(const LegGeometry &leg_geometry, + const PhantomNode &target_node, + const bool traversed_in_reverse) { BOOST_ASSERT(leg_geometry.locations.size() >= 2); const auto turn_coordinate = leg_geometry.locations.back(); const auto pre_turn_coordinate = *(leg_geometry.locations.end() - 2); + if (turn_coordinate == pre_turn_coordinate) + { + return std::make_pair(target_node.GetBearing(traversed_in_reverse), 0); + } return std::make_pair( std::round(util::coordinate_calculation::bearing(pre_turn_coordinate, turn_coordinate)), 0); }