From 372e1e7f681360d315e3fd49232319aa3d878ab7 Mon Sep 17 00:00:00 2001 From: tatiana-kondakova Date: Tue, 21 Nov 2017 19:19:53 +0300 Subject: [PATCH] Restrict pedestrian time in transit routing --- routing/base/astar_algorithm.hpp | 139 ++++++++++++------ routing/cross_mwm_router.cpp | 2 +- routing/fake_edges_container.hpp | 5 +- routing/index_graph.cpp | 10 +- routing/index_graph_starter.cpp | 56 ++++--- routing/index_graph_starter.hpp | 15 +- routing/index_router.cpp | 33 +++-- routing/route_weight.cpp | 6 +- routing/route_weight.hpp | 41 ++++-- .../routing_tests/astar_algorithm_test.cpp | 42 +++++- routing/routing_tests/index_graph_tools.cpp | 6 +- routing/single_vehicle_world_graph.cpp | 15 +- routing/single_vehicle_world_graph.hpp | 1 + routing/transit_graph.cpp | 22 ++- routing/transit_world_graph.cpp | 9 +- routing/transit_world_graph.hpp | 12 ++ routing/world_graph.hpp | 6 +- track_analyzing/track_matcher.cpp | 2 +- 18 files changed, 274 insertions(+), 148 deletions(-) diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp index 2bf7503352..9ebd2b7e3e 100644 --- a/routing/base/astar_algorithm.hpp +++ b/routing/base/astar_algorithm.hpp @@ -7,6 +7,7 @@ #include "base/cancellable.hpp" #include +#include #include #include #include @@ -47,7 +48,11 @@ public: return os; } - using TOnVisitedVertexCallback = std::function; + using OnVisitedVertexCallback = std::function; + // Callback used to check path length from start/finish to the edge (including the edge itself) + // before adding the edge to AStar queue. + // Can be used to clip some path which does not meet restrictions. + using CheckLengthCallback = std::function; class Context final { @@ -91,10 +96,10 @@ public: // VisitVertex returns true: wave will continue // VisitVertex returns false: wave will stop - template + template void PropagateWave(TGraphType & graph, TVertexType const & startVertex, VisitVertex && visitVertex, AdjustEdgeWeight && adjustEdgeWeight, - Context & context) const; + FilterStates && filterStates, Context & context) const; template void PropagateWave(TGraphType & graph, TVertexType const & startVertex, @@ -103,20 +108,23 @@ public: Result FindPath(TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, RoutingResult & result, my::Cancellable const & cancellable = my::Cancellable(), - TOnVisitedVertexCallback onVisitedVertexCallback = nullptr) const; + OnVisitedVertexCallback onVisitedVertexCallback = {}, + CheckLengthCallback checkLengthCallback = {}) const; - Result FindPathBidirectional(TGraphType & graph, - TVertexType const & startVertex, TVertexType const & finalVertex, + Result FindPathBidirectional(TGraphType & graph, TVertexType const & startVertex, + TVertexType const & finalVertex, RoutingResult & result, my::Cancellable const & cancellable = my::Cancellable(), - TOnVisitedVertexCallback onVisitedVertexCallback = nullptr) const; + OnVisitedVertexCallback onVisitedVertexCallback = {}, + CheckLengthCallback checkLengthCallback = {}) const; // Adjust route to the previous one. - // adjustLimit - distance limit for wave propagation, measured in same units as graph edges length. + // Expects |checkLengthCallback| to check wave propagation limit. typename AStarAlgorithm::Result AdjustRoute( TGraphType & graph, TVertexType const & startVertex, std::vector const & prevRoute, - TWeightType const & adjustLimit, RoutingResult & result, - my::Cancellable const & cancellable, TOnVisitedVertexCallback onVisitedVertexCallback) const; + RoutingResult & result, my::Cancellable const & cancellable, + OnVisitedVertexCallback onVisitedVertexCallback, + CheckLengthCallback checkLengthCallback) const; private: // Periodicity of switching a wave of bidirectional algorithm. @@ -241,10 +249,11 @@ template constexpr typename TGraph::TWeightType AStarAlgorithm::kZeroDistance; template -template +template void AStarAlgorithm::PropagateWave(TGraphType & graph, TVertexType const & startVertex, VisitVertex && visitVertex, AdjustEdgeWeight && adjustEdgeWeight, + FilterStates && filterStates, AStarAlgorithm::Context & context) const { context.Clear(); @@ -281,6 +290,10 @@ void AStarAlgorithm::PropagateWave(TGraphType & graph, TVertexType const continue; stateW.distance = newReducedDist; + + if (!filterStates(stateW)) + continue; + context.SetDistance(stateW.vertex, newReducedDist); context.SetParent(stateW.vertex, stateV.vertex); queue.push(stateW); @@ -297,7 +310,8 @@ void AStarAlgorithm::PropagateWave(TGraphType & graph, TVertexType const auto const adjustEdgeWeight = [](TVertexType const & /* vertex */, TEdgeType const & edge) { return edge.GetWeight(); }; - PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, context); + auto const filterStates = [](State const & /* state */) { return true; }; + PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, context); } // This implementation is based on the view that the A* algorithm @@ -315,16 +329,34 @@ template typename AStarAlgorithm::Result AStarAlgorithm::FindPath( TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, RoutingResult & result, my::Cancellable const & cancellable, - TOnVisitedVertexCallback onVisitedVertexCallback) const + OnVisitedVertexCallback onVisitedVertexCallback, CheckLengthCallback checkLengthCallback) const { result.Clear(); - if (nullptr == onVisitedVertexCallback) + if (onVisitedVertexCallback == nullptr) onVisitedVertexCallback = [](TVertexType const &, TVertexType const &) {}; + if (checkLengthCallback == nullptr) + checkLengthCallback = [](TWeightType const & /* weight */) { return true; }; + Context context; PeriodicPollCancellable periodicCancellable(cancellable); Result resultCode = Result::NoPath; + auto const heuristicDiff = [&](TVertexType const & vertexFrom, TVertexType const & vertexTo) { + return graph.HeuristicCostEstimate(vertexFrom, finalVertex) - + graph.HeuristicCostEstimate(vertexTo, finalVertex); + }; + + auto const fullToReducedLength = [&](TVertexType const & vertexFrom, TVertexType const & vertexTo, + TWeightType const length) { + return length - heuristicDiff(vertexFrom, vertexTo); + }; + + auto const reducedToFullLength = [&](TVertexType const & vertexFrom, TVertexType const & vertexTo, + TWeightType const reducedLength) { + return reducedLength + heuristicDiff(vertexFrom, vertexTo); + }; + auto visitVertex = [&](TVertexType const & vertex) { if (periodicCancellable.IsCancelled()) { @@ -343,24 +375,28 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( return true; }; - auto adjustEdgeWeight = [&](TVertexType const & vertex, TEdgeType const & edge) { - auto const len = edge.GetWeight(); - auto const piV = graph.HeuristicCostEstimate(vertex, finalVertex); - auto const piW = graph.HeuristicCostEstimate(edge.GetTarget(), finalVertex); - auto const reducedLen = len + piW - piV; + auto const adjustEdgeWeight = [&](TVertexType const & vertexV, TEdgeType const & edge) { + auto const reducedWeight = fullToReducedLength(vertexV, edge.GetTarget(), edge.GetWeight()); - CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon)); - return std::max(reducedLen, kZeroDistance); + CHECK_GREATER_OR_EQUAL(reducedWeight, -kEpsilon, ("Invariant violated.")); + + return std::max(reducedWeight, kZeroDistance); }; - PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, context); + auto const filterStates = [&](State const & state) { + return checkLengthCallback(reducedToFullLength(startVertex, state.vertex, state.distance)); + }; + + PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, context); if (resultCode == Result::OK) { context.ReconstructPath(finalVertex, result.m_path); - result.m_distance = context.GetDistance(finalVertex) - - graph.HeuristicCostEstimate(finalVertex, finalVertex) + - graph.HeuristicCostEstimate(startVertex, finalVertex); + result.m_distance = + reducedToFullLength(startVertex, finalVertex, context.GetDistance(finalVertex)); + + if (!checkLengthCallback(result.m_distance)) + resultCode = Result::NoPath; } return resultCode; @@ -368,15 +404,16 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPath( template typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirectional( - TGraphType & graph, - TVertexType const & startVertex, TVertexType const & finalVertex, - RoutingResult & result, - my::Cancellable const & cancellable, - TOnVisitedVertexCallback onVisitedVertexCallback) const + TGraphType & graph, TVertexType const & startVertex, TVertexType const & finalVertex, + RoutingResult & result, my::Cancellable const & cancellable, + OnVisitedVertexCallback onVisitedVertexCallback, CheckLengthCallback checkLengthCallback) const { - if (nullptr == onVisitedVertexCallback) + if (onVisitedVertexCallback == nullptr) onVisitedVertexCallback = [](TVertexType const &, TVertexType const &){}; + if (checkLengthCallback == nullptr) + checkLengthCallback = [](TWeightType const & /* weight */) { return true; }; + BidirectionalStepContext forward(true /* forward */, startVertex, finalVertex, graph); BidirectionalStepContext backward(false /* forward */, startVertex, finalVertex, graph); @@ -433,6 +470,9 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect if (curTop + nxtTop >= bestPathReducedLength - kEpsilon) { + if (!checkLengthCallback(bestPathRealLength)) + return Result::NoPath; + ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent, result.m_path); result.m_distance = bestPathRealLength; @@ -458,13 +498,17 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect if (stateV.vertex == stateW.vertex) continue; - auto const len = edge.GetWeight(); + auto const weight = edge.GetWeight(); auto const pV = cur->ConsistentHeuristic(stateV.vertex); auto const pW = cur->ConsistentHeuristic(stateW.vertex); - auto const reducedLen = len + pW - pV; + auto const reducedWeight = weight + pW - pV; - CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon)); - auto const newReducedDist = stateV.distance + std::max(reducedLen, kZeroDistance); + CHECK_GREATER_OR_EQUAL(reducedWeight, -kEpsilon, ("Invariant violated.")); + auto const newReducedDist = stateV.distance + std::max(reducedWeight, kZeroDistance); + + auto const fullLength = weight + stateV.distance + cur->pS - pV; + if (!checkLengthCallback(fullLength)) + continue; auto const itCur = cur->bestDistance.find(stateW.vertex); if (itCur != cur->bestDistance.end() && newReducedDist >= itCur->second - kEpsilon) @@ -482,7 +526,7 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect { bestPathReducedLength = curPathReducedLength; - bestPathRealLength = stateV.distance + len + distW; + bestPathRealLength = stateV.distance + weight + distW; bestPathRealLength += cur->pS - pV; bestPathRealLength += nxt->pS - nxt->ConsistentHeuristic(stateW.vertex); @@ -505,11 +549,14 @@ typename AStarAlgorithm::Result AStarAlgorithm::FindPathBidirect template typename AStarAlgorithm::Result AStarAlgorithm::AdjustRoute( TGraphType & graph, TVertexType const & startVertex, std::vector const & prevRoute, - TWeightType const & adjustLimit, RoutingResult & result, - my::Cancellable const & cancellable, TOnVisitedVertexCallback onVisitedVertexCallback) const + RoutingResult & result, my::Cancellable const & cancellable, + OnVisitedVertexCallback onVisitedVertexCallback, CheckLengthCallback checkLengthCallback) const { CHECK(!prevRoute.empty(), ()); + CHECK(checkLengthCallback != nullptr, + ("CheckLengthCallback expected to be set to limit wave propagation.")); + result.Clear(); if (onVisitedVertexCallback == nullptr) onVisitedVertexCallback = [](TVertexType const &, TVertexType const &) {}; @@ -538,16 +585,12 @@ typename AStarAlgorithm::Result AStarAlgorithm::AdjustRoute( return false; } - auto const distance = context.GetDistance(vertex); - if (distance > adjustLimit) - return false; - onVisitedVertexCallback(startVertex, vertex); auto it = remainingDistances.find(vertex); if (it != remainingDistances.cend()) { - auto const fullDistance = distance + it->second; + auto const fullDistance = context.GetDistance(vertex) + it->second; if (fullDistance < minDistance) { minDistance = fullDistance; @@ -558,7 +601,15 @@ typename AStarAlgorithm::Result AStarAlgorithm::AdjustRoute( return true; }; - PropagateWave(graph, startVertex, visitVertex, context); + auto const adjustEdgeWeight = [](TVertexType const & /* vertex */, TEdgeType const & edge) { + return edge.GetWeight(); + }; + + auto const filterStates = [&](State const & state) { + return checkLengthCallback(state.distance); + }; + + PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, context); if (wasCancelled) return Result::Cancelled; diff --git a/routing/cross_mwm_router.cpp b/routing/cross_mwm_router.cpp index 8c012ac179..5fbfd9a5eb 100644 --- a/routing/cross_mwm_router.cpp +++ b/routing/cross_mwm_router.cpp @@ -18,7 +18,7 @@ IRouter::ResultCode CalculateRoute(BorderCross const & startPos, BorderCross con { using TAlgorithm = AStarAlgorithm; - TAlgorithm::TOnVisitedVertexCallback onVisitedVertex = + TAlgorithm::OnVisitedVertexCallback onVisitedVertex = [&delegate](BorderCross const & cross, BorderCross const & /* target */) { delegate.OnPointCheck(MercatorBounds::FromLatLon(cross.fromNode.point)); diff --git a/routing/fake_edges_container.hpp b/routing/fake_edges_container.hpp index f3a3eca24b..50d8e7a649 100644 --- a/routing/fake_edges_container.hpp +++ b/routing/fake_edges_container.hpp @@ -19,6 +19,7 @@ class FakeEdgesContainer final public: FakeEdgesContainer(IndexGraphStarter && starter) : m_finishId(starter.m_finishId) + , m_finishPassThroughAllowed(starter.m_finishPassThroughAllowed) , m_fake(std::move(starter.m_fake)) { } @@ -32,8 +33,10 @@ public: } private: - // finish segment id + // Finish segment id. uint32_t m_finishId; + // Finish segment is located in a pass-through/non-pass-through area. + bool m_finishPassThroughAllowed; FakeGraph m_fake; }; } // namespace routing diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp index a8b28bf852..03532cdbb1 100644 --- a/routing/index_graph.cpp +++ b/routing/index_graph.cpp @@ -112,15 +112,13 @@ void IndexGraph::GetIngoingEdgesList(Segment const & segment, vectorCalcHeuristic(GetPoint(from, true /* front */), GetPoint(to, true /* front */)), - 0 /* nonPassThroughCross */); + m_estimator->CalcHeuristic(GetPoint(from, true /* front */), GetPoint(to, true /* front */))); } RouteWeight IndexGraph::CalcSegmentWeight(Segment const & segment) { return RouteWeight( - m_estimator->CalcSegmentWeight(segment, m_geometry.GetRoad(segment.GetFeatureId())), - 0 /* nonPassThroughCross */); + m_estimator->CalcSegmentWeight(segment, m_geometry.GetRoad(segment.GetFeatureId()))); } void IndexGraph::GetNeighboringEdges(Segment const & from, RoadPoint const & rp, bool isOutgoing, @@ -178,8 +176,8 @@ RouteWeight IndexGraph::GetPenalties(Segment const & u, Segment const & v) uint32_t const passThroughPenalty = fromPassThroughAllowed == toPassThroughAllowed ? 0 : 1; if (IsUTurn(u, v)) - return RouteWeight(m_estimator->GetUTurnPenalty(), passThroughPenalty); + return RouteWeight(m_estimator->GetUTurnPenalty(), passThroughPenalty, 0.0 /* transitTime */); - return RouteWeight(0.0, passThroughPenalty); + return RouteWeight(0.0, passThroughPenalty, 0.0 /* transitTime */); } } // namespace routing diff --git a/routing/index_graph_starter.cpp b/routing/index_graph_starter.cpp index 576298d057..e27b63306d 100644 --- a/routing/index_graph_starter.cpp +++ b/routing/index_graph_starter.cpp @@ -41,16 +41,25 @@ IndexGraphStarter::IndexGraphStarter(FakeEnding const & startEnding, FakeEnding const & finishEnding, uint32_t fakeNumerationStart, bool strictForward, WorldGraph & graph) : m_graph(graph) + , m_startId(fakeNumerationStart) + , m_startPassThroughAllowed(EndingPassThroughAllowed(startEnding)) + , m_finishPassThroughAllowed(EndingPassThroughAllowed(finishEnding)) { - m_startId = fakeNumerationStart; AddStart(startEnding, finishEnding, strictForward, fakeNumerationStart); m_finishId = fakeNumerationStart; AddFinish(finishEnding, startEnding, fakeNumerationStart); + auto const startPoint = GetPoint(GetStartSegment(), false /* front */); + auto const finishPoint = GetPoint(GetFinishSegment(), true /* front */); + m_startToFinishDistanceM = MercatorBounds::DistanceOnEarth(startPoint, finishPoint); } void IndexGraphStarter::Append(FakeEdgesContainer const & container) { m_finishId = container.m_finishId; + m_finishPassThroughAllowed = container.m_finishPassThroughAllowed; + auto const startPoint = GetPoint(GetStartSegment(), false /* front */); + auto const finishPoint = GetPoint(GetFinishSegment(), true /* front */); + m_startToFinishDistanceM = MercatorBounds::DistanceOnEarth(startPoint, finishPoint); m_fake.Append(container.m_fake); } @@ -109,37 +118,15 @@ set IndexGraphStarter::GetMwms() const return mwms; } -bool IndexGraphStarter::DoesRouteCrossNonPassThrough( - RoutingResult const & result) const +bool IndexGraphStarter::CheckLength(RouteWeight const & weight) const { - int nonPassThroughCrossAllowed = 0; - auto isRealOrPart = [this](Segment const & segment) { - if (!IsFakeSegment(segment)) - return true; - Segment dummy; - return m_fake.FindReal(segment, dummy); - }; - auto isPassThroughAllowed = [this](Segment const & segment) { - auto real = segment; - bool const convertionResult = ConvertToReal(real); - CHECK(convertionResult, ()); - return m_graph.IsPassThroughAllowed(real.GetMwmId(), real.GetFeatureId()); - }; + // We allow 1 pass-through/non-pass-through crossing per ending located in + // non-pass-through zone to allow user to leave this zone. + int const nonPassThroughCrossAllowed = + (m_startPassThroughAllowed ? 0 : 1) + (m_finishPassThroughAllowed ? 0 : 1); - auto const firstRealOrPart = find_if(result.m_path.begin(), result.m_path.end(), isRealOrPart); - if (firstRealOrPart != result.m_path.end() && !isPassThroughAllowed(*firstRealOrPart)) - ++nonPassThroughCrossAllowed; - - auto const lastRealOrPart = find_if(result.m_path.rbegin(), result.m_path.rend(), isRealOrPart); - // If firstRealOrPart and lastRealOrPart point to the same segment increment - // nonPassThroughCrossAllowed once - if (lastRealOrPart != result.m_path.rend() && &*lastRealOrPart != &*firstRealOrPart && - !isPassThroughAllowed(*lastRealOrPart)) - { - ++nonPassThroughCrossAllowed; - } - - return nonPassThroughCrossAllowed < result.m_distance.GetNonPassThroughCross(); + return weight.GetNonPassThroughCross() <= nonPassThroughCrossAllowed && + m_graph.CheckLength(weight, m_startToFinishDistanceM); } void IndexGraphStarter::GetEdgesList(Segment const & segment, bool isOutgoing, @@ -321,4 +308,13 @@ void IndexGraphStarter::AddRealEdges(Segment const & segment, bool isOutgoing, bool const isEnding = !m_fake.GetFake(segment).empty(); m_graph.GetEdgeList(segment, isOutgoing, IsLeap(segment.GetMwmId()), isEnding, edges); } + +bool IndexGraphStarter::EndingPassThroughAllowed(FakeEnding const & ending) +{ + return any_of(ending.m_projections.cbegin(), ending.m_projections.cend(), + [this](Projection const & projection) { + return m_graph.IsPassThroughAllowed(projection.m_segment.GetMwmId(), + projection.m_segment.GetFeatureId()); + }); +} } // namespace routing diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp index e4cf456cf1..d874eca965 100644 --- a/routing/index_graph_starter.hpp +++ b/routing/index_graph_starter.hpp @@ -74,10 +74,9 @@ public: std::set GetMwms() const; - // Checks |result| meets non-pass-through crossing restrictions according to placement of - // |result.path| start and finish in pass through/non-pass-through area and number of - // non-pass-through crosses. - bool DoesRouteCrossNonPassThrough(RoutingResult const & result) const; + // Checks whether |weight| meets non-pass-through crossing restrictions according to placement of + // start and finish in pass-through/non-pass-through area and number of non-pass-through crosses. + bool CheckLength(RouteWeight const & weight) const; void GetEdgesList(Segment const & segment, bool isOutgoing, std::vector & edges) const; @@ -131,12 +130,20 @@ private: void AddRealEdges(Segment const & segment, bool isOutgoing, std::vector & edges) const; + // Checks whether ending belongs to pass-through or non-pass-through zone. + bool EndingPassThroughAllowed(FakeEnding const & ending); + static uint32_t constexpr kFakeFeatureId = FakeFeatureIds::kIndexGraphStarterId; WorldGraph & m_graph; // Start segment id uint32_t m_startId; // Finish segment id uint32_t m_finishId; + // Start segment is located in a pass-through/non-pass-through area. + bool m_startPassThroughAllowed; + // Finish segment is located in a pass-through/non-pass-through area. + bool m_finishPassThroughAllowed; + double m_startToFinishDistanceM; FakeGraph m_fake; }; } // namespace routing diff --git a/routing/index_router.cpp b/routing/index_router.cpp index 47a50d7c7a..358c48c94c 100644 --- a/routing/index_router.cpp +++ b/routing/index_router.cpp @@ -138,12 +138,13 @@ template IRouter::ResultCode FindPath( typename Graph::TVertexType const & start, typename Graph::TVertexType const & finish, RouterDelegate const & delegate, Graph & graph, - typename AStarAlgorithm::TOnVisitedVertexCallback const & onVisitedVertexCallback, + typename AStarAlgorithm::OnVisitedVertexCallback const & onVisitedVertexCallback, + typename AStarAlgorithm::CheckLengthCallback const & checkLengthCallback, RoutingResult & routingResult) { AStarAlgorithm algorithm; - return ConvertResult(algorithm.FindPathBidirectional(graph, start, finish, routingResult, - delegate, onVisitedVertexCallback)); + return ConvertResult(algorithm.FindPathBidirectional( + graph, start, finish, routingResult, delegate, onVisitedVertexCallback, checkLengthCallback)); } bool IsDeadEnd(Segment const & segment, bool isOutgoing, WorldGraph & worldGraph) @@ -561,14 +562,13 @@ IRouter::ResultCode IndexRouter::CalculateSubroute(Checkpoints const & checkpoin }; RoutingResult routingResult; - IRouter::ResultCode const result = FindPath(starter.GetStartSegment(), starter.GetFinishSegment(), - delegate, starter, onVisitJunction, routingResult); + IRouter::ResultCode const result = FindPath( + starter.GetStartSegment(), starter.GetFinishSegment(), delegate, starter, onVisitJunction, + [&starter](RouteWeight const & weight) { return starter.CheckLength(weight); }, + routingResult); if (result != IRouter::NoError) return result; - if (starter.DoesRouteCrossNonPassThrough(routingResult)) - return IRouter::RouteNotFound; - IRouter::ResultCode const leapsResult = ProcessLeaps(routingResult.m_path, delegate, starter.GetGraph().GetMode(), starter, subroute); if (leapsResult != IRouter::NoError) @@ -635,12 +635,15 @@ IRouter::ResultCode IndexRouter::AdjustRoute(Checkpoints const & checkpoints, delegate.OnPointCheck(point); }; + auto const checkLength = [&starter](RouteWeight const & weight) { + return weight <= RouteWeight(kAdjustLimitSec) && starter.CheckLength(weight); + }; + AStarAlgorithm algorithm; RoutingResult result; auto resultCode = ConvertResult( - algorithm.AdjustRoute(starter, starter.GetStartSegment(), prevEdges, - RouteWeight(kAdjustLimitSec, 0 /* nonPassThroughCross */), result, - delegate, onVisitJunction)); + algorithm.AdjustRoute(starter, starter.GetStartSegment(), prevEdges, result, delegate, + onVisitJunction, checkLength)); if (resultCode != IRouter::NoError) return resultCode; @@ -784,14 +787,18 @@ IRouter::ResultCode IndexRouter::ProcessLeaps(vector const & input, { // World graph route. worldGraph.SetMode(WorldGraph::Mode::NoLeaps); - result = FindPath(current, next, delegate, worldGraph, {} /* onVisitedVertexCallback */, routingResult); } else { // Single mwm route. worldGraph.SetMode(WorldGraph::Mode::SingleMwm); - result = FindPath(current, next, delegate, worldGraph, {} /* onVisitedVertexCallback */, routingResult); } + + result = + FindPath(current, next, delegate, worldGraph, {} /* onVisitedVertexCallback */, + [&starter](RouteWeight const & weight) { return starter.CheckLength(weight); }, + routingResult); + if (result != IRouter::NoError) return result; diff --git a/routing/route_weight.cpp b/routing/route_weight.cpp index 0fa67dd15a..164a525cad 100644 --- a/routing/route_weight.cpp +++ b/routing/route_weight.cpp @@ -15,12 +15,14 @@ double RouteWeight::ToCrossMwmWeight() const ostream & operator<<(ostream & os, RouteWeight const & routeWeight) { - os << "(" << routeWeight.GetNonPassThroughCross() << ", " << routeWeight.GetWeight() << ")"; + os << "(" << routeWeight.GetNonPassThroughCross() << ", " << routeWeight.GetWeight() << ", " + << routeWeight.GetTransitTime() << ")"; return os; } RouteWeight operator*(double lhs, RouteWeight const & rhs) { - return RouteWeight(lhs * rhs.GetWeight(), rhs.GetNonPassThroughCross()); + return RouteWeight(lhs * rhs.GetWeight(), rhs.GetNonPassThroughCross(), + lhs * rhs.GetTransitTime()); } } // namespace routing diff --git a/routing/route_weight.hpp b/routing/route_weight.hpp index f4c823d7de..f36f2e66cc 100644 --- a/routing/route_weight.hpp +++ b/routing/route_weight.hpp @@ -12,25 +12,28 @@ class RouteWeight final public: RouteWeight() = default; - constexpr RouteWeight(double weight, int nonPassThroughCross) - : m_weight(weight), m_nonPassThroughCross(nonPassThroughCross) + explicit constexpr RouteWeight(double weight) : m_weight(weight) {} + + constexpr RouteWeight(double weight, int nonPassThroughCross, double transitTime) + : m_weight(weight), m_nonPassThroughCross(nonPassThroughCross), m_transitTime(transitTime) { } - static RouteWeight FromCrossMwmWeight(double weight) - { - return RouteWeight(weight, 0 /* nonPassThroughCross */); - } + static RouteWeight FromCrossMwmWeight(double weight) { return RouteWeight(weight); } double ToCrossMwmWeight() const; double GetWeight() const { return m_weight; } int GetNonPassThroughCross() const { return m_nonPassThroughCross; } + double GetTransitTime() const { return m_transitTime; } bool operator<(RouteWeight const & rhs) const { if (m_nonPassThroughCross != rhs.m_nonPassThroughCross) return m_nonPassThroughCross < rhs.m_nonPassThroughCross; - return m_weight < rhs.m_weight; + if (m_weight != rhs.m_weight) + return m_weight < rhs.m_weight; + // Preffer bigger transit time if total weights are same. + return m_transitTime > rhs.m_transitTime; } bool operator==(RouteWeight const & rhs) const { return !((*this) < rhs) && !(rhs < (*this)); } @@ -41,30 +44,40 @@ public: bool operator>=(RouteWeight const & rhs) const { return !((*this) < rhs); } + bool operator<=(RouteWeight const & rhs) const { return rhs >= (*this); } + RouteWeight operator+(RouteWeight const & rhs) const { - return RouteWeight(m_weight + rhs.m_weight, m_nonPassThroughCross + rhs.m_nonPassThroughCross); + return RouteWeight(m_weight + rhs.m_weight, m_nonPassThroughCross + rhs.m_nonPassThroughCross, + m_transitTime + rhs.m_transitTime); } RouteWeight operator-(RouteWeight const & rhs) const { - return RouteWeight(m_weight - rhs.m_weight, m_nonPassThroughCross - rhs.m_nonPassThroughCross); + return RouteWeight(m_weight - rhs.m_weight, m_nonPassThroughCross - rhs.m_nonPassThroughCross, + m_transitTime - rhs.m_transitTime); } RouteWeight & operator+=(RouteWeight const & rhs) { m_weight += rhs.m_weight; m_nonPassThroughCross += rhs.m_nonPassThroughCross; + m_transitTime += rhs.m_transitTime; return *this; } - RouteWeight operator-() const { return RouteWeight(-m_weight, -m_nonPassThroughCross); } + RouteWeight operator-() const + { + return RouteWeight(-m_weight, -m_nonPassThroughCross, -m_transitTime); + } private: // Regular weight (seconds). double m_weight = 0.0; // Number of pass-through/non-pass-through area border cross. int m_nonPassThroughCross = 0; + // Transit time. It's already included in |m_weight| (m_transitTime <= m_weight). + double m_transitTime = 0.0; }; std::ostream & operator<<(std::ostream & os, RouteWeight const & routeWeight); @@ -75,19 +88,21 @@ template <> constexpr RouteWeight GetAStarWeightMax() { return RouteWeight(std::numeric_limits::max() /* weight */, - std::numeric_limits::max() /* nonPassThroughCross */); + std::numeric_limits::max() /* nonPassThroughCross */, + std::numeric_limits::max() /* transitTime */); } template <> constexpr RouteWeight GetAStarWeightZero() { - return RouteWeight(0.0 /* weight */, 0 /* nonPassThroughCross */); + return RouteWeight(0.0 /* weight */, 0 /* nonPassThroughCross */, 0.0 /* transitTime */); } template <> constexpr RouteWeight GetAStarWeightEpsilon() { - return RouteWeight(GetAStarWeightEpsilon(), 0 /* nonPassThroughCross */); + return RouteWeight(GetAStarWeightEpsilon(), 0 /* nonPassThroughCross */, + 0.0 /* transitTime */); } } // namespace routing diff --git a/routing/routing_tests/astar_algorithm_test.cpp b/routing/routing_tests/astar_algorithm_test.cpp index 5dcb3035ee..99bace87e1 100644 --- a/routing/routing_tests/astar_algorithm_test.cpp +++ b/routing/routing_tests/astar_algorithm_test.cpp @@ -93,6 +93,33 @@ UNIT_TEST(AStarAlgorithm_Sample) TestAStar(graph, expectedRoute, 23); } +UNIT_TEST(AStarAlgorithm_CheckLength) +{ + UndirectedGraph graph; + + // Inserts edges in a format: . + graph.AddEdge(0, 1, 10); + graph.AddEdge(1, 2, 5); + graph.AddEdge(2, 3, 5); + graph.AddEdge(2, 4, 10); + graph.AddEdge(3, 4, 3); + + TAlgorithm algo; + RoutingResult routingResult; + TAlgorithm::Result result = + algo.FindPath(graph, 0u, 4u, routingResult, {} /* cancellable */, + {} /* onVisitedVertexCallback */, [](double weight) { return weight < 23; }); + // Best route weight is 23 so we expect to find no route with restriction |weight < 23|. + TEST_EQUAL(result, TAlgorithm::Result::NoPath, ()); + + routingResult = {}; + result = algo.FindPathBidirectional(graph, 0u, 4u, routingResult, {} /* cancellable */, + {} /* onVisitedVertexCallback */, + [](double weight) { return weight < 23; }); + // Best route weight is 23 so we expect to find no route with restriction |weight < 23|. + TEST_EQUAL(result, TAlgorithm::Result::NoPath, ()); +} + UNIT_TEST(AdjustRoute) { UndirectedGraph graph; @@ -109,8 +136,9 @@ UNIT_TEST(AdjustRoute) TAlgorithm algo; RoutingResult result; - auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, 1.0 /* limit */, result, - my::Cancellable(), nullptr /* onVisitedVertexCallback */); + auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, result, + my::Cancellable(), nullptr /* onVisitedVertexCallback */, + [](double weight){ return weight <= 1.0; }); vector const expectedRoute = {6, 2, 3, 4, 5}; TEST_EQUAL(code, TAlgorithm::Result::OK, ()); @@ -130,8 +158,9 @@ UNIT_TEST(AdjustRouteNoPath) TAlgorithm algo; RoutingResult result; - auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, 1.0 /* limit */, result, - my::Cancellable(), nullptr /* onVisitedVertexCallback */); + auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, result, + my::Cancellable(), nullptr /* onVisitedVertexCallback */, + [](double weight){ return weight <= 1.0; }); TEST_EQUAL(code, TAlgorithm::Result::NoPath, ()); TEST(result.m_path.empty(), ()); @@ -151,8 +180,9 @@ UNIT_TEST(AdjustRouteOutOfLimit) TAlgorithm algo; RoutingResult result; - auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, 1.0 /* limit */, result, - my::Cancellable(), nullptr /* onVisitedVertexCallback */); + auto code = algo.AdjustRoute(graph, 6 /* start */, prevRoute, result, + my::Cancellable(), nullptr /* onVisitedVertexCallback */, + [](double weight){ return weight <= 1.0; }); TEST_EQUAL(code, TAlgorithm::Result::NoPath, ()); TEST(result.m_path.empty(), ()); diff --git a/routing/routing_tests/index_graph_tools.cpp b/routing/routing_tests/index_graph_tools.cpp index 3a2984040e..8e3fe944a8 100644 --- a/routing/routing_tests/index_graph_tools.cpp +++ b/routing/routing_tests/index_graph_tools.cpp @@ -353,10 +353,8 @@ AStarAlgorithm::Result CalculateRoute(IndexGraphStarter & sta auto resultCode = algorithm.FindPathBidirectional( starter, starter.GetStartSegment(), starter.GetFinishSegment(), routingResult, - {} /* cancellable */, {} /* onVisitedVertexCallback */); - - if (starter.DoesRouteCrossNonPassThrough(routingResult)) - resultCode = AStarAlgorithm::Result::NoPath; + {} /* cancellable */, {} /* onVisitedVertexCallback */, + [&](RouteWeight const & weight) { return starter.CheckLength(weight); }); timeSec = routingResult.m_distance.GetWeight(); roadPoints = routingResult.m_path; diff --git a/routing/single_vehicle_world_graph.cpp b/routing/single_vehicle_world_graph.cpp index 7612fedbd0..15e651c8fe 100644 --- a/routing/single_vehicle_world_graph.cpp +++ b/routing/single_vehicle_world_graph.cpp @@ -32,10 +32,8 @@ void SingleVehicleWorldGraph::GetEdgeList(Segment const & segment, bool isOutgoi // point. So |isEnter| below should be set to true. m_crossMwmGraph->ForEachTransition( segment.GetMwmId(), !isOutgoing /* isEnter */, [&](Segment const & transition) { - edges.emplace_back( - transition, RouteWeight(m_estimator->CalcLeapWeight(segmentPoint, - GetPoint(transition, isOutgoing)), - 0 /* nonPassThroughCross */)); + edges.emplace_back(transition, RouteWeight(m_estimator->CalcLeapWeight( + segmentPoint, GetPoint(transition, isOutgoing)))); }); return; } @@ -100,20 +98,19 @@ RouteWeight SingleVehicleWorldGraph::HeuristicCostEstimate(Segment const & from, RouteWeight SingleVehicleWorldGraph::HeuristicCostEstimate(m2::PointD const & from, m2::PointD const & to) { - return RouteWeight(m_estimator->CalcHeuristic(from, to), 0 /* nonPassThroughCross */); + return RouteWeight(m_estimator->CalcHeuristic(from, to)); } RouteWeight SingleVehicleWorldGraph::CalcSegmentWeight(Segment const & segment) { return RouteWeight(m_estimator->CalcSegmentWeight( - segment, GetRoadGeometry(segment.GetMwmId(), segment.GetFeatureId())), - 0 /* nonPassThroughCross */); + segment, GetRoadGeometry(segment.GetMwmId(), segment.GetFeatureId()))); } RouteWeight SingleVehicleWorldGraph::CalcOffroadWeight(m2::PointD const & from, m2::PointD const & to) const { - return RouteWeight(m_estimator->CalcOffroadWeight(from, to), 0 /* nonPassThroughCross */); + return RouteWeight(m_estimator->CalcOffroadWeight(from, to)); } bool SingleVehicleWorldGraph::LeapIsAllowed(NumMwmId mwmId) const @@ -141,7 +138,7 @@ void SingleVehicleWorldGraph::GetTwins(Segment const & segment, bool isOutgoing, // in different mwms. But if we have mwms with different versions and feature // was moved in one of them we can have nonzero weight here. double const weight = m_estimator->CalcHeuristic(from, to); - edges.emplace_back(twin, RouteWeight(weight, 0 /* nonPassThroughCross */)); + edges.emplace_back(twin, RouteWeight(weight)); } } } // namespace routing diff --git a/routing/single_vehicle_world_graph.hpp b/routing/single_vehicle_world_graph.hpp index ff66ba9bc3..ab5365cba8 100644 --- a/routing/single_vehicle_world_graph.hpp +++ b/routing/single_vehicle_world_graph.hpp @@ -29,6 +29,7 @@ public: // WorldGraph overrides: void GetEdgeList(Segment const & segment, bool isOutgoing, bool isLeap, bool isEnding, std::vector & edges) override; + bool CheckLength(RouteWeight const &, double) const override { return true; } Junction const & GetJunction(Segment const & segment, bool front) override; m2::PointD const & GetPoint(Segment const & segment, bool front) override; bool IsOneWay(NumMwmId mwmId, uint32_t featureId) override; diff --git a/routing/transit_graph.cpp b/routing/transit_graph.cpp index 5e3a370c26..c6a3d6992c 100644 --- a/routing/transit_graph.cpp +++ b/routing/transit_graph.cpp @@ -54,34 +54,39 @@ RouteWeight TransitGraph::CalcSegmentWeight(Segment const & segment) const { CHECK(IsTransitSegment(segment), ("Nontransit segment passed to TransitGraph.")); if (IsGate(segment)) - return RouteWeight(GetGate(segment).GetWeight(), 0 /* nontransitCross */); + { + auto const weight = GetGate(segment).GetWeight(); + return RouteWeight(weight /* weight */, 0 /* nonPassThrougCross */, weight /* transitTime */); + } if (IsEdge(segment)) - return RouteWeight(GetEdge(segment).GetWeight(), 0 /* nontransitCross */); + { + auto const weight = GetEdge(segment).GetWeight(); + return RouteWeight(weight /* weight */, 0 /* nonPassThrougCross */, weight /* transitTime */); + } return RouteWeight( m_estimator->CalcOffroadWeight(GetJunction(segment, false /* front */).GetPoint(), - GetJunction(segment, true /* front */).GetPoint()), - 0 /* nontransitCross */); + GetJunction(segment, true /* front */).GetPoint())); } RouteWeight TransitGraph::GetTransferPenalty(Segment const & from, Segment const & to) const { // We need to wait transport and apply additional penalty only if we change to transit::Edge. if (!IsEdge(to)) - return RouteWeight(0 /* weight */, 0 /* nontransitCross */); + return GetAStarWeightZero(); auto const & edgeTo = GetEdge(to); // We are changing to transfer and do not need to apply extra penalty here. We'll do it while // changing from transfer. if (edgeTo.GetTransfer()) - return RouteWeight(0 /* weight */, 0 /* nontransitCross */); + return GetAStarWeightZero(); auto const lineIdTo = edgeTo.GetLineId(); if (IsEdge(from) && GetEdge(from).GetLineId() == lineIdTo) - return RouteWeight(0 /* weight */, 0 /* nontransitCross */); + return GetAStarWeightZero(); // We need to apply extra penalty when: // 1. |from| is gate, |to| is edge @@ -89,7 +94,8 @@ RouteWeight TransitGraph::GetTransferPenalty(Segment const & from, Segment const // 3. |from| is edge, |to| is edge from another line directly connected to |from|. auto const it = m_transferPenalties.find(lineIdTo); CHECK(it != m_transferPenalties.cend(), ("Segment", to, "belongs to unknown line:", lineIdTo)); - return RouteWeight(it->second, 0 /* nontransitCross */); + return RouteWeight(it->second /* weight */, 0 /* nonPassThrougCross */, + it->second /* transitTime */); } void TransitGraph::GetTransitEdges(Segment const & segment, bool isOutgoing, diff --git a/routing/transit_world_graph.cpp b/routing/transit_world_graph.cpp index 8d12e6d36b..f6a4622d43 100644 --- a/routing/transit_world_graph.cpp +++ b/routing/transit_world_graph.cpp @@ -117,7 +117,7 @@ RouteWeight TransitWorldGraph::HeuristicCostEstimate(Segment const & from, Segme RouteWeight TransitWorldGraph::HeuristicCostEstimate(m2::PointD const & from, m2::PointD const & to) { - return RouteWeight(m_estimator->CalcHeuristic(from, to), 0 /* nontransitCross */); + return RouteWeight(m_estimator->CalcHeuristic(from, to)); } RouteWeight TransitWorldGraph::CalcSegmentWeight(Segment const & segment) @@ -129,14 +129,13 @@ RouteWeight TransitWorldGraph::CalcSegmentWeight(Segment const & segment) } return RouteWeight(m_estimator->CalcSegmentWeight( - segment, GetRealRoadGeometry(segment.GetMwmId(), segment.GetFeatureId())), - 0 /* nontransitCross */); + segment, GetRealRoadGeometry(segment.GetMwmId(), segment.GetFeatureId()))); } RouteWeight TransitWorldGraph::CalcOffroadWeight(m2::PointD const & from, m2::PointD const & to) const { - return RouteWeight(m_estimator->CalcOffroadWeight(from, to), 0 /* nontransitCross */); + return RouteWeight(m_estimator->CalcOffroadWeight(from, to)); } bool TransitWorldGraph::LeapIsAllowed(NumMwmId /* mwmId */) const { return false; } @@ -172,7 +171,7 @@ void TransitWorldGraph::GetTwins(Segment const & segment, bool isOutgoing, // in different mwms. But if we have mwms with different versions and feature // was moved in one of them we can have nonzero weight here. double const weight = m_estimator->CalcHeuristic(from, to); - edges.emplace_back(twin, RouteWeight(weight, 0 /* nontransitCross */)); + edges.emplace_back(twin, RouteWeight(weight)); } } diff --git a/routing/transit_world_graph.hpp b/routing/transit_world_graph.hpp index dd87255780..1cfd88a4cb 100644 --- a/routing/transit_world_graph.hpp +++ b/routing/transit_world_graph.hpp @@ -32,6 +32,11 @@ public: // WorldGraph overrides: void GetEdgeList(Segment const & segment, bool isOutgoing, bool isLeap, bool isEnding, std::vector & edges) override; + bool CheckLength(RouteWeight const & weight, double startToFinishDistanceM) const override + { + return weight.GetWeight() - weight.GetTransitTime() <= + MaxPedestrianTimeSec(startToFinishDistanceM); + } Junction const & GetJunction(Segment const & segment, bool front) override; m2::PointD const & GetPoint(Segment const & segment, bool front) override; // All transit features are oneway. @@ -51,6 +56,13 @@ public: std::unique_ptr GetTransitInfo(Segment const & segment) override; private: + static double MaxPedestrianTimeSec(double startToFinishDistanceM) + { + // @todo(tatiana-kondakova) test and adjust constants. + // 15 min + 2 additional minutes per 1 km for now. + return 15 * 60 + (startToFinishDistanceM / 1000) * 2 * 60; + } + RoadGeometry const & GetRealRoadGeometry(NumMwmId mwmId, uint32_t featureId); void AddRealEdges(Segment const & segment, bool isOutgoing, vector & edges); void GetTwins(Segment const & s, bool isOutgoing, std::vector & edges); diff --git a/routing/world_graph.hpp b/routing/world_graph.hpp index 2535de541f..c6959f6a56 100644 --- a/routing/world_graph.hpp +++ b/routing/world_graph.hpp @@ -46,11 +46,15 @@ public: virtual void GetEdgeList(Segment const & segment, bool isOutgoing, bool isLeap, bool isEnding, std::vector & edges) = 0; + // Checks whether path length meets restrictions. Restrictions may depend on the distance from + // start to finish of the route. + virtual bool CheckLength(RouteWeight const & weight, double startToFinishDistanceM) const = 0; + virtual Junction const & GetJunction(Segment const & segment, bool front) = 0; virtual m2::PointD const & GetPoint(Segment const & segment, bool front) = 0; virtual bool IsOneWay(NumMwmId mwmId, uint32_t featureId) = 0; - // Checks feature is allowed for through passage. + // Checks whether feature is allowed for through passage. virtual bool IsPassThroughAllowed(NumMwmId mwmId, uint32_t featureId) = 0; // Clear memory used by loaded graphs. diff --git a/track_analyzing/track_matcher.cpp b/track_analyzing/track_matcher.cpp index 4232ac6fa4..78ed6f5a93 100644 --- a/track_analyzing/track_matcher.cpp +++ b/track_analyzing/track_matcher.cpp @@ -205,7 +205,7 @@ void TrackMatcher::Step::ChooseSegment(Step const & nextStep, IndexGraph & index vector edges; indexGraph.GetEdgeList(nextStep.m_segment, false /* isOutgoing */, edges); - edges.emplace_back(nextStep.m_segment, RouteWeight(0.0, 0)); + edges.emplace_back(nextStep.m_segment, GetAStarWeightZero()); for (Candidate const & candidate : m_candidates) {