From 828e21a02dba20a7b18eea20401d114951115fc1 Mon Sep 17 00:00:00 2001 From: tatiana-kondakova Date: Mon, 14 Aug 2017 18:43:58 +0300 Subject: [PATCH] Fix following route with intermediate points --- routing/base/followed_polyline.cpp | 11 +++++++---- routing/base/followed_polyline.hpp | 7 ++++++- routing/route.hpp | 24 ++++++++++++++++++++++-- routing/routing_session.cpp | 1 + 4 files changed, 36 insertions(+), 7 deletions(-) diff --git a/routing/base/followed_polyline.cpp b/routing/base/followed_polyline.cpp index 8a38cf321c..eb03760871 100644 --- a/routing/base/followed_polyline.cpp +++ b/routing/base/followed_polyline.cpp @@ -78,6 +78,7 @@ void FollowedPolyline::Swap(FollowedPolyline & rhs) m_segDistance.swap(rhs.m_segDistance); m_segProj.swap(rhs.m_segProj); swap(m_current, rhs.m_current); + swap(m_nextCheckpointIndex, rhs.m_nextCheckpointIndex); } void FollowedPolyline::Update() @@ -141,16 +142,18 @@ Iter FollowedPolyline::GetBestProjection(m2::RectD const & posRect, DistanceFn const & distFn) const { CHECK_EQUAL(m_segProj.size() + 1, m_poly.GetSize(), ()); - // At first trying to find a projection to two closest route segments of route which is near - // enough to |posRect| center. + // At first trying to find a projection to two closest route segments of route which is close + // enough to |posRect| center. If m_current is right before intermediate point we can get closestIter + // right after intermediate point (in next subroute). size_t const hoppingBorderIdx = min(m_segProj.size(), m_current.m_ind + 2); Iter const closestIter = GetClosestProjectionInInterval(posRect, distFn, m_current.m_ind, hoppingBorderIdx); if (closestIter.IsValid()) return closestIter; - // If a projection to two closest route segments is not found trying to find projection to other route segments. - return GetClosestProjectionInInterval(posRect, distFn, hoppingBorderIdx, m_segProj.size()); + // If a projection to the two closest route segments is not found tries to find projection to other route + // segments of current subroute. + return GetClosestProjectionInInterval(posRect, distFn, hoppingBorderIdx, m_nextCheckpointIndex); } Iter FollowedPolyline::UpdateProjectionByPrediction(m2::RectD const & posRect, diff --git a/routing/base/followed_polyline.hpp b/routing/base/followed_polyline.hpp index 7e7241533c..a63d0efa62 100644 --- a/routing/base/followed_polyline.hpp +++ b/routing/base/followed_polyline.hpp @@ -18,8 +18,12 @@ public: : m_poly(begin, end) { Update(); + // Initially we do not have intermediate points. Next checkpoint is finish. + m_nextCheckpointIndex = m_segProj.size(); } + void SetNextCheckpointIndex(size_t index) { m_nextCheckpointIndex = index; } + void Swap(FollowedPolyline & rhs); void Append(FollowedPolyline const & poly) @@ -37,7 +41,7 @@ public: bool IsValid() const { return (m_current.IsValid() && m_poly.GetSize() > 1); } m2::PolylineD const & GetPolyline() const { return m_poly; } - vector const & GetSegDistanceMeters() const { return m_segDistance; } + std::vector const & GetSegDistanceMeters() const { return m_segDistance; } double GetTotalDistanceMeters() const; double GetDistanceFromStartMeters() const; double GetDistanceToEndMeters() const; @@ -92,6 +96,7 @@ private: /// Iterator with the current position. Position sets with UpdateProjection methods. mutable Iter m_current; + size_t m_nextCheckpointIndex; /// Precalculated info for fast projection finding. std::vector> m_segProj; /// Accumulated cache of segments length in meters. diff --git a/routing/route.hpp b/routing/route.hpp index fc1a7ce034..1fa370c384 100644 --- a/routing/route.hpp +++ b/routing/route.hpp @@ -5,13 +5,15 @@ #include "routing/segment.hpp" #include "routing/turns.hpp" +#include "routing/base/followed_polyline.hpp" + #include "traffic/speed_groups.hpp" #include "indexer/feature_altitude.hpp" #include "geometry/polyline2d.hpp" -#include "base/followed_polyline.hpp" +#include "base/assert.hpp" #include #include @@ -156,9 +158,15 @@ public: template void SetGeometry(TIter beg, TIter end) { if (beg == end) + { FollowedPolyline().Swap(m_poly); + } else + { + ASSERT_GREATER(m_subrouteAttrs.size(), m_currentSubrouteIdx, ()); FollowedPolyline(beg, end).Swap(m_poly); + m_poly.SetNextCheckpointIndex(m_subrouteAttrs[m_currentSubrouteIdx].GetEndSegmentIdx()); + } } template @@ -180,7 +188,19 @@ public: void SetCurrentSubrouteIdx(size_t currentSubrouteIdx) { m_currentSubrouteIdx = currentSubrouteIdx; } template - void SetSubroteAttrs(V && subroutes) { m_subrouteAttrs = std::forward(subroutes); } + void SetSubroteAttrs(V && subroutes) + { + m_subrouteAttrs = std::forward(subroutes); + ASSERT_GREATER(m_subrouteAttrs.size(), m_currentSubrouteIdx, ()); + m_poly.SetNextCheckpointIndex(m_subrouteAttrs[m_currentSubrouteIdx].GetEndSegmentIdx()); + } + + void PassNextSubroute() + { + ASSERT_GREATER(m_subrouteAttrs.size(), m_currentSubrouteIdx, ()); + m_currentSubrouteIdx = std::min(m_currentSubrouteIdx + 1, m_subrouteAttrs.size() - 1); + m_poly.SetNextCheckpointIndex(m_subrouteAttrs[m_currentSubrouteIdx].GetEndSegmentIdx()); + } /// \returns estimated time for the whole route. double GetTotalTimeSec() const; diff --git a/routing/routing_session.cpp b/routing/routing_session.cpp index 7d0b93caae..3156e9b0be 100644 --- a/routing/routing_session.cpp +++ b/routing/routing_session.cpp @@ -409,6 +409,7 @@ void RoutingSession::PassCheckpoints() { while (!m_checkpoints.IsFinished() && m_route->IsSubroutePassed(m_checkpoints.GetPassedIdx())) { + m_route->PassNextSubroute(); m_checkpoints.PassNextPoint(); LOG(LINFO, ("Pass checkpoint, ", m_checkpoints)); m_checkpointCallback(m_checkpoints.GetPassedIdx());