diff --git a/routing/cross_mwm_road_graph.cpp b/routing/cross_mwm_road_graph.cpp index 81fd522a8e..520703de96 100644 --- a/routing/cross_mwm_road_graph.cpp +++ b/routing/cross_mwm_road_graph.cpp @@ -54,9 +54,12 @@ IRouter::ResultCode CrossMwmGraph::SetStartNode(CrossNode const & startNode) { if (IsValidEdgeWeight(weights[i])) { - BorderCross nextCross = ConstructBorderCross(outgoingNodes[i], startMapping); - if (nextCross.toNode.IsValid()) - dummyEdges.emplace_back(nextCross, weights[i]); + vector const & nextCrosses = ConstructBorderCross(outgoingNodes[i], startMapping); + for (auto const & nextCross : nextCrosses) + { + if (nextCross.toNode.IsValid()) + dummyEdges.emplace_back(nextCross, weights[i]); + } } } @@ -126,8 +129,8 @@ IRouter::ResultCode CrossMwmGraph::SetFinalNode(CrossNode const & finalNode) return IRouter::NoError; } -BorderCross CrossMwmGraph::ConstructBorderCross(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping) const +vector const & CrossMwmGraph::ConstructBorderCross(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const { // Check cached crosses. auto const key = make_pair(startNode.m_nodeId, currentMapping->GetMwmId()); @@ -136,33 +139,28 @@ BorderCross CrossMwmGraph::ConstructBorderCross(OutgoingCrossNode const & startN return it->second; // Cache miss case. - BorderCross cross; - if (!ConstructBorderCrossImpl(startNode, currentMapping, cross)) - return BorderCross(); - m_cachedNextNodes.insert(make_pair(key, cross)); - return cross; + auto & crosses = m_cachedNextNodes[key]; + ConstructBorderCrossImpl(startNode, currentMapping, crosses); + return crosses; } bool CrossMwmGraph::ConstructBorderCrossImpl(OutgoingCrossNode const & startNode, TRoutingMappingPtr const & currentMapping, - BorderCross & cross) const + vector & crosses) const { string const & nextMwm = currentMapping->m_crossContext.GetOutgoingMwmName(startNode); TRoutingMappingPtr nextMapping = m_indexManager.GetMappingByName(nextMwm); // If we haven't this routing file, we skip this path. if (!nextMapping->IsValid()) return false; + crosses.clear(); nextMapping->LoadCrossContext(); - - IngoingCrossNode ingoingNode; - if (nextMapping->m_crossContext.FindIngoingNodeByPoint(startNode.m_point, ingoingNode)) + nextMapping->m_crossContext.ForEachIngoingNodeNearPoint(startNode.m_point, [&](IngoingCrossNode const & node) { - auto const & targetPoint = ingoingNode.m_point; - cross = BorderCross(CrossNode(startNode.m_nodeId, currentMapping->GetMwmId(), targetPoint), - CrossNode(ingoingNode.m_nodeId, nextMapping->GetMwmId(), targetPoint)); - return true; - } - return false; + crosses.emplace_back(CrossNode(startNode.m_nodeId, currentMapping->GetMwmId(), node.m_point), + CrossNode(node.m_nodeId, nextMapping->GetMwmId(), node.m_point)); + }); + return !crosses.empty(); } void CrossMwmGraph::GetOutgoingEdgesList(BorderCross const & v, @@ -187,16 +185,18 @@ void CrossMwmGraph::GetOutgoingEdgesList(BorderCross const & v, // Find income node. IngoingCrossNode ingoingNode; - CHECK(currentContext.FindIngoingNodeByPoint(v.toNode.point, ingoingNode), ()); - if (ingoingNode.m_nodeId != v.toNode.node) - { - LOG(LDEBUG, ("Several nodes stores in one border point.", v.toNode.point)); - currentContext.ForEachIngoingNode([&ingoingNode, &v](IngoingCrossNode const & node) - { - if (node.m_nodeId == v.toNode.node) - ingoingNode = node; - }); - } + bool found = false; + auto findingFn = [&ingoingNode, &v, &found](IngoingCrossNode const & node) + { + 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) @@ -204,9 +204,12 @@ void CrossMwmGraph::GetOutgoingEdgesList(BorderCross const & v, EdgeWeight const outWeight = currentContext.GetAdjacencyCost(ingoingNode, node); if (outWeight != kInvalidContextEdgeWeight && outWeight != 0) { - BorderCross target = ConstructBorderCross(node, currentMapping); - if (target.toNode.IsValid()) - adj.emplace_back(target, outWeight); + vector const & targets = ConstructBorderCross(node, currentMapping); + for (auto const & target : targets) + { + if (target.toNode.IsValid()) + adj.emplace_back(target, outWeight); + } } }); } diff --git a/routing/cross_mwm_road_graph.hpp b/routing/cross_mwm_road_graph.hpp index 6b82173002..e0a59911f7 100644 --- a/routing/cross_mwm_road_graph.hpp +++ b/routing/cross_mwm_road_graph.hpp @@ -120,13 +120,13 @@ public: private: // Cashing wrapper for the ConstructBorderCrossImpl function. - BorderCross ConstructBorderCross(OutgoingCrossNode const & startNode, - TRoutingMappingPtr const & currentMapping) const; + vector const & ConstructBorderCross(OutgoingCrossNode const & startNode, + TRoutingMappingPtr const & currentMapping) const; // Pure function to construct boder cross by outgoing cross node. bool ConstructBorderCrossImpl(OutgoingCrossNode const & startNode, TRoutingMappingPtr const & currentMapping, - BorderCross & cross) const; + vector & cross) 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 @@ -151,7 +151,7 @@ private: } }; - mutable unordered_map m_cachedNextNodes; + mutable unordered_map, Hash> m_cachedNextNodes; }; //-------------------------------------------------------------------------------------------------- diff --git a/routing/cross_routing_context.cpp b/routing/cross_routing_context.cpp index ecaededd23..f9fb4bc724 100644 --- a/routing/cross_routing_context.cpp +++ b/routing/cross_routing_context.cpp @@ -96,17 +96,16 @@ void CrossRoutingContextReader::Load(Reader const & r) } } -bool CrossRoutingContextReader::FindIngoingNodeByPoint(ms::LatLon const & point, - IngoingCrossNode & node) const +bool CrossRoutingContextReader::ForEachIngoingNodeNearPoint(ms::LatLon const & point, function && fn) const { bool found = false; m_ingoingIndex.ForEachInRect(m2::RectD(point.lat - kMwmCrossingNodeEqualityRadiusDegrees, point.lon - kMwmCrossingNodeEqualityRadiusDegrees, point.lat + kMwmCrossingNodeEqualityRadiusDegrees, point.lon + kMwmCrossingNodeEqualityRadiusDegrees), - [&found, &node](IngoingCrossNode const & nd) + [&found, &fn](IngoingCrossNode const & node) { - node = nd; + fn(node); found = true; }); return found; diff --git a/routing/cross_routing_context.hpp b/routing/cross_routing_context.hpp index c50cb7e37d..6e3a0e04b6 100644 --- a/routing/cross_routing_context.hpp +++ b/routing/cross_routing_context.hpp @@ -7,6 +7,7 @@ #include "geometry/rect2d.hpp" #include "geometry/tree4d.hpp" +#include "std/function.hpp" #include "std/string.hpp" #include "std/vector.hpp" @@ -87,7 +88,7 @@ public: const string & GetOutgoingMwmName(OutgoingCrossNode const & mwmIndex) const; - bool FindIngoingNodeByPoint(ms::LatLon const & point, IngoingCrossNode & node) const; + bool ForEachIngoingNodeNearPoint(ms::LatLon const & point, function && fn) const; TWrittenEdgeWeight GetAdjacencyCost(IngoingCrossNode const & ingoing, OutgoingCrossNode const & outgoing) const; diff --git a/routing/routing_tests/cross_routing_tests.cpp b/routing/routing_tests/cross_routing_tests.cpp index f9c591ef57..c0b5e643ef 100644 --- a/routing/routing_tests/cross_routing_tests.cpp +++ b/routing/routing_tests/cross_routing_tests.cpp @@ -121,12 +121,14 @@ UNIT_TEST(TestFindingByPoint) routing::CrossRoutingContextWriter context; routing::CrossRoutingContextReader newContext; - ms::LatLon p1(1., 1.), p2(5., 5.), p3(10.,1.); + ms::LatLon p1(1., 1.), p2(5., 5.), p3(10., 1.), p4(20., 1.); context.AddIngoingNode(1, ms::LatLon::Zero()); context.AddIngoingNode(2, p1); context.AddIngoingNode(3, p2); context.AddOutgoingNode(4, "foo", ms::LatLon::Zero()); + context.AddIngoingNode(5, p3); + context.AddIngoingNode(6, p3); context.ReserveAdjacencyMatrix(); vector buffer; @@ -136,11 +138,21 @@ UNIT_TEST(TestFindingByPoint) MemReader reader(buffer.data(), buffer.size()); newContext.Load(reader); - IngoingCrossNode node; - TEST(newContext.FindIngoingNodeByPoint(p1, node), ()); - TEST_EQUAL(node.m_nodeId, 2, ()); - TEST(newContext.FindIngoingNodeByPoint(p2, node), ()); - TEST_EQUAL(node.m_nodeId, 3, ()); - TEST(!newContext.FindIngoingNodeByPoint(p3, node), ()); + vector node; + auto fn = [&node](IngoingCrossNode const & nd) {node.push_back(nd);}; + TEST(newContext.ForEachIngoingNodeNearPoint(p1, fn), ()); + TEST_EQUAL(node.size(), 1, ()); + TEST_EQUAL(node[0].m_nodeId, 2, ()); + node.clear(); + TEST(newContext.ForEachIngoingNodeNearPoint(p2, fn), ()); + TEST_EQUAL(node.size(), 1, ()); + TEST_EQUAL(node[0].m_nodeId, 3, ()); + node.clear(); + TEST(!newContext.ForEachIngoingNodeNearPoint(p4, fn), ()); + node.clear(); + TEST(newContext.ForEachIngoingNodeNearPoint(p3, fn), ()); + TEST_EQUAL(node.size(), 2, ()); + TEST_EQUAL(node[0].m_nodeId, 5, ()); + TEST_EQUAL(node[1].m_nodeId, 6, ()); } } // namespace