diff --git a/routing/CMakeLists.txt b/routing/CMakeLists.txt index 27b0311612..27957371bb 100644 --- a/routing/CMakeLists.txt +++ b/routing/CMakeLists.txt @@ -106,8 +106,11 @@ set( routing_session.hpp routing_settings.hpp segment.hpp + segmented_route.cpp + segmented_route.hpp speed_camera.cpp speed_camera.hpp + traffic_stash.cpp traffic_stash.hpp transition_points.hpp turn_candidate.hpp diff --git a/routing/async_router.cpp b/routing/async_router.cpp index b021167358..140bb8c044 100644 --- a/routing/async_router.cpp +++ b/routing/async_router.cpp @@ -147,7 +147,8 @@ void AsyncRouter::SetRouter(unique_ptr && router, unique_ptr delegate; m2::PointD startPoint, finalPoint, startDirection; + bool adjust = false; shared_ptr absentFetcher; shared_ptr router; @@ -275,6 +278,7 @@ void AsyncRouter::CalculateRoute() startPoint = m_startPoint; finalPoint = m_finalPoint; startDirection = m_startDirection; + adjust = m_adjust; delegate = m_delegate; router = m_router; absentFetcher = m_absentFetcher; @@ -294,7 +298,8 @@ void AsyncRouter::CalculateRoute() absentFetcher->GenerateRequest(startPoint, finalPoint); // Run basic request. - code = router->CalculateRoute(startPoint, startDirection, finalPoint, delegate->GetDelegate(), route); + code = router->CalculateRoute(startPoint, startDirection, finalPoint, adjust, + delegate->GetDelegate(), route); elapsedSec = timer.ElapsedSeconds(); // routing time LogCode(code, elapsedSec); diff --git a/routing/async_router.hpp b/routing/async_router.hpp index 9dbe332e5f..21698ed376 100644 --- a/routing/async_router.hpp +++ b/routing/async_router.hpp @@ -43,11 +43,13 @@ public: /// @param startPoint point to start routing /// @param direction start direction for routers with high cost of the turnarounds /// @param finalPoint target point for route + /// @param adjust adjust route to the previous one if possible /// @param readyCallback function to return routing result /// @param progressCallback function to update the router progress /// @param timeoutSec timeout to cancel routing. 0 is infinity. void CalculateRoute(m2::PointD const & startPoint, m2::PointD const & direction, - m2::PointD const & finalPoint, TReadyCallback const & readyCallback, + m2::PointD const & finalPoint, bool adjust, + TReadyCallback const & readyCallback, RouterDelegate::TProgressCallback const & progressCallback, uint32_t timeoutSec); @@ -114,6 +116,7 @@ private: m2::PointD m_startPoint; m2::PointD m_finalPoint; m2::PointD m_startDirection; + bool m_adjust = false; shared_ptr m_delegate; shared_ptr m_absentFetcher; shared_ptr m_router; diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp index b1ba850a9e..d7f8964011 100644 --- a/routing/base/astar_algorithm.hpp +++ b/routing/base/astar_algorithm.hpp @@ -66,6 +66,13 @@ public: CheckForStop && checkForStop, AdjustEdgeWeight && adjustEdgeWeight, PutToParents && putToParents, map & bestDistance) const; + template + Result FindPath(TGraphType & graph, TVertexType const & startVertex, + TVertexType const & finalVertex, RoutingResult & result, + my::Cancellable const & cancellable, + TOnVisitedVertexCallback onVisitedVertexCallback, + IsFinalVertex && isFinalVertex) const; + Result FindPath(TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, RoutingResult & result, my::Cancellable const & cancellable = my::Cancellable(), @@ -237,10 +244,11 @@ void AStarAlgorithm::PropagateWave(TGraphType & graph, TVertexType const // http://www.cs.princeton.edu/courses/archive/spr06/cos423/Handouts/EPP%20shortest%20path%20algorithms.pdf template +template typename AStarAlgorithm::Result AStarAlgorithm::FindPath( TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, RoutingResult & result, my::Cancellable const & cancellable, - TOnVisitedVertexCallback onVisitedVertexCallback) const + TOnVisitedVertexCallback onVisitedVertexCallback, IsFinalVertex && isFinalVertex) const { result.Clear(); if (nullptr == onVisitedVertexCallback) @@ -263,12 +271,11 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( if (steps % kVisitedVerticesPeriod == 0) onVisitedVertexCallback(vertex, finalVertex); - if (vertex == finalVertex) + if (isFinalVertex(vertex)) { ReconstructPath(vertex, parents, result.path); result.distance = bestDistance[vertex] - graph.HeuristicCostEstimate(vertex, finalVertex) + graph.HeuristicCostEstimate(startVertex, finalVertex); - ASSERT_EQUAL(graph.HeuristicCostEstimate(vertex, finalVertex), 0, ()); resultCode = Result::OK; return true; } @@ -292,6 +299,17 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( return resultCode; } +template +typename AStarAlgorithm::Result AStarAlgorithm::FindPath( + TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, + RoutingResult & result, my::Cancellable const & cancellable, + TOnVisitedVertexCallback onVisitedVertexCallback) const +{ + auto const isFinalVertex = [&](TVertexType const & vertex) { return vertex == finalVertex; }; + return FindPath(graph, startVertex, finalVertex, result, cancellable, onVisitedVertexCallback, + isFinalVertex); +} + template typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirectional( TGraphType & graph, diff --git a/routing/edge_estimator.cpp b/routing/edge_estimator.cpp index f0b5e584ac..6cc7f31f33 100644 --- a/routing/edge_estimator.cpp +++ b/routing/edge_estimator.cpp @@ -78,21 +78,12 @@ double CarEdgeEstimator::CalcSegmentWeight(Segment const & segment, RoadGeometry if (m_trafficStash) { - auto const * trafficColoring = m_trafficStash->Get(segment.GetMwmId()); - if (trafficColoring) - { - auto const dir = segment.IsForward() ? TrafficInfo::RoadSegmentId::kForwardDirection - : TrafficInfo::RoadSegmentId::kReverseDirection; - auto const it = trafficColoring->find( - TrafficInfo::RoadSegmentId(segment.GetFeatureId(), segment.GetSegmentIdx(), dir)); - SpeedGroup const speedGroup = - (it == trafficColoring->cend()) ? SpeedGroup::Unknown : it->second; - ASSERT_LESS(speedGroup, SpeedGroup::Count, ()); - double const trafficFactor = CalcTrafficFactor(speedGroup); - result *= trafficFactor; - if (speedGroup != SpeedGroup::Unknown && speedGroup != SpeedGroup::G5) - result *= kTimePenalty; - } + SpeedGroup const speedGroup = m_trafficStash->GetSpeedGroup(segment); + ASSERT_LESS(speedGroup, SpeedGroup::Count, ()); + double const trafficFactor = CalcTrafficFactor(speedGroup); + result *= trafficFactor; + if (speedGroup != SpeedGroup::Unknown && speedGroup != SpeedGroup::G5) + result *= kTimePenalty; } return result; diff --git a/routing/index_router.cpp b/routing/index_router.cpp index 9349fcc0fd..9212eedd13 100644 --- a/routing/index_router.cpp +++ b/routing/index_router.cpp @@ -38,22 +38,34 @@ size_t constexpr kMaxRoadCandidates = 6; float constexpr kProgressInterval = 2; uint32_t constexpr kDrawPointsPeriod = 10; +// If user left the route within this range, adjust the route. Else full rebuild. +double constexpr kAdjustRange = 5000.0; +// Propagate astar wave for some distance to try to find a better return point. +double constexpr kAdjustDistance = 2000.0; +// Full rebuild if distance to finish is lesser. +double const kMinDistanceToFinish = kAdjustDistance * 2.0; + +template +IRouter::ResultCode ConvertResult(typename AStarAlgorithm::Result result) +{ + switch (result) + { + case AStarAlgorithm::Result::NoPath: return IRouter::RouteNotFound; + case AStarAlgorithm::Result::Cancelled: return IRouter::Cancelled; + case AStarAlgorithm::Result::OK: return IRouter::NoError; + } +} + template IRouter::ResultCode FindPath( - typename Graph::TVertexType const & start, typename Graph::TVertexType const & finish, - RouterDelegate const & delegate, Graph & graph, - typename AStarAlgorithm::TOnVisitedVertexCallback const & onVisitedVertexCallback, - RoutingResult & routingResult) + typename Graph::TVertexType const & start, typename Graph::TVertexType const & finish, + RouterDelegate const & delegate, Graph & graph, + typename AStarAlgorithm::TOnVisitedVertexCallback const & onVisitedVertexCallback, + RoutingResult & routingResult) { AStarAlgorithm algorithm; - auto const resultCode = algorithm.FindPathBidirectional(graph, start, finish, routingResult, - delegate, onVisitedVertexCallback); - switch (resultCode) - { - case AStarAlgorithm::Result::NoPath: return IRouter::RouteNotFound; - case AStarAlgorithm::Result::Cancelled: return IRouter::Cancelled; - case AStarAlgorithm::Result::OK: return IRouter::NoError; - } + return ConvertResult(algorithm.FindPathBidirectional(graph, start, finish, routingResult, + delegate, onVisitedVertexCallback)); } bool IsDeadEnd(Segment const & segment, bool isOutgoing, WorldGraph & worldGraph) @@ -121,6 +133,7 @@ IndexRouter::IndexRouter(string const & name, TCountryFileFn const & countryFile { CHECK(!m_name.empty(), ()); CHECK(m_numMwmIds, ()); + CHECK(m_numMwmTree, ()); CHECK(m_trafficStash, ()); CHECK(m_vehicleModelFactory, ()); CHECK(m_estimator, ()); @@ -129,7 +142,7 @@ IndexRouter::IndexRouter(string const & name, TCountryFileFn const & countryFile IRouter::ResultCode IndexRouter::CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, + m2::PointD const & finalPoint, bool adjust, RouterDelegate const & delegate, Route & route) { if (!AllMwmsHaveRoutingIndex(m_index, route)) @@ -139,19 +152,30 @@ IRouter::ResultCode IndexRouter::CalculateRoute(m2::PointD const & startPoint, string const finishCountry = m_countryFileFn(finalPoint); return CalculateRoute(startCountry, finishCountry, false /* blockMwmBorders */, startPoint, - startDirection, finalPoint, delegate, route); + startDirection, finalPoint, adjust, delegate, route); } IRouter::ResultCode IndexRouter::CalculateRoute(string const & startCountry, string const & finishCountry, bool forSingleMwm, m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, + m2::PointD const & finalPoint, bool adjust, RouterDelegate const & delegate, Route & route) { try { - return DoCalculateRoute(startCountry, finishCountry, forSingleMwm, startPoint, startDirection, + auto const startFile = platform::CountryFile(startCountry); + auto const finishFile = platform::CountryFile(finishCountry); + + if (adjust && !m_lastRoute.IsEmpty() && finalPoint == m_lastRoute.GetFinish()) + { + double const distanceToRoute = m_lastRoute.CalcDistance(startPoint); + double const distanceToFinish = MercatorBounds::DistanceOnEarth(startPoint, finalPoint); + if (distanceToRoute <= kAdjustRange && distanceToFinish >= kMinDistanceToFinish) + return AdjustRoute(startFile, startPoint, startDirection, finalPoint, delegate, route); + } + + return DoCalculateRoute(startFile, finishFile, forSingleMwm, startPoint, startDirection, finalPoint, delegate, route); } catch (RootException const & e) @@ -162,54 +186,39 @@ IRouter::ResultCode IndexRouter::CalculateRoute(string const & startCountry, } } -IRouter::ResultCode IndexRouter::DoCalculateRoute(string const & startCountry, - string const & finishCountry, bool forSingleMwm, - m2::PointD const & startPoint, +IRouter::ResultCode IndexRouter::DoCalculateRoute(platform::CountryFile const & startCountry, + platform::CountryFile const & finishCountry, + bool forSingleMwm, m2::PointD const & startPoint, m2::PointD const & /* startDirection */, m2::PointD const & finalPoint, RouterDelegate const & delegate, Route & route) { - CHECK(m_numMwmTree, ()); - auto const startFile = platform::CountryFile(startCountry); - auto const finishFile = platform::CountryFile(finishCountry); + m_lastRoute.Clear(); TrafficStash::Guard guard(*m_trafficStash); - WorldGraph graph( - make_unique(m_numMwmIds, m_numMwmTree, m_vehicleModelFactory, m_countryRectFn, - m_index, m_indexManager), - IndexGraphLoader::Create(m_numMwmIds, m_vehicleModelFactory, m_estimator, m_index), - m_estimator); + WorldGraph graph = MakeWorldGraph(); - bool const isStartMwmLoaded = m_index.IsLoaded(startFile); - bool const isFinishMwmLoaded = m_index.IsLoaded(finishFile); + bool const isStartMwmLoaded = m_index.IsLoaded(startCountry); + bool const isFinishMwmLoaded = m_index.IsLoaded(finishCountry); if (!isStartMwmLoaded) - route.AddAbsentCountry(startCountry); + route.AddAbsentCountry(startCountry.GetName()); if (!isFinishMwmLoaded) - route.AddAbsentCountry(finishCountry); + route.AddAbsentCountry(finishCountry.GetName()); if (!isStartMwmLoaded || !isFinishMwmLoaded) return IRouter::NeedMoreMaps; - Edge startEdge; - if (!FindClosestEdge(startFile, startPoint, true /* isOutgoing */, graph, startEdge)) + Segment startSegment; + if (!FindClosestSegment(startCountry, startPoint, true /* isOutgoing */, graph, startSegment)) return IRouter::StartPointNotFound; - Edge finishEdge; - if (!FindClosestEdge(finishFile, finalPoint, false /* isOutgoing */, graph, finishEdge)) + Segment finishSegment; + if (!FindClosestSegment(finishCountry, finalPoint, false /* isOutgoing */, graph, finishSegment)) return IRouter::EndPointNotFound; - IndexGraphStarter::FakeVertex const start( - Segment(m_numMwmIds->GetId(startFile), startEdge.GetFeatureId().m_index, startEdge.GetSegId(), - true /* forward */), - startPoint); - IndexGraphStarter::FakeVertex const finish( - Segment(m_numMwmIds->GetId(finishFile), finishEdge.GetFeatureId().m_index, - finishEdge.GetSegId(), true /* forward */), - finalPoint); - WorldGraph::Mode mode = WorldGraph::Mode::SingleMwm; if (forSingleMwm) mode = WorldGraph::Mode::SingleMwm; - else if (AreMwmsNear(start.GetMwmId(), finish.GetMwmId())) + else if (AreMwmsNear(startSegment.GetMwmId(), finishSegment.GetMwmId())) mode = WorldGraph::Mode::LeapsIfPossible; else mode = WorldGraph::Mode::LeapsOnly; @@ -217,7 +226,8 @@ IRouter::ResultCode IndexRouter::DoCalculateRoute(string const & startCountry, LOG(LINFO, ("Routing in mode:", graph.GetMode())); - IndexGraphStarter starter(start, finish, graph); + IndexGraphStarter starter(IndexGraphStarter::FakeVertex(startSegment, startPoint), + IndexGraphStarter::FakeVertex(finishSegment, finalPoint), graph); AStarProgress progress(0, 100); progress.Initialize(starter.GetStartVertex().GetPoint(), starter.GetFinishVertex().GetPoint()); @@ -249,16 +259,134 @@ IRouter::ResultCode IndexRouter::DoCalculateRoute(string const & startCountry, CHECK_GREATER_OR_EQUAL(segments.size(), routingResult.path.size(), ()); - if (!RedressRoute(segments, delegate, forSingleMwm, starter, route)) - return IRouter::InternalError; - if (delegate.IsCancelled()) - return IRouter::Cancelled; + auto redressResult = RedressRoute(segments, delegate, forSingleMwm, starter, route); + if (redressResult != IRouter::NoError) + return redressResult; + + m_lastRoute.Init(startPoint, finalPoint); + for (Segment const & segment : routingResult.path) + m_lastRoute.AddStep(segment, starter.GetPoint(segment, true /* front */)); + return IRouter::NoError; } -bool IndexRouter::FindClosestEdge(platform::CountryFile const & file, m2::PointD const & point, - bool isOutgoing, WorldGraph & worldGraph, - Edge & closestEdge) const +IRouter::ResultCode IndexRouter::AdjustRoute(platform::CountryFile const & startCountry, + m2::PointD const & startPoint, + m2::PointD const & startDirection, + m2::PointD const & finalPoint, + RouterDelegate const & delegate, Route & route) +{ + my::Timer timer; + TrafficStash::Guard guard(*m_trafficStash); + WorldGraph graph = MakeWorldGraph(); + graph.SetMode(WorldGraph::Mode::NoLeaps); + + Segment startSegment; + if (!FindClosestSegment(startCountry, startPoint, true /* isOutgoing */, graph, startSegment)) + return IRouter::StartPointNotFound; + + IndexGraphStarter starter( + IndexGraphStarter::FakeVertex(startSegment, startPoint), + IndexGraphStarter::FakeVertex(m_lastRoute.GetFinishSegment(), finalPoint), graph); + + AStarProgress progress(0, 100); + progress.Initialize(starter.GetStartVertex().GetPoint(), starter.GetFinishVertex().GetPoint()); + + uint32_t drawPointsStep = 0; + auto onVisitJunction = [&](Segment const & from, Segment const & /* to */) { + m2::PointD const & point = starter.GetPoint(from, true /* front */); + auto const lastValue = progress.GetLastValue(); + auto const newValue = progress.GetProgressForDirectedAlgo(point); + if (newValue - lastValue > kProgressInterval) + delegate.OnProgress(newValue); + if (drawPointsStep % kDrawPointsPeriod == 0) + delegate.OnPointCheck(point); + ++drawPointsStep; + }; + + AStarAlgorithm algorithm; + RoutingResult routingResult; + + auto const & steps = m_lastRoute.GetSteps(); + set routeSegmentsSet; + for (auto const & step : steps) + { + auto const & segment = step.GetSegment(); + if (!IndexGraphStarter::IsFakeSegment(segment)) + routeSegmentsSet.insert(segment); + } + + double const requiredDistanceToFinish = + MercatorBounds::DistanceOnEarth(startPoint, finalPoint) - kAdjustDistance; + CHECK_GREATER(requiredDistanceToFinish, 0.0, ()); + + auto const isFinalVertex = [&](Segment const & vertex) { + if (vertex == starter.GetFinish()) + return true; + + return routeSegmentsSet.count(vertex) > 0 && + MercatorBounds::DistanceOnEarth( + starter.GetPoint(vertex, true /* front */), finalPoint) <= requiredDistanceToFinish; + }; + + auto const code = ConvertResult( + algorithm.FindPath(starter, starter.GetStart(), starter.GetFinish(), routingResult, delegate, + onVisitJunction, isFinalVertex)); + if (code != IRouter::NoError) + return code; + + size_t const adjustingRouteSize = routingResult.path.size(); + AppendRemainingRoute(routingResult.path); + + auto const redressResult = RedressRoute(routingResult.path, delegate, false, starter, route); + if (redressResult != IRouter::NoError) + return redressResult; + + LOG(LINFO, ("Adjust route, elapsed:", timer.ElapsedSeconds(), ", start:", + MercatorBounds::ToLatLon(startPoint), ", finish:", + MercatorBounds::ToLatLon(finalPoint), ", old route:", steps.size(), ", new route:", + routingResult.path.size(), ", adjust:", adjustingRouteSize)); + + return IRouter::NoError; +} + +void IndexRouter::AppendRemainingRoute(vector & route) const +{ + auto const steps = m_lastRoute.GetSteps(); + Segment const joinSegment = route.back(); + + // Route to finish found, append is not needed. + if (IndexGraphStarter::IsFakeSegment(joinSegment)) + return; + + for (size_t i = 0; i < steps.size(); ++i) + { + if (steps[i].GetSegment() == joinSegment) + { + for (size_t j = i + 1; j < steps.size(); ++j) + route.push_back(steps[j].GetSegment()); + + return; + } + } + + CHECK(false, + ("Can't find", joinSegment, ", m_routeSegments:", steps.size(), "path:", route.size())); +} + +WorldGraph IndexRouter::MakeWorldGraph() +{ + WorldGraph graph( + make_unique(m_numMwmIds, m_numMwmTree, m_vehicleModelFactory, m_countryRectFn, + m_index, m_indexManager), + IndexGraphLoader::Create(m_numMwmIds, m_vehicleModelFactory, m_estimator, m_index), + m_estimator); + return graph; +} + +bool IndexRouter::FindClosestSegment(platform::CountryFile const & file, m2::PointD const & point, + bool isOutgoing, WorldGraph & worldGraph, + Segment & closestSegment) const { MwmSet::MwmHandle handle = m_index.GetMwmHandleByCountryFile(file); if (!handle.IsAlive()) @@ -294,7 +422,9 @@ bool IndexRouter::FindClosestEdge(platform::CountryFile const & file, m2::PointD if (minIndex == candidates.size()) return false; - closestEdge = candidates[minIndex].first; + Edge const & closestEdge = candidates[minIndex].first; + closestSegment = Segment(m_numMwmIds->GetId(file), closestEdge.GetFeatureId().m_index, + closestEdge.GetSegId(), true /* forward */); return true; } @@ -358,8 +488,9 @@ IRouter::ResultCode IndexRouter::ProcessLeaps(vector const & input, return IRouter::NoError; } -bool IndexRouter::RedressRoute(vector const & segments, RouterDelegate const & delegate, - bool forSingleMwm, IndexGraphStarter & starter, Route & route) const +IRouter::ResultCode IndexRouter::RedressRoute(vector const & segments, + RouterDelegate const & delegate, bool forSingleMwm, + IndexGraphStarter & starter, Route & route) const { vector junctions; size_t const numPoints = IndexGraphStarter::GetRouteNumPoints(segments); @@ -382,7 +513,7 @@ bool IndexRouter::RedressRoute(vector const & segments, RouterDelegate if (!route.IsValid()) { LOG(LERROR, ("ReconstructRoute failed. Segments:", segments.size())); - return false; + return IRouter::InternalError; } Route::TTimes times; @@ -397,7 +528,11 @@ bool IndexRouter::RedressRoute(vector const & segments, RouterDelegate } route.SetSectionTimes(move(times)); - return true; + + if (delegate.IsCancelled()) + return IRouter::Cancelled; + + return IRouter::NoError; } bool IndexRouter::AreMwmsNear(NumMwmId startId, NumMwmId finishId) const diff --git a/routing/index_router.hpp b/routing/index_router.hpp index 5b12412106..a2c003d064 100644 --- a/routing/index_router.hpp +++ b/routing/index_router.hpp @@ -8,6 +8,7 @@ #include "routing/num_mwm_id.hpp" #include "routing/router.hpp" #include "routing/routing_mapping.hpp" +#include "routing/segmented_route.hpp" #include "routing/world_graph.hpp" #include "routing_common/vehicle_model.hpp" @@ -40,7 +41,7 @@ public: virtual string GetName() const override { return m_name; } virtual IRouter::ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, + m2::PointD const & finalPoint, bool adjust, RouterDelegate const & delegate, Route & route) override; @@ -56,19 +57,27 @@ private: IRouter::ResultCode CalculateRoute(string const & startCountry, string const & finishCountry, bool forSingleMwm, m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, RouterDelegate const & delegate, - Route & route); - IRouter::ResultCode DoCalculateRoute(string const & startCountry, string const & finishCountry, + m2::PointD const & finalPoint, bool adjust, + RouterDelegate const & delegate, Route & route); + IRouter::ResultCode DoCalculateRoute(platform::CountryFile const & startCountry, + platform::CountryFile const & finishCountry, bool forSingleMwm, m2::PointD const & startPoint, m2::PointD const & startDirection, m2::PointD const & finalPoint, RouterDelegate const & delegate, Route & route); + IRouter::ResultCode AdjustRoute(platform::CountryFile const & startCountry, + m2::PointD const & startPoint, m2::PointD const & startDirection, + m2::PointD const & finalPoint, RouterDelegate const & delegate, + Route & route); + void AppendRemainingRoute(std::vector & route) const; + + WorldGraph MakeWorldGraph(); /// \brief Finds closest edges which may be considered as start of finish of the route. /// \param isOutgoing == true is |point| is considered as the start of the route. /// isOutgoing == false is |point| is considered as the finish of the route. - bool FindClosestEdge(platform::CountryFile const & file, m2::PointD const & point, - bool isOutgoing, WorldGraph & worldGraph, Edge & closestEdge) const; + bool FindClosestSegment(platform::CountryFile const & file, m2::PointD const & point, + bool isOutgoing, WorldGraph & worldGraph, Segment & closestSegment) const; // Input route may contains 'leaps': shortcut edges from mwm border enter to exit. // ProcessLeaps replaces each leap with calculated route through mwm. IRouter::ResultCode ProcessLeaps(vector const & input, @@ -76,8 +85,9 @@ private: WorldGraph::Mode prevMode, IndexGraphStarter & starter, vector & output); - bool RedressRoute(vector const & segments, RouterDelegate const & delegate, - bool forSingleMwm, IndexGraphStarter & starter, Route & route) const; + IRouter::ResultCode RedressRoute(vector const & segments, + RouterDelegate const & delegate, bool forSingleMwm, + IndexGraphStarter & starter, Route & route) const; bool AreMwmsNear(NumMwmId startId, NumMwmId finishId) const; @@ -93,5 +103,6 @@ private: shared_ptr m_vehicleModelFactory; shared_ptr m_estimator; unique_ptr m_directionsEngine; + SegmentedRoute m_lastRoute; }; } // namespace routing diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 0a8c69a6ed..fe1ba87524 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -147,6 +147,7 @@ bool RoadGraphRouter::CheckMapExistence(m2::PointD const & point, Route & route) IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoint, m2::PointD const & /* startDirection */, m2::PointD const & finalPoint, + bool /* adjust */, RouterDelegate const & delegate, Route & route) { if (!CheckMapExistence(startPoint, route) || !CheckMapExistence(finalPoint, route)) diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp index 8982601a26..280f96b23d 100644 --- a/routing/road_graph_router.hpp +++ b/routing/road_graph_router.hpp @@ -35,8 +35,8 @@ public: string GetName() const override { return m_name; } void ClearState() override; ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, RouterDelegate const & delegate, - Route & route) override; + m2::PointD const & finalPoint, bool adjust, + RouterDelegate const & delegate, Route & route) override; private: /// Checks existance and add absent maps to route. diff --git a/routing/router.hpp b/routing/router.hpp index 07400158f2..9ce12a91af 100644 --- a/routing/router.hpp +++ b/routing/router.hpp @@ -72,14 +72,15 @@ public: /// @param startPoint point to start routing /// @param startDirection start direction for routers with high cost of the turnarounds /// @param finalPoint target point for route + /// @param adjust adjust route to the previous one if possible /// @param delegate callback functions and cancellation flag /// @param route result route /// @return ResultCode error code or NoError if route was initialised /// @see Cancellable virtual ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, RouterDelegate const & delegate, - Route & route) = 0; + m2::PointD const & finalPoint, bool adjust, + RouterDelegate const & delegate, Route & route) = 0; }; } // namespace routing diff --git a/routing/routing.pro b/routing/routing.pro index f8341d79ef..e5b813a1ef 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -56,7 +56,9 @@ SOURCES += \ routing_helpers.cpp \ routing_mapping.cpp \ routing_session.cpp \ + segmented_route.cpp \ speed_camera.cpp \ + traffic_stash.cpp \ turns.cpp \ turns_generator.cpp \ turns_notification_manager.cpp \ @@ -120,6 +122,7 @@ HEADERS += \ routing_session.hpp \ routing_settings.hpp \ segment.hpp \ + segmented_route.hpp \ speed_camera.hpp \ traffic_stash.hpp \ transition_points.hpp \ diff --git a/routing/routing_session.cpp b/routing/routing_session.cpp index dfdf2c3525..b2d8b68db7 100644 --- a/routing/routing_session.cpp +++ b/routing/routing_session.cpp @@ -79,12 +79,12 @@ void RoutingSession::BuildRoute(m2::PointD const & startPoint, m2::PointD const m_router->ClearState(); m_isFollowing = false; m_routingRebuildCount = -1; // -1 for the first rebuild. - RebuildRoute(startPoint, m_buildReadyCallback, timeoutSec, RouteBuilding); + RebuildRoute(startPoint, m_buildReadyCallback, timeoutSec, RouteBuilding, false /* adjust */); } void RoutingSession::RebuildRoute(m2::PointD const & startPoint, TReadyCallback const & readyCallback, uint32_t timeoutSec, - State routeRebuildingState) + State routeRebuildingState, bool adjust) { ASSERT(m_router != nullptr, ()); ASSERT_NOT_EQUAL(m_endPoint, m2::PointD::Zero(), ("End point was not set")); @@ -95,7 +95,7 @@ void RoutingSession::RebuildRoute(m2::PointD const & startPoint, // Use old-style callback construction, because lambda constructs buggy function on Android // (callback param isn't captured by value). - m_router->CalculateRoute(startPoint, startPoint - m_lastGoodPosition, m_endPoint, + m_router->CalculateRoute(startPoint, startPoint - m_lastGoodPosition, m_endPoint, adjust, DoReadyCallback(*this, readyCallback, m_routingSessionMutex), m_progressCallback, timeoutSec); } @@ -157,7 +157,7 @@ void RoutingSession::RebuildRouteOnTrafficUpdate() // Cancel current route building if going. m_router->ClearState(); RebuildRoute(m_lastGoodPosition, m_rebuildReadyCallback, 0 /* timeoutSec */, - routing::RoutingSession::State::RouteRebuilding); + routing::RoutingSession::State::RouteRebuilding, false /* adjust */); } void RoutingSession::Reset() diff --git a/routing/routing_session.hpp b/routing/routing_session.hpp index a94a102dc3..6acfe3e244 100644 --- a/routing/routing_session.hpp +++ b/routing/routing_session.hpp @@ -90,7 +90,7 @@ public: void BuildRoute(m2::PointD const & startPoint, m2::PointD const & endPoint, uint32_t timeoutSec); void RebuildRoute(m2::PointD const & startPoint, TReadyCallback const & readyCallback, - uint32_t timeoutSec, State routeRebuildingState); + uint32_t timeoutSec, State routeRebuildingState, bool adjust); m2::PointD GetEndPoint() const { return m_endPoint; } bool IsActive() const { return (m_state != RoutingNotActive); } diff --git a/routing/routing_tests/async_router_test.cpp b/routing/routing_tests/async_router_test.cpp index 83827f58b6..6aeb0ba1dc 100644 --- a/routing/routing_tests/async_router_test.cpp +++ b/routing/routing_tests/async_router_test.cpp @@ -30,8 +30,8 @@ public: // IRouter overrides: string GetName() const override { return "Dummy"; } ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, - m2::PointD const & finalPoint, RouterDelegate const & delegate, - Route & route) override + m2::PointD const & finalPoint, bool adjust, + RouterDelegate const & delegate, Route & route) override { vector points({startPoint, finalPoint}); route = Route("dummy", points.begin(), points.end()); @@ -97,8 +97,9 @@ UNIT_TEST(NeedMoreMapsSignalTest) DummyResultCallback resultCallback(2 /* expectedCalls */); AsyncRouter async(DummyStatisticsCallback, nullptr /* pointCheckCallback */); async.SetRouter(move(router), move(fetcher)); - async.CalculateRoute({1, 2}, {3, 4}, {5, 6}, bind(ref(resultCallback), _1, _2), - nullptr /* progressCallback */, 0 /* timeoutSec */); + async.CalculateRoute({1, 2}, {3, 4}, {5, 6}, false /* rebuild */, + bind(ref(resultCallback), _1, _2), nullptr /* progressCallback */, + 0 /* timeoutSec */); resultCallback.WaitFinish(); @@ -118,8 +119,9 @@ UNIT_TEST(StandartAsyncFogTest) DummyResultCallback resultCallback(1 /* expectedCalls */); AsyncRouter async(DummyStatisticsCallback, nullptr /* pointCheckCallback */); async.SetRouter(move(router), move(fetcher)); - async.CalculateRoute({1, 2}, {3, 4}, {5, 6}, bind(ref(resultCallback), _1, _2), - nullptr /* progressCallback */, 0 /* timeoutSec */); + async.CalculateRoute({1, 2}, {3, 4}, {5, 6}, false /* rebuild */, + bind(ref(resultCallback), _1, _2), nullptr /* progressCallback */, + 0 /* timeoutSec */); resultCallback.WaitFinish(); diff --git a/routing/routing_tests/routing_session_test.cpp b/routing/routing_tests/routing_session_test.cpp index b4a830a2c2..df5dba265a 100644 --- a/routing/routing_tests/routing_session_test.cpp +++ b/routing/routing_tests/routing_session_test.cpp @@ -32,7 +32,7 @@ public: void ClearState() override {} ResultCode CalculateRoute(m2::PointD const & /* startPoint */, m2::PointD const & /* startDirection */, - m2::PointD const & /* finalPoint */, + m2::PointD const & /* finalPoint */, bool adjust, RouterDelegate const & /* delegate */, Route & route) override { ++m_buildCount; @@ -218,7 +218,7 @@ UNIT_TEST(TestFollowRouteFlagPersistence) session.RebuildRoute( kTestRoute.front(), [&rebuildTimedSignal](Route const &, IRouter::ResultCode) { rebuildTimedSignal.Signal(); }, 0, - RoutingSession::State::RouteBuilding); + RoutingSession::State::RouteBuilding, false /* adjust */); TEST(rebuildTimedSignal.WaitUntil(time), ("Route was not built.")); TEST(session.IsFollowing(), ()); } diff --git a/routing/segmented_route.cpp b/routing/segmented_route.cpp new file mode 100644 index 0000000000..dbd475d379 --- /dev/null +++ b/routing/segmented_route.cpp @@ -0,0 +1,36 @@ +#include "routing/segmented_route.hpp" + +#include "geometry/mercator.hpp" + +#include "base/assert.hpp" + +#include +#include + +namespace routing +{ +void SegmentedRoute::Init(m2::PointD const & start, m2::PointD const & finish) +{ + m_start = start; + m_finish = finish; + m_steps.clear(); +} + +double SegmentedRoute::CalcDistance(m2::PointD const & point) const +{ + CHECK(!IsEmpty(), ()); + + double result = std::numeric_limits::max(); + for (auto const & step : m_steps) + result = std::min(result, MercatorBounds::DistanceOnEarth(point, step.GetPoint())); + + return result; +} + +Segment const & SegmentedRoute::GetFinishSegment() const +{ + // Last segment is fake, before last is finish + CHECK_GREATER(m_steps.size(), 2, ()); + return m_steps[m_steps.size() - 2].GetSegment(); +} +} // namespace routing diff --git a/routing/segmented_route.hpp b/routing/segmented_route.hpp new file mode 100644 index 0000000000..c393d88cec --- /dev/null +++ b/routing/segmented_route.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "routing/segment.hpp" + +#include "geometry/point2d.hpp" + +#include + +namespace routing +{ +class SegmentedRouteStep final +{ +public: + SegmentedRouteStep() = default; + + SegmentedRouteStep(Segment const & segment, m2::PointD const & point) + : m_segment(segment), m_point(point) + { + } + + Segment const & GetSegment() const { return m_segment; } + m2::PointD const & GetPoint() const { return m_point; } + +private: + Segment m_segment; + // The front point of segment + m2::PointD m_point = m2::PointD::Zero(); +}; + +class SegmentedRoute final +{ +public: + void Init(m2::PointD const & start, m2::PointD const & finish); + + void AddStep(Segment const & segment, m2::PointD const & point) + { + m_steps.emplace_back(segment, point); + } + + void Clear() { Init(m2::PointD::Zero(), m2::PointD::Zero()); } + + double CalcDistance(m2::PointD const & point) const; + Segment const & GetFinishSegment() const; + + m2::PointD const & GetStart() const { return m_start; } + m2::PointD const & GetFinish() const { return m_finish; } + std::vector const & GetSteps() const { return m_steps; } + bool IsEmpty() const { return m_steps.empty(); } + +private: + m2::PointD m_start = m2::PointD::Zero(); + m2::PointD m_finish = m2::PointD::Zero(); + std::vector m_steps; +}; +} // namespace routing diff --git a/routing/traffic_stash.cpp b/routing/traffic_stash.cpp new file mode 100644 index 0000000000..04c2856b5b --- /dev/null +++ b/routing/traffic_stash.cpp @@ -0,0 +1,38 @@ +#include "routing/traffic_stash.hpp" + +#include "base/checked_cast.hpp" + +#include + +namespace routing +{ +traffic::SpeedGroup TrafficStash::GetSpeedGroup(Segment const & segment) const +{ + auto itMwm = m_mwmToTraffic.find(segment.GetMwmId()); + if (itMwm == m_mwmToTraffic.cend()) + return traffic::SpeedGroup::Unknown; + + auto const & coloring = itMwm->second; + auto const itSeg = coloring->find(traffic::TrafficInfo::RoadSegmentId( + segment.GetFeatureId(), base::asserted_cast(segment.GetSegmentIdx()), + segment.IsForward() ? traffic::TrafficInfo::RoadSegmentId::kForwardDirection + : traffic::TrafficInfo::RoadSegmentId::kReverseDirection)); + + if (itSeg == coloring->cend()) + return traffic::SpeedGroup::Unknown; + + return itSeg->second; +} + +void TrafficStash::CopyTraffic() +{ + std::map> copy; + m_source.CopyTraffic(copy); + for (auto const & kv : copy) + { + auto const numMwmId = m_numMwmIds->GetId(kv.first.GetInfo()->GetLocalFile().GetCountryFile()); + CHECK(kv.second, ()); + SetColoring(numMwmId, kv.second); + } +} +} // namespace routing diff --git a/routing/traffic_stash.hpp b/routing/traffic_stash.hpp index 9b456a27fc..679ebb686e 100644 --- a/routing/traffic_stash.hpp +++ b/routing/traffic_stash.hpp @@ -1,12 +1,15 @@ #pragma once #include "routing/num_mwm_id.hpp" +#include "routing/segment.hpp" #include "traffic/traffic_cache.hpp" #include "traffic/traffic_info.hpp" #include "indexer/mwm_set.hpp" +#include "base/assert.hpp" + #include namespace routing @@ -27,16 +30,10 @@ public: TrafficStash(traffic::TrafficCache const & source, shared_ptr numMwmIds) : m_source(source), m_numMwmIds(numMwmIds) { + CHECK(m_numMwmIds, ()); } - traffic::TrafficInfo::Coloring const * Get(NumMwmId numMwmId) const - { - auto it = m_mwmToTraffic.find(numMwmId); - if (it == m_mwmToTraffic.cend()) - return nullptr; - - return it->second.get(); - } + traffic::SpeedGroup GetSpeedGroup(Segment const & segment) const; void SetColoring(NumMwmId numMwmId, std::shared_ptr coloring) { @@ -49,16 +46,7 @@ public: } private: - void CopyTraffic() - { - std::map> copy; - m_source.CopyTraffic(copy); - for (auto it : copy) - { - auto const numMwmId = m_numMwmIds->GetId(it.first.GetInfo()->GetLocalFile().GetCountryFile()); - SetColoring(numMwmId, it.second); - } - } + void CopyTraffic(); void Clear() { m_mwmToTraffic.clear(); } diff --git a/xcode/routing/routing.xcodeproj/project.pbxproj b/xcode/routing/routing.xcodeproj/project.pbxproj index 8e47a6a4ad..aaedc75fb7 100644 --- a/xcode/routing/routing.xcodeproj/project.pbxproj +++ b/xcode/routing/routing.xcodeproj/project.pbxproj @@ -46,6 +46,9 @@ 0C5FEC6D1DDE19A40017688C /* index_graph_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 0C5FEC6C1DDE19A40017688C /* index_graph_test.cpp */; }; 0C62BFE61E8ADC3100055A79 /* coding.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 0C62BFE51E8ADC3100055A79 /* coding.hpp */; }; 0C8705051E0182F200BCAF71 /* route_point.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 0C8705041E0182F200BCAF71 /* route_point.hpp */; }; + 0CA683581EE04ADB004B7269 /* segmented_route.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 0CA683561EE04ADB004B7269 /* segmented_route.cpp */; }; + 0CA683591EE04ADB004B7269 /* segmented_route.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 0CA683571EE04ADB004B7269 /* segmented_route.hpp */; }; + 0CA6835B1EE04AF3004B7269 /* traffic_stash.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 0CA6835A1EE04AF3004B7269 /* traffic_stash.cpp */; }; 0CF5E8AA1E8EA7A1001ED497 /* coding_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 0CF5E8A91E8EA7A1001ED497 /* coding_test.cpp */; }; 3462FDAD1DC1E5BF00906FD7 /* libopening_hours.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 3462FDAC1DC1E5BF00906FD7 /* libopening_hours.a */; }; 349D1CE01E3F589900A878FD /* restrictions_serialization.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 349D1CDE1E3F589900A878FD /* restrictions_serialization.cpp */; }; @@ -302,6 +305,9 @@ 0C5FEC6C1DDE19A40017688C /* index_graph_test.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = index_graph_test.cpp; sourceTree = ""; }; 0C62BFE51E8ADC3100055A79 /* coding.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = coding.hpp; sourceTree = ""; }; 0C8705041E0182F200BCAF71 /* route_point.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = route_point.hpp; sourceTree = ""; }; + 0CA683561EE04ADB004B7269 /* segmented_route.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = segmented_route.cpp; sourceTree = ""; }; + 0CA683571EE04ADB004B7269 /* segmented_route.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = segmented_route.hpp; sourceTree = ""; }; + 0CA6835A1EE04AF3004B7269 /* traffic_stash.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = traffic_stash.cpp; sourceTree = ""; }; 0CF5E8A91E8EA7A1001ED497 /* coding_test.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = coding_test.cpp; sourceTree = ""; }; 3462FDAC1DC1E5BF00906FD7 /* libopening_hours.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libopening_hours.a; path = "../../../omim-build/xcode/Debug/libopening_hours.a"; sourceTree = ""; }; 349D1CDE1E3F589900A878FD /* restrictions_serialization.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = restrictions_serialization.cpp; sourceTree = ""; }; @@ -813,8 +819,11 @@ 670EE55B1B6001E7001E8064 /* routing_session.hpp */, 670EE55C1B6001E7001E8064 /* routing_settings.hpp */, 0C470E6F1E0D4EB1005B824D /* segment.hpp */, + 0CA683561EE04ADB004B7269 /* segmented_route.cpp */, + 0CA683571EE04ADB004B7269 /* segmented_route.hpp */, A1876BC41BB19C4300C9C743 /* speed_camera.cpp */, A1876BC51BB19C4300C9C743 /* speed_camera.hpp */, + 0CA6835A1EE04AF3004B7269 /* traffic_stash.cpp */, 0C090C831E4E275E00D52AFD /* traffic_stash.hpp */, 56099E271CC7C97D00A7772A /* turn_candidate.hpp */, 674F9BC61B0A580E00704FFA /* turns_generator.cpp */, @@ -920,6 +929,7 @@ 0C5FEC6B1DDE193F0017688C /* road_point.hpp in Headers */, 56099E2B1CC7C97D00A7772A /* turn_candidate.hpp in Headers */, 56C4392D1E93E5DF00998E29 /* transition_points.hpp in Headers */, + 0CA683591EE04ADB004B7269 /* segmented_route.hpp in Headers */, 0C5FEC551DDE191E0017688C /* edge_estimator.hpp in Headers */, 670B84C11A9381D900CE4492 /* cross_routing_context.hpp in Headers */, 0C12ED241E5C822A0080D0F4 /* index_router.hpp in Headers */,