diff --git a/map/framework.cpp b/map/framework.cpp index f29ee532b3..806d6fef34 100644 --- a/map/framework.cpp +++ b/map/framework.cpp @@ -215,7 +215,6 @@ Framework::Framework() m_stringsBundle.SetDefaultString("my_places", "My Places"); m_stringsBundle.SetDefaultString("my_position", "My Position"); m_stringsBundle.SetDefaultString("routes", "Routes"); - m_stringsBundle.SetDefaultString("recalculating_route", "Recalculating ..."); m_guiController->SetStringsBundle(&m_stringsBundle); @@ -1922,7 +1921,7 @@ void Framework::CheckLocationForRouting(GpsInfo const & info) break; case RoutingSession::OnRoute: - //m_routingSession.UpdatePosition(position, info); + m_routingSession.MoveRoutePosition(position, info); break; default: @@ -1930,7 +1929,7 @@ void Framework::CheckLocationForRouting(GpsInfo const & info) } } -void Framework::GetRouteFollowingInfo(location::FollowingInfo & info) +void Framework::GetRouteFollowingInfo(location::FollowingInfo & info) const { - + m_routingSession.GetRouteFollowingInfo(info); } diff --git a/map/framework.hpp b/map/framework.hpp index 85059d4adb..f314f96da2 100644 --- a/map/framework.hpp +++ b/map/framework.hpp @@ -482,7 +482,7 @@ public: bool IsRoutingActive() const; bool StartRoutingSession(m2::PointD const & destination); void CancelRoutingSession(); - void GetRouteFollowingInfo(location::FollowingInfo & info); + void GetRouteFollowingInfo(location::FollowingInfo & info) const; private: void RemoveRoute(); diff --git a/map/routing_session.cpp b/map/routing_session.cpp index 8ea4c4a1a6..4789ff7706 100644 --- a/map/routing_session.cpp +++ b/map/routing_session.cpp @@ -1,6 +1,11 @@ #include "routing_session.hpp" +#include "measurement_utils.hpp" + #include "../indexer/mercator.hpp" + +using namespace location; + namespace routing { @@ -13,30 +18,27 @@ RoutingSession::RoutingSession() } void RoutingSession::BuildRoute(m2::PointD const & startPoint, m2::PointD const & endPoint, - IRouter::ReadyCallback const & callback) + ReadyCallback const & callback) { ASSERT(m_router != nullptr, ()); m_router->SetFinalPoint(endPoint); RebuildRoute(startPoint, callback); } -void RoutingSession::RebuildRoute(m2::PointD const & startPoint, IRouter::ReadyCallback const & callback) +void RoutingSession::RebuildRoute(m2::PointD const & startPoint, ReadyCallback const & callback) { ASSERT(m_router != nullptr, ()); Reset(); m_state = RouteNotReady; - m2::RectD const errorRect = MercatorBounds::RectByCenterXYAndSizeInMeters(startPoint, 20); + m2::RectD const errorRect = MercatorBounds::RectByCenterXYAndSizeInMeters( + startPoint, Route::GetOnRoadTolerance()); m_tolerance = (errorRect.SizeX() + errorRect.SizeY()) / 2.0; - m_router->CalculateRoute(startPoint, [this, callback](Route const & route, IRouter::ResultCode e) + m_router->CalculateRoute(startPoint, [&] (Route & route, IRouter::ResultCode e) { - if (route.GetPoly().GetSize() < 2) - return; - - m_state = RouteNotStarted; - m_route = route; - callback(route, e); + AssignRoute(route); + callback(m_route); }); } @@ -49,7 +51,7 @@ void RoutingSession::Reset() { m_state = RoutingNotActive; m_lastMinDist = 0.0; - m_route = Route(string()); + Route(string()).Swap(m_route); } RoutingSession::State RoutingSession::OnLocationPositionChanged(m2::PointD const & position, @@ -93,6 +95,25 @@ RoutingSession::State RoutingSession::OnLocationPositionChanged(m2::PointD const return m_state; } +void RoutingSession::MoveRoutePosition(m2::PointD const & position, GpsInfo const & info) +{ + ASSERT_EQUAL(m_state, OnRoute, ()); + m_route.MoveIterator(position, info); +} + +void RoutingSession::GetRouteFollowingInfo(location::FollowingInfo & info) const +{ + if (m_route.IsValid()) + { + MeasurementUtils::FormatDistance(m_route.GetCurrentDistanceToEnd(), info.m_distToTarget); + + size_t const delim = info.m_distToTarget.find(' '); + ASSERT(delim != string::npos, ()); + info.m_unitsSuffix = info.m_distToTarget.substr(delim + 1); + info.m_distToTarget.erase(delim); + } +} + bool RoutingSession::IsOnRoute(m2::PointD const & position, double errorRadius, double & minDist) const { minDist = sqrt(m_route.GetPoly().GetShortestSquareDistance(position)); @@ -112,6 +133,17 @@ bool RoutingSession::IsOnDestPoint(m2::PointD const & position, double errorRadi return false; } +void RoutingSession::AssignRoute(Route & route) +{ + if (route.IsValid()) + { + m_state = RouteNotStarted; + m_route.Swap(route); + } + else + m_state = RouteNotReady; +} + void RoutingSession::SetRouter(IRouter * router) { m_router.reset(router); diff --git a/map/routing_session.hpp b/map/routing_session.hpp index ced3542c55..bee618f7f4 100644 --- a/map/routing_session.hpp +++ b/map/routing_session.hpp @@ -3,11 +3,14 @@ #include "../routing/router.hpp" #include "../routing/route.hpp" +#include "../platform/location.hpp" + #include "../geometry/point2d.hpp" #include "../geometry/polyline2d.hpp" #include "../std/unique_ptr.hpp" + namespace routing { @@ -39,28 +42,34 @@ public: RoutingSession(); void SetRouter(IRouter * router); - /// startPoint and destPoint in mercator + typedef function ReadyCallback; + /// @param[in] startPoint and endPoint in mercator void BuildRoute(m2::PointD const & startPoint, m2::PointD const & endPoint, - IRouter::ReadyCallback const & callback); - void RebuildRoute(m2::PointD const & startPoint, IRouter::ReadyCallback const & callback); + ReadyCallback const & callback); + + void RebuildRoute(m2::PointD const & startPoint, ReadyCallback const & callback); bool IsActive() const; void Reset(); State OnLocationPositionChanged(m2::PointD const & position, double errorRadius); + void MoveRoutePosition(m2::PointD const & position, location::GpsInfo const & info); + void GetRouteFollowingInfo(location::FollowingInfo & info) const; + private: - // errorRadius is error in determining the position in the Mercator + /// @param[in] errorRadius Tolerance determining that position belongs to the route (mercator). bool IsOnRoute(m2::PointD const & position, double errorRadius, double & minDist) const; bool IsOnDestPoint(m2::PointD const & position, double errorRadius) const; + void AssignRoute(Route & route); + private: unique_ptr m_router; Route m_route; State m_state; - double m_tolerance; - double m_lastMinDist; - + double m_tolerance; //!< Default tolerance for point-on-route checking (mercator). + double m_lastMinDist; //!< Last calculated position-on-route (mercator). uint32_t m_moveAwayCounter; }; diff --git a/routing/route.cpp b/routing/route.cpp index 0705b33c8c..c474a52a36 100644 --- a/routing/route.cpp +++ b/routing/route.cpp @@ -2,144 +2,195 @@ #include "../indexer/mercator.hpp" +#include "../platform/location.hpp" + #include "../geometry/distance_on_sphere.hpp" +#include "../base/logging.hpp" + +#include "../std/numeric.hpp" + + namespace routing { +static double const TIME_THRESHOLD_SECONDS = 60.0*1.0; + double GetDistanceOnEarth(m2::PointD const & p1, m2::PointD const & p2) { return ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), MercatorBounds::XToLon(p1.x), MercatorBounds::YToLat(p2.y), MercatorBounds::XToLon(p2.x)); -}; +} +Route::Route(string const & router, vector const & points, string const & name) + : m_router(router), m_poly(points), m_name(name) +{ + Update(); +} + +void Route::Swap(Route & rhs) +{ + m_router.swap(rhs.m_router); + m_poly.Swap(rhs.m_poly); + m_name.swap(rhs.m_name); + m_segDistance.swap(rhs.m_segDistance); + m_segProj.swap(rhs.m_segProj); + swap(m_current, rhs.m_current); + swap(m_currentTime, rhs.m_currentTime); +} + +double Route::GetDistance() const +{ + ASSERT(!m_segDistance.empty(), ()); + return m_segDistance.back(); +} + +double Route::GetCurrentDistanceFromBegin() const +{ + ASSERT(IsValid() && m_current.IsValid(), ()); + + return ((m_current.m_ind > 0 ? m_segDistance[m_current.m_ind - 1] : 0.0) + + GetDistanceOnEarth(m_poly.GetPoint(m_current.m_ind), m_current.m_pt)); +} + +double Route::GetCurrentDistanceToEnd() const +{ + ASSERT(IsValid() && m_current.IsValid(), ()); + + return (m_segDistance.back() - m_segDistance[m_current.m_ind] + + GetDistanceOnEarth(m_current.m_pt, m_poly.GetPoint(m_current.m_ind + 1))); +} + +void Route::MoveIterator(m2::PointD const & currPos, location::GpsInfo const & info) const +{ + double predictDistance = -1.0; + if (m_currentTime > 0.0 && info.HasSpeed()) + { + /// @todo Need to distinguish GPS and WiFi locations. + /// They may have different time metrics in case of incorrect system time on a device. + + double const deltaT = info.m_timestamp - m_currentTime; + if (deltaT > 0.0 && deltaT < TIME_THRESHOLD_SECONDS) + predictDistance = info.m_speed * deltaT; + } + + IterT const res = FindProjection(currPos, + max(GetOnRoadTolerance(), info.m_horizontalAccuracy), + predictDistance); + if (res.IsValid()) + { + m_current = res; + m_currentTime = info.m_timestamp; + } +} + +Route::IterT Route::FindProjection(m2::PointD const & currPos, + double errorRadius, + double predictDistance) const +{ + ASSERT(m_current.IsValid(), ()); + ASSERT_LESS(m_current.m_ind, m_poly.GetSize() - 1, ()); + ASSERT_GREATER(errorRadius, 0.0, ()); + + m2::RectD const rect = MercatorBounds::RectByCenterXYAndSizeInMeters(currPos, errorRadius); + + IterT res; + if (predictDistance >= 0.0) + { + res = GetClosestProjection(currPos, rect, [&] (IterT const & it) + { + return fabs(GetDistanceOnPolyline(m_current, it) - predictDistance); + }); + } + else + { + res = GetClosestProjection(currPos, rect, [&] (IterT const & it) + { + return GetDistanceOnEarth(it.m_pt, currPos); + }); + } + + return res; +} + +double Route::GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const +{ + size_t const n = m_poly.GetSize(); + + ASSERT(it1.IsValid() && it2.IsValid(), ()); + ASSERT_LESS_OR_EQUAL(it1.m_ind, it2.m_ind, ()); + ASSERT_LESS(it1.m_ind, n, ()); + ASSERT_LESS(it2.m_ind, n, ()); + + if (it1.m_ind == it2.m_ind) + return GetDistanceOnEarth(it1.m_pt, it2.m_pt); + + return (GetDistanceOnEarth(it1.m_pt, m_poly.GetPoint(it1.m_ind + 1)) + + m_segDistance[it2.m_ind - 1] - m_segDistance[it1.m_ind] + + GetDistanceOnEarth(m_poly.GetPoint(it2.m_ind), it2.m_pt)); +} + +void Route::Update() +{ + size_t n = m_poly.GetSize(); + ASSERT_GREATER(n, 1, ()); + --n; + + m_segDistance.resize(n); + m_segProj.resize(n); + + double dist = 0.0; + for (size_t i = 0; i < n; ++i) + { + m2::PointD const & p1 = m_poly.GetPoint(i); + m2::PointD const & p2 = m_poly.GetPoint(i + 1); + + dist += ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), + MercatorBounds::XToLon(p1.x), + MercatorBounds::YToLat(p2.y), + MercatorBounds::XToLon(p2.x)); + + m_segDistance[i] = dist; + m_segProj[i].SetBounds(p1, p2); + } + + m_current = IterT(m_poly.Front(), 0); + m_currentTime = 0.0; +} + +template +Route::IterT Route::GetClosestProjection(m2::PointD const & currPos, + m2::RectD const & rect, + DistanceF const & distFn) const +{ + IterT res; + double minDist = numeric_limits::max(); + + size_t const count = m_poly.GetSize() - 1; + for (size_t i = m_current.m_ind; i < count; ++i) + { + m2::PointD const pt = m_segProj[i](currPos); + + if (!rect.IsPointInside(pt)) + continue; + + IterT it(pt, i); + double const dp = distFn(it); + if (dp < minDist) + { + res = it; + minDist = dp; + } + } + + return res; +} string DebugPrint(Route const & r) { return DebugPrint(r.m_poly); } -Route::Route(string const & router, vector const & points, string const & name) - : m_router(router), m_poly(points), m_name(name), m_currentSegment(0) -{ - UpdateSegInfo(); -} - -double Route::GetDistance() const -{ - double distance = 0.0; - size_t const n = m_poly.GetSize() - 1; - for (size_t i = 0; i < n; ++i) - distance += m_segDistance[i]; - - return distance; -} - -double Route::GetDistanceToTarget(m2::PointD const & currPos, double errorRadius, double predictDistance) const -{ - if (m_poly.GetSize() == m_currentSegment) - return -1; - - pair res; - if (predictDistance >= 0) - { - res = GetClosestProjection(currPos, errorRadius, [&] (m2::PointD const & pt, size_t i) - { - return fabs(GetDistanceOnPolyline(m_currentSegment, m_currentPoint, i - 1, pt) - predictDistance); - }); - } - else - { - res = GetClosestProjection(currPos, errorRadius, [&] (m2::PointD const & pt, size_t i) - { - return GetDistanceOnEarth(pt, currPos); - }); - } - - if (res.second == -1) - return -1; - - m_currentPoint = res.first; - m_currentSegment = res.second; - - return GetDistanceOnPolyline(m_currentSegment, m_currentPoint, m_poly.GetSize() - 1, m_poly.Back()); -} - -double Route::GetDistanceOnPolyline(size_t s1, m2::PointD const & p1, size_t s2, m2::PointD const & p2) const -{ - size_t const n = m_poly.GetSize(); - - ASSERT_LESS_OR_EQUAL(s1, s2, ()); - ASSERT_LESS(s1, n, ()); - ASSERT_LESS(s2, n, ()); - - if (s1 == s2) - return GetDistanceOnEarth(p1, p2); - - double dist = GetDistanceOnEarth(p1, m_poly.GetPoint(s1 + 1)); - for (size_t i = s1 + 1; i < s2; ++i) - dist += m_segDistance[i]; - dist += GetDistanceOnEarth(p2, m_poly.GetPoint(s2)); - - return dist; -} - - -void Route::UpdateSegInfo() -{ - size_t const n = m_poly.GetSize(); - ASSERT_GREATER(n, 1, ()); - - m_segDistance.resize(n - 1); - m_segProj.resize(n - 1); - - for (size_t i = 1; i < n; ++i) - { - m2::PointD const & p1 = m_poly.GetPoint(i - 1); - m2::PointD const & p2 = m_poly.GetPoint(i); - - m_segDistance[i - 1] = ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), - MercatorBounds::XToLon(p1.x), - MercatorBounds::YToLat(p2.y), - MercatorBounds::XToLon(p2.x)); - - m_segProj[i - 1].SetBounds(p1, p2); - } - - m_currentSegment = 0; - m_currentPoint = m_poly.Front(); -} - -template -pair Route::GetClosestProjection(m2::PointD const & currPos, double errorRadius, DistanceF const & distFn) const -{ - double const errorRadius2 = errorRadius * errorRadius; - - size_t minSeg = -1; - m2::PointD minPt; - double minDist = numeric_limits::max(); - - for (size_t i = m_currentSegment + 1; i < m_poly.GetSize(); ++i) - { - m2::PointD const pt = m_segProj[i - 1](currPos); - double const d = pt.SquareLength(currPos); - - if (d > errorRadius2) - continue; - - double const dp = distFn(pt, i); - - if (dp < minDist) - { - minSeg = i - 1; - minPt = pt; - minDist = dp; - } - } - - return make_pair(minPt, minSeg); -} - } // namespace routing diff --git a/routing/route.hpp b/routing/route.hpp index 40fc28bc31..bf823e0c89 100644 --- a/routing/route.hpp +++ b/routing/route.hpp @@ -1,69 +1,98 @@ #pragma once #include "../geometry/polyline2d.hpp" -#include "../geometry/point2d.hpp" #include "../std/vector.hpp" #include "../std/string.hpp" + +namespace location { class GpsInfo; } + namespace routing { class Route { public: - explicit Route(string const & router, string const & name = string()) - : m_router(router), m_name(name), m_currentSegment(0) - { - } + explicit Route(string const & router) : m_router(router) {} template - Route(string const & router, IterT beg, IterT end, - string const & name = string()) - : m_router(router), m_poly(beg, end), m_name(name), m_currentSegment(0) + Route(string const & router, IterT beg, IterT end) + : m_router(router), m_poly(beg, end) { - UpdateSegInfo(); + Update(); } Route(string const & router, vector const & points, string const & name = string()); + void Swap(Route & rhs); + template void SetGeometry(IterT beg, IterT end) { m2::PolylineD(beg, end).Swap(m_poly); - UpdateSegInfo(); + Update(); } + /// Assume that position within road with this tolerance (meters). + static double GetOnRoadTolerance() { return 20.0; } + string const & GetRouterId() const { return m_router; } m2::PolylineD const & GetPoly() const { return m_poly; } string const & GetName() const { return m_name; } + bool IsValid() const { return (m_poly.GetSize() > 1); } - bool IsValid() const { return m_poly.GetSize() > 0; } + struct IterT + { + m2::PointD m_pt; + size_t m_ind; - // Distance of route in meters + IterT(m2::PointD pt, size_t ind) : m_pt(pt), m_ind(ind) {} + IterT() : m_ind(-1) {} + + bool IsValid() const { return m_ind != -1; } + }; + + /// @return Distance on route in meters. + //@{ double GetDistance() const; - double GetDistanceToTarget(m2::PointD const & currPos, double errorRadius, double predictDistance = -1) const; + double GetCurrentDistanceFromBegin() const; + double GetCurrentDistanceToEnd() const; + //@} + + void MoveIterator(m2::PointD const & currPos, location::GpsInfo const & info) const; private: - double GetDistanceOnPolyline(size_t s1, m2::PointD const & p1, size_t s2, m2::PointD const & p2) const; - template pair GetClosestProjection(m2::PointD const & currPos, double errorRadius, DistanceF const & distFn) const; + /// @param[in] errorRadius Radius to check projection candidates (meters). + /// @param[in] predictDistance Predict distance from previous FindProjection call (meters). + IterT FindProjection(m2::PointD const & currPos, + double errorRadius, + double predictDistance = -1.0) const; -protected: - void UpdateSegInfo(); + template + IterT GetClosestProjection(m2::PointD const & currPos, + m2::RectD const & rect, + DistanceF const & distFn) const; + + double GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const; + + /// Call this fucnction when geometry have changed. + void Update(); private: friend string DebugPrint(Route const & r); - /// The source which created the route string m_router; m2::PolylineD m_poly; string m_name; + /// Accumulated cache of segments length in meters. vector m_segDistance; - vector< m2::ProjectionToSection > m_segProj; - - mutable size_t m_currentSegment; - mutable m2::PointD m_currentPoint; + /// Precalculated info for fast projection finding. + vector> m_segProj; + /// Cached result iterator for last MoveIterator query. + mutable IterT m_current; + mutable double m_currentTime; }; } // namespace routing diff --git a/routing/router.hpp b/routing/router.hpp index e2b30e456a..40414c5fb7 100644 --- a/routing/router.hpp +++ b/routing/router.hpp @@ -25,7 +25,8 @@ public: InternalError }; - typedef function ReadyCallback; + /// Callback takes ownership of passed route. + typedef function ReadyCallback; virtual ~IRouter() {} diff --git a/routing/routing_tests/routing_smoke.cpp b/routing/routing_tests/routing_smoke.cpp deleted file mode 100644 index e36a3137aa..0000000000 --- a/routing/routing_tests/routing_smoke.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include "../../testing/testing.hpp" - -#include "../route.hpp" - -using namespace routing; - -UNIT_TEST(Routing_Smoke) -{ - TEST(true, ()); -} - -UNIT_TEST(Route_Distance) -{ - vector points = - { - {0, 0}, - {1, 0}, - {1, 0.5}, - {2, 0.5}, - {2, 1}, - {1.5, 1} - }; - - double const errorRadius = 0.1; - - { - Route route("test", points.begin(), points.end()); - - TEST_LESS(route.GetDistanceToTarget(m2::PointD(0.5, -0.11), errorRadius), 0, ()); - TEST_GREATER(route.GetDistanceToTarget(m2::PointD(0.5, -0.09), errorRadius), 0, ()); - - // note that we are passing route, so we need to move forward by segments - TEST_GREATER(route.GetDistanceToTarget(points.front(), errorRadius), - route.GetDistanceToTarget(m2::PointD(0.9, 0.1), errorRadius), - ()); - - TEST_LESS(route.GetDistanceToTarget(m2::PointD(0.9, 0.1), errorRadius), - route.GetDistanceToTarget(m2::PointD(0.9, 0.05), errorRadius), - ()); - - TEST_GREATER(route.GetDistanceToTarget(m2::PointD(0.9, 0.05), errorRadius), - route.GetDistanceToTarget(points.front(), errorRadius), - ()); - } - - { - double const predictDist = 10000; - - Route route("test", points.begin(), points.end()); - - TEST_GREATER(route.GetDistanceToTarget(points.front(), errorRadius, predictDist), - 0, ()); - - TEST_GREATER(route.GetDistanceToTarget(points.front(), errorRadius, predictDist), - route.GetDistanceToTarget(m2::PointD(0.5, 0), errorRadius, predictDist), - ()); - - TEST_GREATER(route.GetDistanceToTarget(m2::PointD(1.99, 0.51), errorRadius, predictDist), - route.GetDistanceToTarget(m2::PointD(1.99, 0.52), errorRadius, predictDist), - ()); - - TEST_LESS(route.GetDistanceToTarget(points.front(), errorRadius, predictDist), - 0, - ()); - - TEST_EQUAL(route.GetDistanceToTarget(points.back(), errorRadius, predictDist), - 0, ()); - - } - -} diff --git a/routing/routing_tests/routing_tests.pro b/routing/routing_tests/routing_tests.pro index 3bcdd6920e..65c554db1e 100644 --- a/routing/routing_tests/routing_tests.pro +++ b/routing/routing_tests/routing_tests.pro @@ -16,7 +16,6 @@ linux*: QT *= core SOURCES += \ ../../testing/testingmain.cpp \ - routing_smoke.cpp \ osrm_router_test.cpp \ vehicle_model_test.cpp \