diff --git a/routing/cross_mwm_road_graph.cpp b/routing/cross_mwm_road_graph.cpp index 3cabed95ab..efa4e62c21 100644 --- a/routing/cross_mwm_road_graph.cpp +++ b/routing/cross_mwm_road_graph.cpp @@ -5,9 +5,41 @@ namespace { +using namespace routing; + inline bool IsValidEdgeWeight(EdgeWeight const & w) { return w != INVALID_EDGE_WEIGHT; } + +void FindIngoingCrossNode(CrossRoutingContextReader const & currentContext, CrossNode const & toNode, + IngoingCrossNode & ingoingNode) +{ + bool found = false; + auto const findingFn = [&ingoingNode, &toNode, &found](IngoingCrossNode const & node){ + if (node.m_nodeId == toNode.node) + { + found = true; + ingoingNode = node; + } + }; + CHECK(currentContext.ForEachIngoingNodeNearPoint(toNode.point, findingFn), ()); + CHECK(found, ()); } +void FindOutgoingCrossNode(CrossRoutingContextReader const & currentContext, CrossNode const & fromNode, + OutgoingCrossNode & outgoingNode) +{ + bool found = false; + auto const findingFn = [&outgoingNode, &fromNode, &found](OutgoingCrossNode const & node){ + if (node.m_nodeId == fromNode.node) + { + found = true; + outgoingNode = node; + } + }; + CHECK(currentContext.ForEachOutgoingNodeNearPoint(fromNode.point, findingFn), ()); + CHECK(found, ()); +} +} // namespace + namespace routing { IRouter::ResultCode CrossMwmGraph::SetStartNode(CrossNode const & startNode) @@ -54,14 +86,14 @@ IRouter::ResultCode CrossMwmGraph::SetStartNode(CrossNode const & startNode) { if (IsValidEdgeWeight(weights[i])) { - vector const & nextCrosses = ConstructBorderCross(outgoingNodes[i], startMapping); + vector const & nextCrosses = ConstructBorderCrossByOutgoing(outgoingNodes[i], startMapping); for (auto const & nextCross : nextCrosses) { if (nextCross.toNode.IsValid()) dummyEdges.emplace_back(nextCross, weights[i]); + } } } - } m_virtualEdges.insert(make_pair(startNode, dummyEdges)); return IRouter::NoError; @@ -129,24 +161,35 @@ IRouter::ResultCode CrossMwmGraph::SetFinalNode(CrossNode const & finalNode) return IRouter::NoError; } -vector const & CrossMwmGraph::ConstructBorderCross(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping) const +vector const & CrossMwmGraph::ConstructBorderCrossByOutgoing(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const { - // Check cached crosses. auto const key = make_pair(startNode.m_nodeId, currentMapping->GetMwmId()); - auto const it = m_cachedNextNodes.find(key); - if (it != m_cachedNextNodes.end()) + auto const it = m_cachedNextNodesByOutgoing.find(key); + if (it != m_cachedNextNodesByOutgoing.end()) return it->second; - // Cache miss case. - auto & crosses = m_cachedNextNodes[key]; - ConstructBorderCrossImpl(startNode, currentMapping, crosses); + auto & crosses = m_cachedNextNodesByOutgoing[key]; + ConstructBorderCrossByOutgoingImpl(startNode, currentMapping, crosses); return crosses; } -bool CrossMwmGraph::ConstructBorderCrossImpl(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping, - vector & crosses) const +vector const & CrossMwmGraph::ConstructBorderCrossByIngoing(IngoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const +{ + auto const key = make_pair(startNode.m_nodeId, currentMapping->GetMwmId()); + auto const it = m_cachedNextNodesByIngoing.find(key); + if (it != m_cachedNextNodesByIngoing.end()) + return it->second; + + auto & crosses = m_cachedNextNodesByIngoing[key]; + ConstructBorderCrossByIngoingImpl(startNode, currentMapping, crosses); + return crosses; +} + +bool CrossMwmGraph::ConstructBorderCrossByOutgoingImpl(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping, + vector & crosses) const { auto const fromCross = CrossNode(startNode.m_nodeId, currentMapping->GetMwmId(), startNode.m_point); string const & nextMwm = currentMapping->m_crossContext.GetOutgoingMwmName(startNode); @@ -157,70 +200,123 @@ bool CrossMwmGraph::ConstructBorderCrossImpl(OutgoingCrossNode const & startNode ASSERT(crosses.empty(), ()); nextMapping->LoadCrossContext(); nextMapping->m_crossContext.ForEachIngoingNodeNearPoint(startNode.m_point, [&](IngoingCrossNode const & node) - { - if (node.m_point.EqualDxDy(startNode.m_point, kMwmCrossingNodeEqualityMeters * MercatorBounds::degreeInMetres)) - { - auto const toCross = CrossNode(node.m_nodeId, nextMapping->GetMwmId(), node.m_point); - if (toCross.IsValid()) - crosses.emplace_back(fromCross, toCross); - } + { + if (node.m_nodeId == INVALID_NODE_ID) + return; + + if (!node.m_point.EqualDxDy(startNode.m_point, kMwmCrossingNodeEqualityMeters * MercatorBounds::degreeInMetres)) + return; + + crosses.emplace_back(fromCross, CrossNode(node.m_nodeId, nextMapping->GetMwmId(), node.m_point)); }); return !crosses.empty(); } -void CrossMwmGraph::GetOutgoingEdgesList(BorderCross const & v, - vector & adj) const +bool CrossMwmGraph::ConstructBorderCrossByIngoingImpl(IngoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping, + vector & crosses) const +{ + ASSERT(crosses.empty(), ()); + auto const toCross = CrossNode(startNode.m_nodeId, currentMapping->GetMwmId(), startNode.m_point); + vector const & neighboringMwms = currentMapping->m_crossContext.GetNeighboringMwmList(); + string const & currentMwm = currentMapping->GetMwmId().GetInfo()->GetCountryName(); + // Note. There's no field |m_ingoingIndex| in class IngoingCrossNode. Because this + // index is not saved in osrm routing section. So we need to write a work around and + // to check all neighboring mwms. + for (string const & nextMwm : neighboringMwms) + { + TRoutingMappingPtr nextMapping = m_indexManager.GetMappingByName(nextMwm); + if (!nextMapping->IsValid()) + continue; + + nextMapping->LoadCrossContext(); + nextMapping->m_crossContext.ForEachOutgoingNodeNearPoint(startNode.m_point, [&](OutgoingCrossNode const & node){ + if (node.m_nodeId == INVALID_NODE_ID) + return; + + if (nextMapping->m_crossContext.GetOutgoingMwmName(node) != currentMwm) + return; + + if (!node.m_point.EqualDxDy(startNode.m_point, kMwmCrossingNodeEqualityMeters * MercatorBounds::degreeInMetres)) + return; + + crosses.emplace_back(CrossNode(node.m_nodeId, nextMapping->GetMwmId(), node.m_point), toCross); + }); + } + return !crosses.empty(); +} + +void CrossMwmGraph::GetEdgesList(BorderCross const & v, bool isOutgoing, vector & adj) const { // Check for virtual edges. adj.clear(); + + // Note. Code below processes virtual edges. This code does not work properly if isOutgoing == false. + // At the same time when this method is called with isOutgoing == false |m_virtualEdges| is empty. + if (!isOutgoing && !m_virtualEdges.empty()) + { + NOTIMPLEMENTED(); + return; + } + auto const it = m_virtualEdges.find(v.toNode); if (it != m_virtualEdges.end()) { adj.insert(adj.end(), it->second.begin(), it->second.end()); // For last map we need to load virtual shortcuts and real cross roads. It takes to account case - // when we have a path from the mwmw border to the point inside the map throuh another map. + // when we have a path from the mwm border to the point inside the map throuh another map. // See Ust-Katav test for more. if (it->second.empty() || !it->second.front().GetTarget().toNode.isVirtual) return; } // Loading cross routing section. - TRoutingMappingPtr currentMapping = m_indexManager.GetMappingById(v.toNode.mwmId); + TRoutingMappingPtr currentMapping = m_indexManager.GetMappingById(isOutgoing ? v.toNode.mwmId + : v.fromNode.mwmId); ASSERT(currentMapping->IsValid(), ()); currentMapping->LoadCrossContext(); currentMapping->FreeFileIfPossible(); CrossRoutingContextReader const & currentContext = currentMapping->m_crossContext; - // Find income node. - IngoingCrossNode ingoingNode; - bool found = false; - auto findingFn = [&ingoingNode, &v, &found](IngoingCrossNode const & node) + if (isOutgoing) { - if (node.m_nodeId == v.toNode.node) - { - found = true; - ingoingNode = node; - } - }; - CHECK(currentContext.ForEachIngoingNodeNearPoint(v.toNode.point, findingFn), ()); - - CHECK(found, ()); - - // Find outs. Generate adjacency list. - currentContext.ForEachOutgoingNode([&, this](OutgoingCrossNode const & node) - { - EdgeWeight const outWeight = currentContext.GetAdjacencyCost(ingoingNode, node); - if (outWeight != kInvalidContextEdgeWeight && outWeight != 0) + IngoingCrossNode ingoingNode; + FindIngoingCrossNode(currentContext, v.toNode, ingoingNode); + currentContext.ForEachOutgoingNode([&, this](OutgoingCrossNode const & node) { - vector const & targets = ConstructBorderCross(node, currentMapping); - for (auto const & target : targets) + EdgeWeight const outWeight = currentContext.GetAdjacencyCost(ingoingNode, node); + if (outWeight != kInvalidContextEdgeWeight && outWeight != 0) { - if (target.toNode.IsValid()) - adj.emplace_back(target, outWeight); - } - } - }); + vector const & targets = ConstructBorderCrossByOutgoing( + node, currentMapping); + for (auto const & target : targets) + { + if (target.toNode.IsValid()) + adj.emplace_back(target, outWeight); + } + } + }); + } + else + { + OutgoingCrossNode outgoingNode; + FindOutgoingCrossNode(currentContext, v.fromNode, outgoingNode); + currentContext.ForEachIngoingNode([&, this](IngoingCrossNode const & node) + { + EdgeWeight const weight = currentContext.GetAdjacencyCost(node, outgoingNode); + if (weight != kInvalidContextEdgeWeight && weight != 0) + { + vector const & targets = ConstructBorderCrossByIngoing( + node, currentMapping); + for (auto const & target : targets) + { + if (target.fromNode.IsValid()) + adj.emplace_back(target, weight); + } + } + }); + } } double CrossMwmGraph::HeuristicCostEstimate(BorderCross const & v, BorderCross const & w) const diff --git a/routing/cross_mwm_road_graph.hpp b/routing/cross_mwm_road_graph.hpp index 7687c3246f..4b597a62ff 100644 --- a/routing/cross_mwm_road_graph.hpp +++ b/routing/cross_mwm_road_graph.hpp @@ -72,6 +72,7 @@ struct BorderCross BorderCross(CrossNode const & from, CrossNode const & to) : fromNode(from), toNode(to) {} BorderCross() = default; + // TODO(bykoianko) Consider using fields |fromNode| and |toNode| in operator== and operator<. inline bool operator==(BorderCross const & a) const { return toNode == a.toNode; } inline bool operator<(BorderCross const & a) const { return toNode < a.toNode; } }; @@ -106,11 +107,14 @@ public: explicit CrossMwmGraph(RoutingIndexManager & indexManager) : m_indexManager(indexManager) {} - void GetOutgoingEdgesList(BorderCross const & v, vector & adj) const; - void GetIngoingEdgesList(BorderCross const & /* v */, - vector & /* adj */) const + void GetOutgoingEdgesList(BorderCross const & v, vector & adj) const { - NOTIMPLEMENTED(); + GetEdgesList(v, true /* isOutgoing */, adj); + } + + void GetIngoingEdgesList(BorderCross const & v, vector & adj) const + { + GetEdgesList(v, false /* isOutgoing */, adj); } double HeuristicCostEstimate(BorderCross const & v, BorderCross const & w) const; @@ -118,15 +122,20 @@ public: IRouter::ResultCode SetStartNode(CrossNode const & startNode); IRouter::ResultCode SetFinalNode(CrossNode const & finalNode); -private: - // Cashing wrapper for the ConstructBorderCrossImpl function. - vector const & ConstructBorderCross(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping) const; + // Cashing wrapper for the ConstructBorderCrossByOutgoingImpl function. + vector const & ConstructBorderCrossByOutgoing(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const; + vector const & ConstructBorderCrossByIngoing(IngoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const; +private: // Pure function to construct boder cross by outgoing cross node. - bool ConstructBorderCrossImpl(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping, - vector & cross) const; + bool ConstructBorderCrossByOutgoingImpl(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping, + vector & cross) const; + bool ConstructBorderCrossByIngoingImpl(IngoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping, + vector & crosses) const; /*! * Adds a virtual edge to the graph so that it is possible to represent * the final segment of the path that leads from the map's border @@ -135,6 +144,7 @@ private: */ void AddVirtualEdge(IngoingCrossNode const & node, CrossNode const & finalNode, EdgeWeight weight); + void GetEdgesList(BorderCross const & v, bool isOutgoing, vector & adj) const; map > m_virtualEdges; @@ -151,7 +161,9 @@ private: } }; - mutable unordered_map, Hash> m_cachedNextNodes; + // @TODO(bykoianko) Consider removing key work mutable. + mutable unordered_map, Hash> m_cachedNextNodesByIngoing; + mutable unordered_map, Hash> m_cachedNextNodesByOutgoing; }; //-------------------------------------------------------------------------------------------------- diff --git a/routing/cross_routing_context.hpp b/routing/cross_routing_context.hpp index 186ee6b78c..ec2bde2dde 100644 --- a/routing/cross_routing_context.hpp +++ b/routing/cross_routing_context.hpp @@ -91,7 +91,7 @@ class CrossRoutingContextReader public: void Load(Reader const & r); - const string & GetOutgoingMwmName(OutgoingCrossNode const & mwmIndex) const; + const string & GetOutgoingMwmName(OutgoingCrossNode const & outgoingNode) const; TWrittenEdgeWeight GetAdjacencyCost(IngoingCrossNode const & ingoing, OutgoingCrossNode const & outgoing) const;