diff --git a/routing/route.cpp b/routing/route.cpp index 046f2b508a..458b8555ff 100644 --- a/routing/route.cpp +++ b/routing/route.cpp @@ -6,6 +6,7 @@ #include "geometry/angles.hpp" #include "geometry/point2d.hpp" +#include "geometry/simplification.hpp" #include "base/logging.hpp" @@ -16,9 +17,12 @@ namespace routing { +namespace +{ +double constexpr kLocationTimeThreshold = 60.0 * 1.0; +double constexpr kOnEndToleranceM = 10.0; -static double const LOCATION_TIME_THRESHOLD = 60.0*1.0; -static double const ON_END_TOLERANCE_M = 10.0; +} // namespace Route::Route(string const & router, vector const & points, string const & name) : m_router(router), m_routingSettings(GetCarRoutingSettings()), m_poly(points), m_name(name) @@ -40,6 +44,7 @@ void Route::Swap(Route & rhs) swap(m_times, rhs.m_times); m_turnsGeom.swap(rhs.m_turnsGeom); m_absentCountries.swap(rhs.m_absentCountries); + m_pedestrianFollower.Swap(rhs.m_pedestrianFollower); } double Route::GetTotalDistanceMeters() const @@ -159,9 +164,7 @@ void Route::GetCurrentTurn(double & distanceToTurnMeters, turns::TurnItem & turn void Route::GetCurrentDirectionPoint(m2::PointD & pt) const { - ASSERT(m_current.IsValid(), ()); - - pt = m_poly.GetPoint(min(m_current.m_ind + 1, m_poly.GetSize() - 1)); + m_pedestrianFollower.GetCurrentDirectionPoint(pt); } bool Route::MoveIterator(location::GpsInfo const & info) const @@ -172,13 +175,14 @@ bool Route::MoveIterator(location::GpsInfo const & info) const /// @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 < LOCATION_TIME_THRESHOLD) + if (deltaT > 0.0 && deltaT < kLocationTimeThreshold) predictDistance = info.m_speed * deltaT; } m2::RectD const rect = MercatorBounds::MetresToXY( info.m_longitude, info.m_latitude, max(m_routingSettings.m_matchingThresholdM, info.m_horizontalAccuracy)); + m_pedestrianFollower.FindProjection(rect, predictDistance); IterT const res = FindProjection(rect, predictDistance); if (res.IsValid()) { @@ -237,7 +241,7 @@ void Route::MatchLocationToRoute(location::GpsInfo & location, location::RouteMa bool Route::IsCurrentOnEnd() const { - return (GetCurrentDistanceToEndMeters() < ON_END_TOLERANCE_M); + return (GetCurrentDistanceToEndMeters() < kOnEndToleranceM); } Route::IterT Route::FindProjection(m2::RectD const & posRect, double predictDistance) const @@ -282,6 +286,13 @@ double Route::GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const void Route::Update() { + vector points; + auto distf = m2::DistanceToLineSquare(); + // TODO (ldargunov) Rewrite dist f to distance in meters and avoid 0.00000 constants. + SimplifyNearOptimal(20, m_poly.Begin(), m_poly.End(), 0.00000001, distf, + MakeBackInsertFunctor(points)); + m_pedestrianFollower = RouteFollower(points.begin(), points.end()); + size_t n = m_poly.GetSize(); ASSERT_GREATER(n, 1, ()); --n; diff --git a/routing/route.hpp b/routing/route.hpp index c5ae7706cf..b7279b941b 100644 --- a/routing/route.hpp +++ b/routing/route.hpp @@ -1,7 +1,8 @@ #pragma once -#include "routing/routing_settings.hpp" -#include "routing/turns.hpp" +#include "route_follower.hpp" +#include "routing_settings.hpp" +#include "turns.hpp" #include "geometry/polyline2d.hpp" @@ -127,6 +128,8 @@ private: private: friend string DebugPrint(Route const & r); + mutable RouteFollower m_pedestrianFollower; + string m_router; RoutingSettings m_routingSettings; m2::PolylineD m_poly; diff --git a/routing/route_follower.cpp b/routing/route_follower.cpp new file mode 100644 index 0000000000..9ab97fd44f --- /dev/null +++ b/routing/route_follower.cpp @@ -0,0 +1,126 @@ +#include "route_follower.hpp" + +namespace routing +{ +namespace +{ +double constexpr kPedestrianEdgeSwitchMeters = 5.0; +} // namespace + +double RouteFollower::GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const +{ + ASSERT(it1.IsValid() && it2.IsValid(), ()); + ASSERT_LESS_OR_EQUAL(it1.m_ind, it2.m_ind, ()); + ASSERT_LESS(it1.m_ind, m_poly.GetSize(), ()); + ASSERT_LESS(it2.m_ind, m_poly.GetSize(), ()); + + if (it1.m_ind == it2.m_ind) + return MercatorBounds::DistanceOnEarth(it1.m_pt, it2.m_pt); + + return (MercatorBounds::DistanceOnEarth(it1.m_pt, m_poly.GetPoint(it1.m_ind + 1)) + + m_segDistance[it2.m_ind - 1] - m_segDistance[it1.m_ind] + + MercatorBounds::DistanceOnEarth(m_poly.GetPoint(it2.m_ind), it2.m_pt)); +} + +void RouteFollower::Swap(RouteFollower & rhs) +{ + m_poly.Swap(rhs.m_poly); + m_segDistance.swap(rhs.m_segDistance); + m_segProj.swap(rhs.m_segProj); + swap(m_current, rhs.m_current); +} + +void RouteFollower::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 += MercatorBounds::DistanceOnEarth(p1, p2); + + m_segDistance[i] = dist; + m_segProj[i].SetBounds(p1, p2); + } + + m_current = IterT(m_poly.Front(), 0); +} + +template +RouteFollower::IterT RouteFollower::GetClosestProjection(m2::RectD const & posRect, + DistanceF const & distFn) const +{ + IterT res; + double minDist = numeric_limits::max(); + + m2::PointD const currPos = posRect.Center(); + 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 (!posRect.IsPointInside(pt)) + continue; + + IterT it(pt, i); + double const dp = distFn(it); + if (dp < minDist) + { + res = it; + minDist = dp; + } + } + + return res; +} + +RouteFollower::IterT RouteFollower::FindProjection(m2::RectD const & posRect, + double predictDistance) const +{ + ASSERT(m_current.IsValid(), ()); + ASSERT_LESS(m_current.m_ind, m_poly.GetSize() - 1, ()); + + IterT res; + if (predictDistance >= 0.0) + { + res = GetClosestProjection(posRect, [&](IterT const & it) + { + return fabs(GetDistanceOnPolyline(m_current, it) - predictDistance); + }); + } + else + { + m2::PointD const currPos = posRect.Center(); + res = GetClosestProjection(posRect, [&](IterT const & it) + { + return MercatorBounds::DistanceOnEarth(it.m_pt, currPos); + }); + } + + if (res.IsValid()) + m_current = res; + return res; +} +void RouteFollower::GetCurrentDirectionPoint(m2::PointD & pt) const +{ + ASSERT(m_current.IsValid(), ()); + size_t currentIndex = min(m_current.m_ind + 1, m_poly.GetSize() - 1); + m2::PointD point = m_poly.GetPoint(currentIndex); + for (; currentIndex < m_poly.GetSize() - 1; point = m_poly.GetPoint(++currentIndex)) + { + if (MercatorBounds::DistanceOnEarth(point, m_current.m_pt) > kPedestrianEdgeSwitchMeters) + break; + } + + pt = point; +} + +} // namespace routing diff --git a/routing/route_follower.hpp b/routing/route_follower.hpp new file mode 100644 index 0000000000..709bb29b58 --- /dev/null +++ b/routing/route_follower.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include "indexer/mercator.hpp" + +#include "geometry/point2d.hpp" +#include "geometry/polyline2d.hpp" + +namespace routing +{ +class RouteFollower +{ +public: + RouteFollower() {} + template + RouteFollower(IterT begin, IterT end) + : m_poly(begin, end) + { + Update(); + } + + void Swap(RouteFollower & rhs); + + struct IterT + { + m2::PointD m_pt; + size_t m_ind; + + IterT(m2::PointD pt, size_t ind) : m_pt(pt), m_ind(ind) {} + IterT() : m_ind(-1) {} + + bool IsValid() const { return m_ind != -1; } + }; + + IterT GetCurrentIter() { return m_current; } + + IterT FindProjection(m2::RectD const & posRect, double predictDistance = -1.0) const; + + void GetCurrentDirectionPoint(m2::PointD & pt) const; + +private: + template + IterT GetClosestProjection(m2::RectD const & posRect, DistanceF const & distFn) const; + + void Update(); + + double GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const; + + m2::PolylineD m_poly; + + mutable IterT m_current; + /// Precalculated info for fast projection finding. + vector> m_segProj; + /// Accumulated cache of segments length in meters. + vector m_segDistance; +}; + +} // namespace routing diff --git a/routing/routing.pro b/routing/routing.pro index 876bcf0c98..9348f704ea 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -30,6 +30,7 @@ SOURCES += \ road_graph.cpp \ road_graph_router.cpp \ route.cpp \ + route_follower.cpp \ router.cpp \ router_delegate.cpp \ routing_algorithm.cpp \ @@ -63,6 +64,7 @@ HEADERS += \ road_graph.hpp \ road_graph_router.hpp \ route.hpp \ + route_follower.hpp \ router.hpp \ router_delegate.hpp \ routing_algorithm.hpp \