From b9720163c76b20fa4bfee7278f4eb56c6f52ab0f Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Fri, 5 Dec 2014 17:32:21 +0300 Subject: [PATCH] [routing] Consider user direction on route rebuild --- map/routing_session.cpp | 4 ++- map/routing_session.hpp | 1 + routing/osrm_online_router.cpp | 2 +- routing/osrm_online_router.hpp | 2 +- routing/osrm_router.cpp | 50 ++++++++++++++++++++++++++-------- routing/osrm_router.hpp | 8 +++--- routing/router.hpp | 2 +- 7 files changed, 50 insertions(+), 19 deletions(-) diff --git a/map/routing_session.cpp b/map/routing_session.cpp index 3f1d433d64..56111b6c96 100644 --- a/map/routing_session.cpp +++ b/map/routing_session.cpp @@ -27,6 +27,7 @@ void RoutingSession::BuildRoute(m2::PointD const & startPoint, m2::PointD const TReadyCallbackFn const & callback) { ASSERT(m_router != nullptr, ()); + m_lastGoodPosition = startPoint; m_router->SetFinalPoint(endPoint); RebuildRoute(startPoint, callback); } @@ -39,7 +40,7 @@ void RoutingSession::RebuildRoute(m2::PointD const & startPoint, TReadyCallbackF // Use old-style callback constraction, because lambda constructs buggy function on Android // (callback param isn't captured by value). - m_router->CalculateRoute(startPoint, DoReadyCallback(*this, callback)); + m_router->CalculateRoute(startPoint, DoReadyCallback(*this, callback), startPoint - m_lastGoodPosition); } void RoutingSession::DoReadyCallback::operator() (Route & route, IRouter::ResultCode e) @@ -93,6 +94,7 @@ RoutingSession::State RoutingSession::OnLocationPositionChanged(m2::PointD const m_state = RouteFinished; else m_state = OnRoute; + m_lastGoodPosition = position; } else { diff --git a/map/routing_session.hpp b/map/routing_session.hpp index 0a8c5be414..21e97f4c65 100644 --- a/map/routing_session.hpp +++ b/map/routing_session.hpp @@ -81,6 +81,7 @@ private: /// Current position metrics to check for RouteNeedRebuild state. double m_lastDistance; int m_moveAwayCounter; + m2::PointD m_lastGoodPosition; }; } diff --git a/routing/osrm_online_router.cpp b/routing/osrm_online_router.cpp index aad142b96b..af8abe4b35 100644 --- a/routing/osrm_online_router.cpp +++ b/routing/osrm_online_router.cpp @@ -29,7 +29,7 @@ void OsrmOnlineRouter::SetFinalPoint(m2::PointD const & finalPt) m_finalPt = finalPt; } -void OsrmOnlineRouter::CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback) +void OsrmOnlineRouter::CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback, m2::PointD const & direction) { // Construct OSRM url request to get the route string url = OSRM_CAR_ROUTING_URL; diff --git a/routing/osrm_online_router.hpp b/routing/osrm_online_router.hpp index a711bcbc2f..f192df8b28 100644 --- a/routing/osrm_online_router.hpp +++ b/routing/osrm_online_router.hpp @@ -19,7 +19,7 @@ class OsrmOnlineRouter : public IRouter public: virtual string GetName() const; virtual void SetFinalPoint(m2::PointD const & finalPt); - virtual void CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback); + virtual void CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback, m2::PointD const & direction = m2::PointD::Zero()); }; } // namespace routing diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 13e11d3af8..52bb029da8 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -37,6 +37,7 @@ namespace class Point2PhantomNode : private noncopyable { m2::PointD m_point; + m2::PointD const m_direction; OsrmFtSegMapping const & m_mapping; struct Candidate @@ -55,8 +56,8 @@ class Point2PhantomNode : private noncopyable Index const * m_pIndex; public: - Point2PhantomNode(OsrmFtSegMapping const & mapping, Index const * pIndex) - : m_mapping(mapping), m_ptIdx(0), + Point2PhantomNode(OsrmFtSegMapping const & mapping, Index const * pIndex, m2::PointD const & direction) + : m_direction(direction), m_mapping(mapping), m_ptIdx(0), m_mwmId(numeric_limits::max()), m_pIndex(pIndex) { } @@ -267,8 +268,33 @@ public: { OsrmRouter::FeatureGraphNode & node = res[idx]; - node.m_node.forward_node_id = it->second.first; - node.m_node.reverse_node_id = it->second.second; + if (!m_direction.IsAlmostZero()) + { + // Filter income nodes by direction mode + OsrmFtSegMapping::FtSeg const & node_seg = segments[idx]; + FeatureType feature; + Index::FeaturesLoaderGuard loader(*m_pIndex, mwmId); + loader.GetFeature(node_seg.m_fid, feature); + feature.ParseGeometry(FeatureType::BEST_GEOMETRY); + m2::PointD const featureDirection = feature.GetPoint(node_seg.m_pointEnd) - feature.GetPoint(node_seg.m_pointStart); + bool const sameDirection = (m2::DotProduct(featureDirection, m_direction) / (featureDirection.Length() * m_direction.Length()) > 0); + if (sameDirection) + { + node.m_node.forward_node_id = it->second.first; + node.m_node.reverse_node_id = INVALID_NODE_ID; + } + else + { + node.m_node.forward_node_id = INVALID_NODE_ID; + node.m_node.reverse_node_id = it->second.second; + } + } + else + { + node.m_node.forward_node_id = it->second.first; + node.m_node.reverse_node_id = it->second.second; + } + node.m_seg = segments[idx]; node.m_segPt = m_candidates[i][j].m_point; @@ -321,13 +347,14 @@ void OsrmRouter::SetFinalPoint(m2::PointD const & finalPt) } } -void OsrmRouter::CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback) +void OsrmRouter::CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback, m2::PointD const & direction) { { threads::MutexGuard guard(m_paramsMutex); UNUSED_VALUE(guard); m_startPt = startPt; + m_startDr = direction; m_requestCancel = true; } @@ -348,13 +375,14 @@ void OsrmRouter::CalculateRouteAsync(ReadyCallback const & callback) m_isReadyThread.clear(); - m2::PointD startPt, finalPt; + m2::PointD startPt, finalPt, startDr; { threads::MutexGuard params(m_paramsMutex); UNUSED_VALUE(params); startPt = m_startPt; finalPt = m_finalPt; + startDr = m_startDr; if (m_isFinalChanged) m_cachedFinalNodes.clear(); @@ -365,7 +393,7 @@ void OsrmRouter::CalculateRouteAsync(ReadyCallback const & callback) try { - code = CalculateRouteImpl(startPt, finalPt, route); + code = CalculateRouteImpl(startPt, startDr, finalPt, route); switch (code) { case StartPointNotFound: @@ -415,7 +443,7 @@ bool IsRouteExist(RawRouteData const & r) } -OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & finalPt, Route & route) +OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, Route & route) { // 1. Find country file name and check that we are in the same MWM. string fName = m_countryFn(startPt); @@ -462,7 +490,7 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt FeatureGraphNodeVecT graphNodes; uint32_t mwmId = -1; - ResultCode const code = FindPhantomNodes(fName, startPt, finalPt, graphNodes, MAX_NODE_CANDIDATES, mwmId); + ResultCode const code = FindPhantomNodes(fName, startPt, startDr, finalPt, graphNodes, MAX_NODE_CANDIDATES, mwmId); if (m_requestCancel) return Cancelled; @@ -829,10 +857,10 @@ void OsrmRouter::FixupTurns(vector const & points, Route::TurnsT & t } } -IRouter::ResultCode OsrmRouter::FindPhantomNodes(string const & fName, m2::PointD const & startPt, m2::PointD const & finalPt, +IRouter::ResultCode OsrmRouter::FindPhantomNodes(string const & fName, m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, FeatureGraphNodeVecT & res, size_t maxCount, uint32_t & mwmId) { - Point2PhantomNode getter(m_mapping, m_pIndex); + Point2PhantomNode getter(m_mapping, m_pIndex, startDr); auto processPt = [&] (m2::PointD const & p, size_t idx) { diff --git a/routing/osrm_router.hpp b/routing/osrm_router.hpp index fcab040be8..2a83106188 100644 --- a/routing/osrm_router.hpp +++ b/routing/osrm_router.hpp @@ -41,16 +41,16 @@ public: virtual string GetName() const; virtual void ClearState(); virtual void SetFinalPoint(m2::PointD const & finalPt); - virtual void CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback); + virtual void CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback, m2::PointD const & direction = m2::PointD::Zero()); protected: - IRouter::ResultCode FindPhantomNodes(string const & fName, m2::PointD const & startPt, m2::PointD const & finalPt, + IRouter::ResultCode FindPhantomNodes(string const & fName, m2::PointD const & startPt, const m2::PointD &startDr, m2::PointD const & finalPt, FeatureGraphNodeVecT & res, size_t maxCount, uint32_t & mwmId); bool NeedReload(string const & fPath) const; void CalculateRouteAsync(ReadyCallback const & callback); - ResultCode CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & finalPt, Route & route); + ResultCode CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, Route & route); void GetTurnDirection(PathData const & node1, PathData const & node2, @@ -66,7 +66,7 @@ private: FilesMappingContainer m_container; bool m_isFinalChanged; - m2::PointD m_startPt, m_finalPt; + m2::PointD m_startPt, m_finalPt, m_startDr; FeatureGraphNodeVecT m_cachedFinalNodes; threads::Mutex m_paramsMutex; diff --git a/routing/router.hpp b/routing/router.hpp index 705be90a11..65b60c1d37 100644 --- a/routing/router.hpp +++ b/routing/router.hpp @@ -38,7 +38,7 @@ public: virtual void ClearState() {} virtual void SetFinalPoint(m2::PointD const & finalPt) = 0; - virtual void CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback) = 0; + virtual void CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback, m2::PointD const & direction = m2::PointD::Zero()) = 0; }; }