diff options
author | Mikhail Gorbushin <m.gorbushin@corp.mail.ru> | 2019-04-04 19:01:53 +0300 |
---|---|---|
committer | Vladimir Byko-Ianko <bykoianko@gmail.com> | 2019-05-06 14:05:29 +0300 |
commit | 93eed5436a1c62c197c258b6bd44f92274b8d0df (patch) | |
tree | 2f9ecdc00781fb9920cbb0915543657c24eda142 /routing | |
parent | 4293e98971db0a682cec3306a9d715988c4a746d (diff) |
Stage 5. Using restriction in routing
Diffstat (limited to 'routing')
-rw-r--r-- | routing/base/astar_algorithm.hpp | 72 | ||||
-rw-r--r-- | routing/base/astar_graph.hpp | 19 | ||||
-rw-r--r-- | routing/index_graph.cpp | 119 | ||||
-rw-r--r-- | routing/index_graph.hpp | 129 | ||||
-rw-r--r-- | routing/index_graph_starter.hpp | 46 | ||||
-rw-r--r-- | routing/index_graph_starter_joints.hpp | 37 | ||||
-rw-r--r-- | routing/joint_segment.cpp | 4 | ||||
-rw-r--r-- | routing/joint_segment.hpp | 5 | ||||
-rw-r--r-- | routing/routing_tests/index_graph_tools.cpp | 38 | ||||
-rw-r--r-- | routing/routing_tests/index_graph_tools.hpp | 3 | ||||
-rw-r--r-- | routing/segment.hpp | 4 | ||||
-rw-r--r-- | routing/single_vehicle_world_graph.cpp | 155 | ||||
-rw-r--r-- | routing/single_vehicle_world_graph.hpp | 53 | ||||
-rw-r--r-- | routing/transit_world_graph.cpp | 3 | ||||
-rw-r--r-- | routing/transit_world_graph.hpp | 3 | ||||
-rw-r--r-- | routing/world_graph.cpp | 19 | ||||
-rw-r--r-- | routing/world_graph.hpp | 21 |
17 files changed, 549 insertions, 181 deletions
diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp index c1b8105a19..08e050fee1 100644 --- a/routing/base/astar_algorithm.hpp +++ b/routing/base/astar_algorithm.hpp @@ -137,7 +137,10 @@ public: m_distanceMap[vertex] = distance; } - void SetParent(Vertex const & parent, Vertex const & child) { m_parents[parent] = child; } + void SetParent(Vertex const & parent, Vertex const & child) + { + m_parents[parent] = child; + } bool HasParent(Vertex const & child) const { @@ -151,6 +154,8 @@ public: return it->second; } + std::map<Vertex, Vertex> & GetParents() { return m_parents; } + void ReconstructPath(Vertex const & v, std::vector<Vertex> & path) const; private: @@ -235,6 +240,7 @@ private: { bestVertex = forward ? startVertex : finalVertex; pS = ConsistentHeuristic(bestVertex); + graph.SetAStarParents(forward, parent); } Weight TopDistance() const @@ -272,6 +278,8 @@ private: graph.GetIngoingEdgesList(v, adj); } + std::map<Vertex, Vertex> & GetParents() { return parent; } + bool const forward; Vertex const & startVertex; Vertex const & finalVertex; @@ -304,11 +312,12 @@ constexpr Weight AStarAlgorithm<Vertex, Edge, Weight>::kZeroDistance; template <typename Vertex, typename Edge, typename Weight> template <typename VisitVertex, typename AdjustEdgeWeight, typename FilterStates> -void AStarAlgorithm<Vertex, Edge, Weight>::PropagateWave(Graph & graph, Vertex const & startVertex, - VisitVertex && visitVertex, - AdjustEdgeWeight && adjustEdgeWeight, - FilterStates && filterStates, - AStarAlgorithm<Vertex, Edge, Weight>::Context & context) const +void AStarAlgorithm<Vertex, Edge, Weight>::PropagateWave( + Graph & graph, Vertex const & startVertex, + VisitVertex && visitVertex, + AdjustEdgeWeight && adjustEdgeWeight, + FilterStates && filterStates, + AStarAlgorithm<Vertex, Edge, Weight>::Context & context) const { context.Clear(); @@ -468,6 +477,9 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, BidirectionalStepContext forward(true /* forward */, startVertex, finalVertex, graph); BidirectionalStepContext backward(false /* forward */, startVertex, finalVertex, graph); + auto & forwardParents = forward.GetParents(); + auto & backwardParents = backward.GetParents(); + bool foundAnyPath = false; auto bestPathReducedLength = kZeroDistance; auto bestPathRealLength = kZeroDistance; @@ -485,6 +497,20 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, BidirectionalStepContext * cur = &forward; BidirectionalStepContext * nxt = &backward; + auto const getResult = [&]() { + if (!params.m_checkLengthCallback(bestPathRealLength)) + return Result::NoPath; + + ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent, + result.m_path); + result.m_distance = bestPathRealLength; + CHECK(!result.m_path.empty(), ()); + if (!cur->forward) + reverse(result.m_path.begin(), result.m_path.end()); + + return Result::OK; + }; + std::vector<Edge> adj; // It is not necessary to check emptiness for both queues here @@ -520,18 +546,7 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, // different real path lengths. if (curTop + nxtTop >= bestPathReducedLength - kEpsilon) - { - if (!params.m_checkLengthCallback(bestPathRealLength)) - return Result::NoPath; - - ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent, - result.m_path); - result.m_distance = bestPathRealLength; - CHECK(!result.m_path.empty(), ()); - if (!cur->forward) - reverse(result.m_path.begin(), result.m_path.end()); - return Result::OK; - } + return getResult(); } State const stateV = cur->queue.top(); @@ -547,6 +562,7 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, for (auto const & edge : adj) { State stateW(edge.GetTarget(), kZeroDistance); + if (stateV.vertex == stateW.vertex) continue; @@ -568,6 +584,10 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, if (itCur != cur->bestDistance.end() && newReducedDist >= itCur->second - kEpsilon) continue; + stateW.distance = newReducedDist; + cur->bestDistance[stateW.vertex] = newReducedDist; + cur->parent[stateW.vertex] = stateV.vertex; + auto const itNxt = nxt->bestDistance.find(stateW.vertex); if (itNxt != nxt->bestDistance.end()) { @@ -576,7 +596,8 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, // find the reduced length of the path's parts in the reduced forward and backward graphs. auto const curPathReducedLength = newReducedDist + distW; // No epsilon here: it is ok to overshoot slightly. - if (!foundAnyPath || bestPathReducedLength > curPathReducedLength) + if ((!foundAnyPath || bestPathReducedLength > curPathReducedLength) && + graph.AreWavesConnectible(forwardParents, stateW.vertex, backwardParents)) { bestPathReducedLength = curPathReducedLength; @@ -590,13 +611,14 @@ AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectional(P & params, } } - stateW.distance = newReducedDist; - cur->bestDistance[stateW.vertex] = newReducedDist; - cur->parent[stateW.vertex] = stateV.vertex; - cur->queue.push(stateW); + if (stateW.vertex != (cur->forward ? cur->finalVertex : cur->startVertex)) + cur->queue.push(stateW); } } + if (foundAnyPath) + return getResult(); + return Result::NoPath; } @@ -632,6 +654,7 @@ typename AStarAlgorithm<Vertex, Edge, Weight>::Result } Context context; + graph.SetAStarParents(true /* forward */, context.GetParents()); PeriodicPollCancellable periodicCancellable(params.m_cancellable); auto visitVertex = [&](Vertex const & vertex) { @@ -714,7 +737,8 @@ void AStarAlgorithm<Vertex, Edge, Weight>::ReconstructPath(Vertex const & v, break; cur = it->second; } - reverse(path.begin(), path.end()); + + std::reverse(path.begin(), path.end()); } // static diff --git a/routing/base/astar_graph.hpp b/routing/base/astar_graph.hpp index 1647c96db7..cf38cfca82 100644 --- a/routing/base/astar_graph.hpp +++ b/routing/base/astar_graph.hpp @@ -1,5 +1,6 @@ #pragma once +#include <map> #include <vector> namespace routing @@ -13,11 +14,29 @@ public: using Edge = EdgeType; using Weight = WeightType; + using Parents = std::map<Vertex, Vertex>; + virtual Weight HeuristicCostEstimate(Vertex const & from, Vertex const & to) = 0; virtual void GetOutgoingEdgesList(Vertex const & v, std::vector<Edge> & edges) = 0; virtual void GetIngoingEdgesList(Vertex const & v, std::vector<Edge> & edges) = 0; + virtual void SetAStarParents(bool forward, Parents & parents); + virtual bool AreWavesConnectible(Parents & forwardParents, Vertex const & commonVertex, + Parents & backwardParents); + virtual ~AStarGraph() = default; }; + +template <typename VertexType, typename EdgeType, typename WeightType> +void AStarGraph<VertexType, EdgeType, WeightType>::SetAStarParents(bool /* forward */, + std::map<Vertex, Vertex> & /* parents */) {} + +template <typename VertexType, typename EdgeType, typename WeightType> +bool AStarGraph<VertexType, EdgeType, WeightType>::AreWavesConnectible(AStarGraph::Parents & /* forwardParents */, + Vertex const & /* commonVertex */, + AStarGraph::Parents & /* backwardParents */) +{ + return true; +} } // namespace routing diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp index 80fc84f54b..5a3b23cf02 100644 --- a/routing/index_graph.cpp +++ b/routing/index_graph.cpp @@ -1,5 +1,3 @@ -#include <utility> - #include "routing/index_graph.hpp" #include "routing/restrictions_serialization.hpp" @@ -15,47 +13,13 @@ #include <algorithm> #include <cstdlib> +#include <iterator> #include <limits> +#include <utility> -namespace -{ using namespace base; -using namespace routing; using namespace std; -/*bool IsRestricted(std::vector<std::vector<uint32_t>> const & restrictions, Segment const & u, Segment const & v, - bool isOutgoing) -{ - uint32_t const featureIdFrom = isOutgoing ? u.GetFeatureId() : v.GetFeatureId(); - uint32_t const featureIdTo = isOutgoing ? v.GetFeatureId() : u.GetFeatureId(); - - if (!binary_search(restrictions.cbegin(), restrictions.cend(), - std::vector<uint32_t>({featureIdFrom, featureIdTo}))) - { - return false; - } - - if (featureIdFrom != featureIdTo) - return true; - - // @TODO(bykoianko) According to current code if a feature id is marked as a feature with - // restrictricted U-turn it's restricted to make a U-turn on the both ends of the feature. - // Generally speaking it's wrong. In osm there's information about the end of the feature - // where the U-turn is restricted. It's necessary to pass the data to mwm and to use it here. - // Please see test LineGraph_RestrictionF1F1No for details. - // Another example when it's necessary to be aware about feature end participated in restriction - // is - // *---F1---* - // | | - // *--F3--A B--F4--* - // | | - // *---F2---* - // In case of restriction F1-A-F2 or F1-B-F2 of any type (No, Only) the important information - // is lost. - return IsUTurn(u, v); -}*/ -} // namespace - namespace routing { bool IsUTurn(Segment const & u, Segment const & v) @@ -68,7 +32,7 @@ std::map<Segment, Segment> IndexGraph::kEmptyParentsSegments = {}; IndexGraph::IndexGraph(shared_ptr<Geometry> geometry, shared_ptr<EdgeEstimator> estimator, RoutingOptions routingOptions) - : m_geometry(std::move(geometry)), + : m_geometry(move(geometry)), m_estimator(move(estimator)), m_avoidRoutingOptions(routingOptions) { @@ -82,7 +46,7 @@ bool IndexGraph::IsJoint(RoadPoint const & roadPoint) const } void IndexGraph::GetEdgeList(Segment const & segment, bool isOutgoing, vector<SegmentEdge> & edges, - std::map<Segment, Segment> & parents) + map<Segment, Segment> & parents) { RoadPoint const roadPoint = segment.GetRoadPoint(isOutgoing); Joint::Id const jointId = m_roadIndex.GetJointId(roadPoint); @@ -142,7 +106,7 @@ void IndexGraph::GetLastPointsForJoint(vector<Segment> const & children, void IndexGraph::GetEdgeList(JointSegment const & parentJoint, Segment const & parent, bool isOutgoing, vector<JointEdge> & edges, - vector<RouteWeight> & parentWeights, std::map<JointSegment, JointSegment> & parents) + vector<RouteWeight> & parentWeights, map<JointSegment, JointSegment> & parents) { vector<Segment> possibleChildren; GetSegmentCandidateForJoint(parent, isOutgoing, possibleChildren); @@ -164,7 +128,7 @@ IndexGraph::GetJointEdgeByLastPoint(Segment const & parent, Segment const & firs vector<JointEdge> edges; vector<RouteWeight> parentWeights; - std::map<JointSegment, JointSegment> emptyParents; + map<JointSegment, JointSegment> emptyParents; ReconstructJointSegment({}, parent, possibleChilds, lastPoints, isOutgoing, edges, parentWeights, emptyParents); @@ -210,18 +174,6 @@ void IndexGraph::SetRestrictions(RestrictionVec && restrictions) void IndexGraph::SetRoadAccess(RoadAccess && roadAccess) { m_roadAccess = move(roadAccess); } -void IndexGraph::GetOutgoingEdgesList(Segment const & segment, vector<SegmentEdge> & edges) -{ - edges.clear(); - GetEdgeList(segment, true /* isOutgoing */, edges); -} - -void IndexGraph::GetIngoingEdgesList(Segment const & segment, vector<SegmentEdge> & edges) -{ - edges.clear(); - GetEdgeList(segment, false /* isOutgoing */, edges); -} - RouteWeight IndexGraph::CalcSegmentWeight(Segment const & segment) { return RouteWeight( @@ -229,7 +181,7 @@ RouteWeight IndexGraph::CalcSegmentWeight(Segment const & segment) } void IndexGraph::GetNeighboringEdges(Segment const & from, RoadPoint const & rp, bool isOutgoing, - vector<SegmentEdge> & edges, std::map<Segment, Segment> & parents) + vector<SegmentEdge> & edges, map<Segment, Segment> & parents) { RoadGeometry const & road = m_geometry->GetRoad(rp.GetFeatureId()); @@ -300,7 +252,7 @@ void IndexGraph::ReconstructJointSegment(JointSegment const & parentJoint, bool isOutgoing, vector<JointEdge> & jointEdges, vector<RouteWeight> & parentWeights, - std::map<JointSegment, JointSegment> & parents) + map<JointSegment, JointSegment> & parents) { CHECK_EQUAL(firstChildren.size(), lastPointIds.size(), ()); @@ -324,23 +276,17 @@ void IndexGraph::ReconstructJointSegment(JointSegment const & parentJoint, if (m_roadAccess.GetPointType(parent.GetRoadPoint(isOutgoing)) == RoadAccess::Type::No) continue; - // Check firstChild for UTurn. - RoadPoint rp = parent.GetRoadPoint(isOutgoing); - if (IsUTurn(parent, firstChild) && m_roadIndex.GetJointId(rp) == Joint::kInvalidId - && !m_geometry->GetRoad(parent.GetFeatureId()).IsEndPointId(rp.GetPointId())) - { + if (IsUTurnAndRestricted(parent, firstChild, isOutgoing)) continue; - } - if (parent.GetFeatureId() != firstChild.GetFeatureId() && - IsRestricted(parentJoint, parent.GetFeatureId(), firstChild.GetFeatureId(), + if (IsRestricted(parentJoint, parent.GetFeatureId(), firstChild.GetFeatureId(), isOutgoing, parents)) { continue; } // Check current JointSegment for bad road access between segments. - rp = firstChild.GetRoadPoint(isOutgoing); + RoadPoint rp = firstChild.GetRoadPoint(isOutgoing); uint32_t start = currentPointId; bool noRoadAccess = false; do @@ -386,25 +332,18 @@ void IndexGraph::ReconstructJointSegment(JointSegment const & parentJoint, } void IndexGraph::GetNeighboringEdge(Segment const & from, Segment const & to, bool isOutgoing, - vector<SegmentEdge> & edges, std::map<Segment, Segment> & parents) + vector<SegmentEdge> & edges, map<Segment, Segment> & parents) { - // Blocking U-turns on internal feature points. - RoadPoint const rp = from.GetRoadPoint(isOutgoing); - if (IsUTurn(from, to) && m_roadIndex.GetJointId(rp) == Joint::kInvalidId - && !m_geometry->GetRoad(from.GetFeatureId()).IsEndPointId(rp.GetPointId())) - { + if (IsUTurnAndRestricted(from, to, isOutgoing)) return; - } - if (from.GetFeatureId() != to.GetFeatureId() && - IsRestricted(from, from.GetFeatureId(), to.GetFeatureId(), isOutgoing, parents)) - { + if (IsRestricted(from, from.GetFeatureId(), to.GetFeatureId(), isOutgoing, parents)) return; - } if (m_roadAccess.GetFeatureType(to.GetFeatureId()) == RoadAccess::Type::No) return; + RoadPoint const rp = from.GetRoadPoint(isOutgoing); if (m_roadAccess.GetPointType(rp) == RoadAccess::Type::No) return; @@ -440,4 +379,32 @@ RouteWeight IndexGraph::GetPenalties(Segment const & u, Segment const & v) } WorldGraphMode IndexGraph::GetMode() const { return WorldGraphMode::Undefined; } + +bool IndexGraph::IsUTurnAndRestricted(Segment const & parent, Segment const & child, + bool isOutgoing) const +{ + if (!IsUTurn(parent, child)) + return false; + + RoadPoint rp = parent.GetRoadPoint(isOutgoing); + if (m_roadIndex.GetJointId(rp) == Joint::kInvalidId && + !m_geometry->GetRoad(parent.GetFeatureId()).IsEndPointId(rp.GetPointId())) + { + return true; + } + + uint32_t const currentFeatureId = child.GetFeatureId(); + auto const & restrictions = isOutgoing ? m_restrictionsForward : m_restrictionsBackward; + auto const it = restrictions.find(currentFeatureId); + if (it == restrictions.cend()) + return false; + + for (vector<uint32_t> const & restriction : it->second) + { + if (restriction.size() == 1 && restriction.back() == currentFeatureId) + return true; + } + + return false; +} } // namespace routing diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp index a5aef48fd0..64507a62ae 100644 --- a/routing/index_graph.hpp +++ b/routing/index_graph.hpp @@ -15,17 +15,19 @@ #include "geometry/point2d.hpp" #include <cstdint> +#include <map> +#include <memory> #include <memory> +#include <unordered_map> #include <utility> #include <vector> -#include <map> -#include <memory> - #include <boost/optional.hpp> namespace routing { +bool IsUTurn(Segment const & u, Segment const & v); + enum class WorldGraphMode; class IndexGraph final @@ -36,15 +38,20 @@ public: using Edge = SegmentEdge; using Weight = RouteWeight; + using Restrictions = std::unordered_map<uint32_t, std::vector<std::vector<uint32_t>>>; + IndexGraph() = default; IndexGraph(std::shared_ptr<Geometry> geometry, std::shared_ptr<EdgeEstimator> estimator, RoutingOptions routingOptions = RoutingOptions()); + static std::map<Segment, Segment> kEmptyParentsSegments; // Put outgoing (or ingoing) egdes for segment to the 'edges' vector. - void GetEdgeList(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges); + void GetEdgeList(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges, + std::map<Segment, Segment> & parents = kEmptyParentsSegments); - void GetEdgeList(Segment const & parent, bool isOutgoing, std::vector<JointEdge> & edges, - std::vector<RouteWeight> & parentWeights); + void GetEdgeList(JointSegment const & parentJoint, + Segment const & parent, bool isOutgoing, std::vector<JointEdge> & edges, + std::vector<RouteWeight> & parentWeights, std::map<JointSegment, JointSegment> & parents); boost::optional<JointEdge> GetJointEdgeByLastPoint(Segment const & parent, Segment const & firstChild, bool isOutgoing, uint32_t lastPoint); @@ -70,10 +77,6 @@ public: void SetRestrictions(RestrictionVec && restrictions); void SetRoadAccess(RoadAccess && roadAccess); - // Interface for AStarAlgorithm: - void GetOutgoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges); - void GetIngoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges); - void PushFromSerializer(Joint::Id jointId, RoadPoint const & rp) { m_roadIndex.PushFromSerializer(jointId, rp); @@ -101,27 +104,119 @@ public: return GetGeometry().GetRoad(segment.GetFeatureId()).GetPoint(segment.GetPointId(front)); } - RouteWeight CalcSegmentWeight(Segment const & segment); + /// \brief Check, that we can go to |currentFeatureId|. + /// We pass |parentFeatureId| and don't use |parent.GetFeatureId()| because + /// |parent| can be fake sometimes but |parentFeatureId| is almost non-fake. + template <typename Parent> + bool IsRestricted(Parent const & parent, + uint32_t parentFeatureId, + uint32_t currentFeatureId, bool isOutgoing, + std::map<Parent, Parent> & parents) const; + bool IsUTurnAndRestricted(Segment const & parent, Segment const & child, bool isOutgoing) const; private: + + RouteWeight CalcSegmentWeight(Segment const & segment); + void GetNeighboringEdges(Segment const & from, RoadPoint const & rp, bool isOutgoing, - std::vector<SegmentEdge> & edges); + std::vector<SegmentEdge> & edges, std::map<Segment, Segment> & parents); void GetNeighboringEdge(Segment const & from, Segment const & to, bool isOutgoing, - std::vector<SegmentEdge> & edges); + std::vector<SegmentEdge> & edges, std::map<Segment, Segment> & parents); RouteWeight GetPenalties(Segment const & u, Segment const & v); void GetSegmentCandidateForJoint(Segment const & parent, bool isOutgoing, std::vector<Segment> & children); - void ReconstructJointSegment(Segment const & parent, std::vector<Segment> const & firstChildren, + void ReconstructJointSegment(JointSegment const & parentJoint, + Segment const & parent, + std::vector<Segment> const & firstChildren, std::vector<uint32_t> const & lastPointIds, - bool isOutgoing, std::vector<JointEdge> & jointEdges, - std::vector<RouteWeight> & parentWeights); + bool isOutgoing, + std::vector<JointEdge> & jointEdges, + std::vector<RouteWeight> & parentWeights, + std::map<JointSegment, JointSegment> & parents); std::shared_ptr<Geometry> m_geometry; std::shared_ptr<EdgeEstimator> m_estimator; RoadIndex m_roadIndex; JointIndex m_jointIndex; - RestrictionVec m_restrictions; + Restrictions m_restrictionsForward; + Restrictions m_restrictionsBackward; RoadAccess m_roadAccess; RoutingOptions m_avoidRoutingOptions; }; + +template <typename Parent> +bool IndexGraph::IsRestricted(Parent const & parent, + uint32_t parentFeatureId, + uint32_t currentFeatureId, + bool isOutgoing, + std::map<Parent, Parent> & parents) const +{ + if (parentFeatureId == currentFeatureId) + return false; + + auto const & restrictions = isOutgoing ? m_restrictionsForward : m_restrictionsBackward; + auto const it = restrictions.find(currentFeatureId); + if (it == restrictions.cend()) + return false; + + std::vector<Parent> parentsFromCurrent; + // Finds the first featureId from parents, that differ from |p.GetFeatureId()|. + auto const appendNextParent = [&parents](Parent const & p, auto & parentsVector) + { + uint32_t prevFeatureId = p.GetFeatureId(); + uint32_t curFeatureId = prevFeatureId; + + auto nextParent = parents.end(); + auto * curParent = &p; + while (curFeatureId == prevFeatureId) + { + auto const parentIt = parents.find(*curParent); + if (parentIt == parents.cend()) + return false; + + curFeatureId = parentIt->second.GetFeatureId(); + nextParent = parentIt; + curParent = &nextParent->second; + } + + ASSERT(nextParent != parents.end(), ()); + parentsVector.emplace_back(nextParent->second); + return true; + }; + + for (std::vector<uint32_t> const & restriction : it->second) + { + bool const prevIsParent = restriction[0] == parentFeatureId; + if (!prevIsParent) + continue; + + if (restriction.size() == 1) + return true; + + // If parents are empty we process only two feature restrictions. + if (parents.empty()) + continue; + + if (!appendNextParent(parent, parentsFromCurrent)) + continue; + + for (size_t i = 1; i < restriction.size(); ++i) + { + ASSERT_GREATER_OR_EQUAL(i, 1, ("Unexpected overflow.")); + if (i - 1 == parentsFromCurrent.size() + && !appendNextParent(parentsFromCurrent.back(), parentsFromCurrent)) + { + break; + } + + if (parentsFromCurrent.back().GetFeatureId() != restriction[i]) + break; + + if (i + 1 == restriction.size()) + return true; + } + } + + return false; +} } // namespace routing diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp index 4d12f200ef..8dc261033f 100644 --- a/routing/index_graph_starter.hpp +++ b/routing/index_graph_starter.hpp @@ -18,7 +18,9 @@ #include <cstddef> #include <cstdint> +#include <functional> #include <limits> +#include <map> #include <set> #include <utility> #include <vector> @@ -80,15 +82,17 @@ public: // start and finish in pass-through/non-pass-through area and number of non-pass-through crosses. bool CheckLength(RouteWeight const & weight); - void GetEdgeList(Segment const & segment, bool isOutgoing, + void GetEdgeList(JointSegment const & parent, Segment const & segment, bool isOutgoing, std::vector<JointEdge> & edges, std::vector<RouteWeight> & parentWeights) const { - return m_graph.GetEdgeList(segment, isOutgoing, edges, parentWeights); + return m_graph.GetEdgeList(parent, segment, isOutgoing, edges, parentWeights); } void GetEdgesList(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges) const; + // AStarGraph overridings: + // @{ void GetOutgoingEdgesList(Vertex const & segment, std::vector<Edge> & edges) override { GetEdgesList(segment, true /* isOutgoing */, edges); @@ -105,23 +109,52 @@ public: GetPoint(to, true /* front */)); } + void SetAStarParents(bool forward, std::map<Segment, Segment> & parents) override + { + m_graph.SetAStarParents(forward, parents); + } + + bool AreWavesConnectible(std::map<Vertex, Vertex> & forwardParents, Vertex const & commonVertex, + std::map<Vertex, Vertex> & backwardParents) override + { + return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents, nullptr); + } + // @} + RouteWeight HeuristicCostEstimate(Vertex const & from, m2::PointD const & to) const { return m_graph.HeuristicCostEstimate(GetPoint(from, true /* front */), to); } + RouteWeight CalcSegmentWeight(Segment const & segment) const; + double CalcSegmentETA(Segment const & segment) const; + + // For compatibility with IndexGraphStarterJoints + // @{ + void SetAStarParents(bool forward, std::map<JointSegment, JointSegment> & parents) + { + m_graph.SetAStarParents(forward, parents); + } + + bool AreWavesConnectible(std::map<JointSegment, JointSegment> & forwardParents, + JointSegment const & commonVertex, + std::map<JointSegment, JointSegment> & backwardParents, + std::function<uint32_t(JointSegment const &)> && fakeFeatureConverter) + { + return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents, + std::move(fakeFeatureConverter)); + } + bool IsJoint(Segment const & segment, bool fromStart) { return GetGraph().GetIndexGraph(segment.GetMwmId()).IsJoint(segment.GetRoadPoint(fromStart)); } - - RouteWeight CalcSegmentWeight(Segment const & segment) const; - double CalcSegmentETA(Segment const & segment) const; + // @} ~IndexGraphStarter() override = default; private: - // Start or finish ending information. + // Start or finish ending information. struct Ending { bool OverlapsWithMwm(NumMwmId mwmId) const; @@ -174,6 +207,5 @@ private: Ending m_finish; double m_startToFinishDistanceM; FakeGraph<Segment, FakeVertex> m_fake; - RoutingOptions m_avoidRoutingOptions; }; } // namespace routing diff --git a/routing/index_graph_starter_joints.hpp b/routing/index_graph_starter_joints.hpp index 16efceeda4..9b8f283387 100644 --- a/routing/index_graph_starter_joints.hpp +++ b/routing/index_graph_starter_joints.hpp @@ -37,23 +37,50 @@ public: void Init(Segment const & startSegment, Segment const & endSegment); + m2::PointD const & GetPoint(JointSegment const & jointSegment, bool start); JointSegment const & GetStartJoint() const { return m_startJoint; } JointSegment const & GetFinishJoint() const { return m_endJoint; } - m2::PointD const & GetPoint(JointSegment const & jointSegment, bool start); // AStarGraph overridings // @{ - RouteWeight HeuristicCostEstimate(JointSegment const & from, JointSegment const & to) override; + RouteWeight HeuristicCostEstimate(Vertex const & from, Vertex const & to) override; - void GetOutgoingEdgesList(JointSegment const & vertex, std::vector<JointEdge> & edges) override + void GetOutgoingEdgesList(Vertex const & vertex, std::vector<Edge> & edges) override { GetEdgeList(vertex, true /* isOutgoing */, edges); } - void GetIngoingEdgesList(JointSegment const & vertex, std::vector<JointEdge> & edges) override + void GetIngoingEdgesList(Vertex const & vertex, std::vector<Edge> & edges) override { GetEdgeList(vertex, false /* isOutgoing */, edges); } + + void SetAStarParents(bool forward, std::map<Vertex, Vertex> & parents) override + { + m_graph.SetAStarParents(forward, parents); + } + + bool AreWavesConnectible(std::map<Vertex, Vertex> & forwardParents, Vertex const & commonVertex, + std::map<Vertex, Vertex> & backwardParents) override + { + auto converter = [&](JointSegment const & vertex) { + if (vertex.IsRealSegment()) + return vertex.GetFeatureId(); + + auto const it = m_fakeJointSegments.find(vertex); + ASSERT(it != m_fakeJointSegments.cend(), ()); + + auto const & first = it->second.GetSegment(true /* start */); + if (first.IsRealSegment()) + return first.GetFeatureId(); + + auto const & second = it->second.GetSegment(false /* start */); + return second.GetFeatureId(); + }; + + return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents, + std::move(converter)); + } // @} WorldGraphMode GetMode() const { return m_graph.GetMode(); } @@ -388,7 +415,7 @@ bool IndexGraphStarterJoints<Graph>::FillEdgesAndParentsWeights(JointSegment con Segment parentSegment = optional.value(); std::vector<JointEdge> jointEdges; - m_graph.GetEdgeList(parentSegment, isOutgoing, jointEdges, parentWeights); + m_graph.GetEdgeList(vertex, parentSegment, isOutgoing, jointEdges, parentWeights); edges.insert(edges.end(), jointEdges.begin(), jointEdges.end()); firstFakeId = edges.size(); diff --git a/routing/joint_segment.cpp b/routing/joint_segment.cpp index 1d5f79960d..87b393586e 100644 --- a/routing/joint_segment.cpp +++ b/routing/joint_segment.cpp @@ -8,7 +8,7 @@ namespace routing { -bool IsRealSegment(Segment const & segment) +bool IsRealSegmentSimple(Segment const & segment) { return segment.GetFeatureId() != FakeFeatureIds::kIndexGraphStarterId; } @@ -17,7 +17,7 @@ JointSegment::JointSegment(Segment const & from, Segment const & to) { // Can not check segment for fake or not with segment.IsRealSegment(), because all segments // have got fake m_numMwmId during mwm generation. - CHECK(IsRealSegment(from) && IsRealSegment(to), + CHECK(IsRealSegmentSimple(from) && IsRealSegmentSimple(to), ("Segments of joints can not be fake. Only through ToFake() method.")); CHECK_EQUAL(from.GetMwmId(), to.GetMwmId(), ("Different mwmIds in segments for JointSegment")); diff --git a/routing/joint_segment.hpp b/routing/joint_segment.hpp index 062945bb5b..eddfe4aeca 100644 --- a/routing/joint_segment.hpp +++ b/routing/joint_segment.hpp @@ -20,10 +20,10 @@ public: JointSegment() = default; JointSegment(Segment const & from, Segment const & to); - uint32_t & GetFeatureId() { return m_featureId; } + void SetFeatureId(uint32_t id) { m_featureId = id; } uint32_t GetFeatureId() const { return m_featureId; } NumMwmId GetMwmId() const { return m_numMwmId; } - NumMwmId & GetMwmId() { return m_numMwmId; } + void SetMwmId(NumMwmId id) { m_numMwmId = id; } uint32_t GetStartSegmentId() const { return m_startSegmentId; } uint32_t GetEndSegmentId() const { return m_endSegmentId; } uint32_t GetSegmentId(bool start) const { return start ? m_startSegmentId : m_endSegmentId; } @@ -31,6 +31,7 @@ public: void ToFake(uint32_t fakeId); bool IsFake() const; + bool IsRealSegment() const { return !IsFake(); } Segment GetSegment(bool start) const { diff --git a/routing/routing_tests/index_graph_tools.cpp b/routing/routing_tests/index_graph_tools.cpp index 63d0c1f513..019ddd5cd5 100644 --- a/routing/routing_tests/index_graph_tools.cpp +++ b/routing/routing_tests/index_graph_tools.cpp @@ -2,6 +2,8 @@ #include "testing/testing.hpp" +#include "testing/testing.hpp" + #include "routing/geometry.hpp" #include "routing/base/routing_result.hpp" @@ -13,6 +15,8 @@ #include <unordered_map> +using namespace std; + namespace routing_test { using namespace routing; @@ -21,33 +25,6 @@ namespace { double constexpr kEpsilon = 1e-6; -class WorldGraphForAStar : public AStarGraph<Segment, SegmentEdge, RouteWeight> -{ -public: - - explicit WorldGraphForAStar(WorldGraph & graph) : m_graph(graph) {} - - Weight HeuristicCostEstimate(Vertex const & from, Vertex const & to) override - { - return m_graph.HeuristicCostEstimate(from, to); - } - - void GetOutgoingEdgesList(Vertex const & v, std::vector<Edge> & edges) override - { - m_graph.GetOutgoingEdgesList(v, edges); - } - - void GetIngoingEdgesList(Vertex const & v, std::vector<Edge> & edges) override - { - m_graph.GetIngoingEdgesList(v, edges); - } - - ~WorldGraphForAStar() override = default; - -private: - WorldGraph & m_graph; -}; - template <typename Graph> Graph & GetGraph(unordered_map<NumMwmId, unique_ptr<Graph>> const & graphs, NumMwmId mwmId) { @@ -197,7 +174,7 @@ bool TestIndexGraphTopology::FindPath(Vertex start, Vertex finish, double & path AlgorithmForWorldGraph algorithm; - routing_test::WorldGraphForAStar graphForAStar(*worldGraph); + WorldGraphForAStar graphForAStar(*worldGraph); AlgorithmForWorldGraph::ParamsForTests params(graphForAStar, startSegment, finishSegment, nullptr /* prevRoute */, @@ -324,7 +301,7 @@ unique_ptr<SingleVehicleWorldGraph> BuildWorldGraph(unique_ptr<TestGeometryLoade auto indexLoader = make_unique<TestIndexGraphLoader>(); indexLoader->AddGraph(kTestNumMwmId, move(graph)); return make_unique<SingleVehicleWorldGraph>(nullptr /* crossMwmGraph */, move(indexLoader), - estimator); + estimator); } unique_ptr<IndexGraph> BuildIndexGraph(unique_ptr<TestGeometryLoader> geometryLoader, @@ -341,11 +318,12 @@ unique_ptr<SingleVehicleWorldGraph> BuildWorldGraph(unique_ptr<ZeroGeometryLoade vector<Joint> const & joints) { auto graph = make_unique<IndexGraph>(make_shared<Geometry>(move(geometryLoader)), estimator); + graph->Import(joints); auto indexLoader = make_unique<TestIndexGraphLoader>(); indexLoader->AddGraph(kTestNumMwmId, move(graph)); return make_unique<SingleVehicleWorldGraph>(nullptr /* crossMwmGraph */, move(indexLoader), - estimator); + estimator); } unique_ptr<TransitWorldGraph> BuildWorldGraph(unique_ptr<TestGeometryLoader> geometryLoader, diff --git a/routing/routing_tests/index_graph_tools.hpp b/routing/routing_tests/index_graph_tools.hpp index f903fa1618..3702894dd6 100644 --- a/routing/routing_tests/index_graph_tools.hpp +++ b/routing/routing_tests/index_graph_tools.hpp @@ -53,6 +53,7 @@ public: using Weight = AStarGraph::Weight; explicit WorldGraphForAStar(WorldGraph & graph) : m_graph(graph) {} + ~WorldGraphForAStar() override = default; Weight HeuristicCostEstimate(Vertex const & from, Vertex const & to) override { @@ -69,8 +70,6 @@ public: m_graph.GetIngoingEdgesList(v, edges); } - ~WorldGraphForAStar() override = default; - private: WorldGraph & m_graph; }; diff --git a/routing/segment.hpp b/routing/segment.hpp index fa78d31e69..a811730770 100644 --- a/routing/segment.hpp +++ b/routing/segment.hpp @@ -31,8 +31,10 @@ public: { } + Segment const & GetSegment(bool /* start */) const { return *this; } NumMwmId GetMwmId() const { return m_mwmId; } uint32_t GetFeatureId() const { return m_featureId; } + void SetFeatureId(uint32_t id) { m_featureId = id; } uint32_t GetSegmentIdx() const { return m_segmentIdx; } bool IsForward() const { return m_forward; } @@ -60,6 +62,8 @@ public: return m_forward < seg.m_forward; } + uint32_t GetStartSegmentId() const { return 0; } + bool operator==(Segment const & seg) const { return m_featureId == seg.m_featureId && m_segmentIdx == seg.m_segmentIdx && diff --git a/routing/single_vehicle_world_graph.cpp b/routing/single_vehicle_world_graph.cpp index c35a3de688..575370948b 100644 --- a/routing/single_vehicle_world_graph.cpp +++ b/routing/single_vehicle_world_graph.cpp @@ -1,15 +1,27 @@ #include "routing/single_vehicle_world_graph.hpp" +#include <algorithm> #include <utility> +#include "base/assert.hpp" +#include "boost/optional.hpp" + namespace routing { using namespace std; +template <> +SingleVehicleWorldGraph::AStarParents<Segment>::ParentType +SingleVehicleWorldGraph::AStarParents<Segment>::kEmpty = {}; + +template <> +SingleVehicleWorldGraph::AStarParents<JointSegment>::ParentType +SingleVehicleWorldGraph::AStarParents<JointSegment>::kEmpty = {}; + SingleVehicleWorldGraph::SingleVehicleWorldGraph(unique_ptr<CrossMwmGraph> crossMwmGraph, unique_ptr<IndexGraphLoader> loader, shared_ptr<EdgeEstimator> estimator) - : m_crossMwmGraph(move(crossMwmGraph)), m_loader(move(loader)), m_estimator(estimator) + : m_crossMwmGraph(move(crossMwmGraph)), m_loader(move(loader)), m_estimator(move(estimator)) { CHECK(m_loader, ()); CHECK(m_estimator, ()); @@ -46,12 +58,13 @@ void SingleVehicleWorldGraph::CheckAndProcessTransitFeatures(Segment const & par twinIndexGraph.GetLastPointsForJoint({start}, isOutgoing, lastPoints); ASSERT_EQUAL(lastPoints.size(), 1, ()); - if (auto edge = currentIndexGraph.GetJointEdgeByLastPoint(parent, target.GetSegment(!opposite), + if (auto edge = currentIndexGraph.GetJointEdgeByLastPoint(parent, + target.GetSegment(!opposite), isOutgoing, lastPoints.back())) { newCrossMwmEdges.emplace_back(*edge); - newCrossMwmEdges.back().GetTarget().GetFeatureId() = twinFeatureId; - newCrossMwmEdges.back().GetTarget().GetMwmId() = twinMwmId; + newCrossMwmEdges.back().GetTarget().SetFeatureId(twinFeatureId); + newCrossMwmEdges.back().GetTarget().SetMwmId(twinMwmId); parentWeights.emplace_back(parentWeights[i]); } @@ -61,7 +74,8 @@ void SingleVehicleWorldGraph::CheckAndProcessTransitFeatures(Segment const & par jointEdges.insert(jointEdges.end(), newCrossMwmEdges.begin(), newCrossMwmEdges.end()); } -void SingleVehicleWorldGraph::GetEdgeList(Segment const & parent, bool isOutgoing, +void SingleVehicleWorldGraph::GetEdgeList(JointSegment const & parentJoint, + Segment const & parent, bool isOutgoing, vector<JointEdge> & jointEdges, vector<RouteWeight> & parentWeights) { @@ -70,8 +84,11 @@ void SingleVehicleWorldGraph::GetEdgeList(Segment const & parent, bool isOutgoin if (!parent.IsRealSegment()) return; + ASSERT(m_parentsForSegments.forward && m_parentsForSegments.backward, + ("m_parentsForSegments was not initialized in SingleVehicleWorldGraph.")); + auto & parents = isOutgoing ? *m_parentsForJoints.forward : *m_parentsForJoints.backward; auto & indexGraph = GetIndexGraph(parent.GetMwmId()); - indexGraph.GetEdgeList(parent, isOutgoing, jointEdges, parentWeights); + indexGraph.GetEdgeList(parentJoint, parent, isOutgoing, jointEdges, parentWeights, parents); if (m_mode != WorldGraphMode::JointSingleMwm) CheckAndProcessTransitFeatures(parent, jointEdges, parentWeights, isOutgoing); @@ -94,8 +111,11 @@ void SingleVehicleWorldGraph::GetEdgeList(Segment const & segment, bool isOutgoi return; } + ASSERT(m_parentsForSegments.forward && m_parentsForSegments.backward, + ("m_parentsForSegments was not initialized in SingleVehicleWorldGraph.")); + auto & parents = isOutgoing ? *m_parentsForSegments.forward : *m_parentsForSegments.backward; IndexGraph & indexGraph = m_loader->GetIndexGraph(segment.GetMwmId()); - indexGraph.GetEdgeList(segment, isOutgoing, edges); + indexGraph.GetEdgeList(segment, isOutgoing, edges, parents); if (m_mode != WorldGraphMode::SingleMwm && m_crossMwmGraph && m_crossMwmGraph->IsTransition(segment, isOutgoing)) GetTwins(segment, isOutgoing, edges); @@ -213,4 +233,125 @@ RoutingOptions SingleVehicleWorldGraph::GetRoutingOptions(Segment const & segmen auto const & geometry = GetRoadGeometry(segment.GetMwmId(), segment.GetFeatureId()); return geometry.GetRoutingOptions(); } + +void SingleVehicleWorldGraph::SetAStarParents(bool forward, map<Segment, Segment> & parents) +{ + if (forward) + m_parentsForSegments.forward = &parents; + else + m_parentsForSegments.backward = &parents; +} + +void SingleVehicleWorldGraph::SetAStarParents(bool forward, map<JointSegment, JointSegment> & parents) +{ + if (forward) + m_parentsForJoints.forward = &parents; + else + m_parentsForJoints.backward = &parents; +} + +template <typename VertexType> +bool +SingleVehicleWorldGraph::AreWavesConnectibleImpl(map<VertexType, VertexType> const & forwardParents, + VertexType const & commonVertex, + map<VertexType, VertexType> const & backwardParents, + function<uint32_t(VertexType const &)> && fakeFeatureConverter) +{ + vector<VertexType> chain; + NumMwmId mwmId = kFakeNumMwmId; + auto const fillUntilNextFeatureId = [&](VertexType const & cur, map<VertexType, VertexType> const & parents) + { + auto startFeatureId = cur.GetFeatureId(); + auto it = parents.find(cur); + while (it != parents.end() && it->second.GetFeatureId() == startFeatureId) + { + chain.emplace_back(it->second); + it = parents.find(it->second); + } + + if (it == parents.end()) + return false; + + chain.emplace_back(it->second); + return true; + }; + + auto const fillParents = [&](VertexType const & start, map<VertexType, VertexType> const & parents) + { + VertexType current = start; + static uint32_t constexpr kStepCountOneSide = 3; + for (uint32_t i = 0; i < kStepCountOneSide; ++i) + { + if (!fillUntilNextFeatureId(current, parents)) + break; + + current = chain.back(); + } + }; + + fillParents(commonVertex, forwardParents); + + reverse(chain.begin(), chain.end()); + chain.emplace_back(commonVertex); + + fillParents(commonVertex, backwardParents); + + if (fakeFeatureConverter) + { + for (size_t i = 0; i < chain.size(); ++i) + { + if (!chain[i].IsRealSegment()) + chain[i].SetFeatureId(fakeFeatureConverter(chain[i])); + } + } + + map<VertexType, VertexType> parents; + for (size_t i = 1; i < chain.size(); ++i) + { + parents[chain[i]] = chain[i - 1]; + if (chain[i].IsRealSegment()) + { + if (mwmId != kFakeNumMwmId && mwmId != chain[i].GetMwmId()) + return true; + + mwmId = chain[i].GetMwmId(); + } + } + + if (mwmId == kFakeNumMwmId) + return true; + + auto & currentIndexGraph = GetIndexGraph(mwmId); + for (size_t i = 1; i < chain.size(); ++i) + { + auto const & parent = chain[i - 1]; + uint32_t const parentFeatureId = chain[i - 1].GetFeatureId(); + uint32_t const currentFeatureId = chain[i].GetFeatureId(); + + if (parentFeatureId != currentFeatureId && + currentIndexGraph.IsRestricted(parent, parentFeatureId, currentFeatureId, + true /* isOutgoing */, parents)) + { + return false; + } + } + + return true; +} + +bool SingleVehicleWorldGraph::AreWavesConnectible(ParentSegments & forwardParents, + Segment const & commonVertex, + ParentSegments & backwardParents, + function<uint32_t(Segment const &)> && fakeFeatureConverter) +{ + return AreWavesConnectibleImpl(forwardParents, commonVertex, backwardParents, move(fakeFeatureConverter)); +} + +bool SingleVehicleWorldGraph::AreWavesConnectible(ParentJoints & forwardParents, + JointSegment const & commonVertex, + ParentJoints & backwardParents, + function<uint32_t(JointSegment const &)> && fakeFeatureConverter) +{ + return AreWavesConnectibleImpl(forwardParents, commonVertex, backwardParents, move(fakeFeatureConverter)); +} } // namespace routing diff --git a/routing/single_vehicle_world_graph.hpp b/routing/single_vehicle_world_graph.hpp index 4efd57de89..2dc1204baa 100644 --- a/routing/single_vehicle_world_graph.hpp +++ b/routing/single_vehicle_world_graph.hpp @@ -16,6 +16,8 @@ #include "geometry/point2d.hpp" +#include <functional> +#include <map> #include <memory> #include <vector> @@ -29,53 +31,82 @@ public: std::shared_ptr<EdgeEstimator> estimator); // WorldGraph overrides: + // @{ ~SingleVehicleWorldGraph() override = default; void GetEdgeList(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges) override; - void GetEdgeList(Segment const & parent, bool isOutgoing, + void GetEdgeList(JointSegment const & parentJoint, Segment const & parent, bool isOutgoing, std::vector<JointEdge> & jointEdges, std::vector<RouteWeight> & parentWeights) 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; bool IsPassThroughAllowed(NumMwmId mwmId, uint32_t featureId) override; void ClearCachedGraphs() override { m_loader->Clear(); } + void SetMode(WorldGraphMode mode) override { m_mode = mode; } WorldGraphMode GetMode() const override { return m_mode; } + void GetOutgoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges) override; void GetIngoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges) override; + RouteWeight HeuristicCostEstimate(Segment const & from, Segment const & to) override; RouteWeight HeuristicCostEstimate(m2::PointD const & from, m2::PointD const & to) override; RouteWeight HeuristicCostEstimate(Segment const & from, m2::PointD const & to) override; + RouteWeight CalcSegmentWeight(Segment const & segment) override; RouteWeight CalcLeapWeight(m2::PointD const & from, m2::PointD const & to) const override; RouteWeight CalcOffroadWeight(m2::PointD const & from, m2::PointD const & to) const override; double CalcSegmentETA(Segment const & segment) override; + std::vector<Segment> const & GetTransitions(NumMwmId numMwmId, bool isEnter) override; + void SetRoutingOptions(RoutingOptions routingOptions) override { m_avoidRoutingOptions = routingOptions; } /// \returns true if feature, associated with segment satisfies users conditions. bool IsRoutingOptionsGood(Segment const & segment) override; RoutingOptions GetRoutingOptions(Segment const & segment) override; - void SetRoutingOptions(RoutingOptions routingOptions) override { m_avoidRoutingOptions = routingOptions; } std::unique_ptr<TransitInfo> GetTransitInfo(Segment const & segment) override; std::vector<RouteSegment::SpeedCamera> GetSpeedCamInfo(Segment const & segment) override; - // This method should be used for tests only - IndexGraph & GetIndexGraphForTests(NumMwmId numMwmId) + IndexGraph & GetIndexGraph(NumMwmId numMwmId) override { return m_loader->GetIndexGraph(numMwmId); } - IndexGraph & GetIndexGraph(NumMwmId numMwmId) override + void SetAStarParents(bool forward, ParentSegments & parents) override; + void SetAStarParents(bool forward, ParentJoints & parents) override; + + bool AreWavesConnectible(ParentSegments & forwardParents, Segment const & commonVertex, + ParentSegments & backwardParents, + std::function<uint32_t(Segment const &)> && fakeFeatureConverter) override; + bool AreWavesConnectible(ParentJoints & forwardParents, JointSegment const & commonVertex, + ParentJoints & backwardParents, + std::function<uint32_t(JointSegment const &)> && fakeFeatureConverter) override; + // @} + + // This method should be used for tests only + IndexGraph & GetIndexGraphForTests(NumMwmId numMwmId) { return m_loader->GetIndexGraph(numMwmId); } private: + /// \brief Get parents' featureIds of |commonVertex| from forward AStar wave and join them with + /// parents' featureIds from backward wave. + /// \return The result chain of fetureIds are used to find restrictions on it and understand whether + /// waves are connectable or not. + template <typename VertexType> + bool AreWavesConnectibleImpl(std::map<VertexType, VertexType> const & forwardParents, + VertexType const & commonVertex, + std::map<VertexType, VertexType> const & backwardParents, + std::function<uint32_t(VertexType const &)> && fakeFeatureConverter); + // Retrieves the same |jointEdges|, but into others mwms. // If they are cross mwm edges, of course. void CheckAndProcessTransitFeatures(Segment const & parent, @@ -92,5 +123,17 @@ private: std::shared_ptr<EdgeEstimator> m_estimator; RoutingOptions m_avoidRoutingOptions = RoutingOptions(); WorldGraphMode m_mode = WorldGraphMode::NoLeaps; + + template <typename Vertex> + struct AStarParents + { + using ParentType = std::map<Vertex, Vertex>; + static ParentType kEmpty; + ParentType * forward = &kEmpty; + ParentType * backward = &kEmpty; + }; + + AStarParents<Segment> m_parentsForSegments; + AStarParents<JointSegment> m_parentsForJoints; }; } // namespace routing diff --git a/routing/transit_world_graph.cpp b/routing/transit_world_graph.cpp index de5577e070..e45fea6ed5 100644 --- a/routing/transit_world_graph.cpp +++ b/routing/transit_world_graph.cpp @@ -24,7 +24,8 @@ TransitWorldGraph::TransitWorldGraph(unique_ptr<CrossMwmGraph> crossMwmGraph, CHECK(m_estimator, ()); } -void TransitWorldGraph::GetEdgeList(Segment const & segment, bool isOutgoing, +void TransitWorldGraph::GetEdgeList(JointSegment const & parentJoint, + Segment const & segment, bool isOutgoing, std::vector<JointEdge> & edges, std::vector<RouteWeight> & parentWeights) { diff --git a/routing/transit_world_graph.hpp b/routing/transit_world_graph.hpp index 97860b8e71..ab97e5a9f4 100644 --- a/routing/transit_world_graph.hpp +++ b/routing/transit_world_graph.hpp @@ -61,7 +61,8 @@ public: double CalcSegmentETA(Segment const & segment) override; std::unique_ptr<TransitInfo> GetTransitInfo(Segment const & segment) override; - void GetEdgeList(Segment const & segment, bool isOutgoing, + void GetEdgeList(JointSegment const & parentJoint, + Segment const & segment, bool isOutgoing, std::vector<JointEdge> & edges, std::vector<RouteWeight> & parentWeights) override; diff --git a/routing/world_graph.cpp b/routing/world_graph.cpp index 3c26ad05b1..bb2db63e90 100644 --- a/routing/world_graph.cpp +++ b/routing/world_graph.cpp @@ -1,5 +1,7 @@ #include "routing/world_graph.hpp" +#include <map> + namespace routing { void WorldGraph::GetTwins(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges) @@ -51,6 +53,23 @@ std::vector<RouteSegment::SpeedCamera> WorldGraph::GetSpeedCamInfo(Segment const return {}; } +void WorldGraph::SetAStarParents(bool forward, std::map<Segment, Segment> & parents) {} +void WorldGraph::SetAStarParents(bool forward, std::map<JointSegment, JointSegment> & parents) {} + +bool WorldGraph::AreWavesConnectible(ParentSegments & forwardParents, Segment const & commonVertex, + ParentSegments & backwardParents, + std::function<uint32_t(Segment const &)> && fakeFeatureConverter) +{ + return true; +} + +bool WorldGraph::AreWavesConnectible(ParentJoints & forwardParents, JointSegment const & commonVertex, + ParentJoints & backwardParents, + std::function<uint32_t(JointSegment const &)> && fakeFeatureConverter) +{ + return true; +} + void WorldGraph::SetRoutingOptions(RoutingOptions /* routingOption */) {} std::vector<Segment> const & WorldGraph::GetTransitions(NumMwmId numMwmId, bool isEnter) diff --git a/routing/world_graph.hpp b/routing/world_graph.hpp index 72090fbe2d..bb0902e422 100644 --- a/routing/world_graph.hpp +++ b/routing/world_graph.hpp @@ -13,6 +13,7 @@ #include "geometry/point2d.hpp" +#include <functional> #include <memory> #include <set> #include <string> @@ -46,7 +47,7 @@ public: virtual void GetEdgeList(Segment const & segment, bool isOutgoing, std::vector<SegmentEdge> & edges) = 0; - virtual void GetEdgeList(Segment const & segment, bool isOutgoing, + virtual void GetEdgeList(JointSegment const & vertex, Segment const & segment, bool isOutgoing, std::vector<JointEdge> & edges, std::vector<RouteWeight> & parentWeights) = 0; // Checks whether path length meets restrictions. Restrictions may depend on the distance from @@ -65,16 +66,19 @@ public: virtual void SetMode(WorldGraphMode mode) = 0; virtual WorldGraphMode GetMode() const = 0; - // Interface for AStarAlgorithm: virtual void GetOutgoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges) = 0; virtual void GetIngoingEdgesList(Segment const & segment, std::vector<SegmentEdge> & edges) = 0; virtual RouteWeight HeuristicCostEstimate(Segment const & from, Segment const & to) = 0; virtual RouteWeight HeuristicCostEstimate(m2::PointD const & from, m2::PointD const & to) = 0; virtual RouteWeight HeuristicCostEstimate(Segment const & from, m2::PointD const & to) = 0; + virtual RouteWeight CalcSegmentWeight(Segment const & segment) = 0; + virtual RouteWeight CalcLeapWeight(m2::PointD const & from, m2::PointD const & to) const = 0; + virtual RouteWeight CalcOffroadWeight(m2::PointD const & from, m2::PointD const & to) const = 0; + virtual double CalcSegmentETA(Segment const & segment) = 0; /// \returns transitions for mwm with id |numMwmId|. @@ -84,6 +88,19 @@ public: virtual RoutingOptions GetRoutingOptions(Segment const & /* segment */); virtual void SetRoutingOptions(RoutingOptions /* routingOptions */); + using ParentSegments = std::map<Segment, Segment>; + using ParentJoints = std::map<JointSegment, JointSegment>; + + virtual void SetAStarParents(bool forward, ParentSegments & parents); + virtual void SetAStarParents(bool forward, ParentJoints & parents); + + virtual bool AreWavesConnectible(ParentSegments & forwardParents, Segment const & commonVertex, + ParentSegments & backwardParents, + std::function<uint32_t(Segment const &)> && fakeFeatureConverter); + virtual bool AreWavesConnectible(ParentJoints & forwardParents, JointSegment const & commonVertex, + ParentJoints & backwardParents, + std::function<uint32_t(JointSegment const &)> && fakeFeatureConverter); + /// \returns transit-specific information for segment. For nontransit segments returns nullptr. virtual std::unique_ptr<TransitInfo> GetTransitInfo(Segment const & segment) = 0; |