Merge pull request #5413 from bykoianko/master-cross-mwm-GetTwins

CrossMwmIndexGraph::GetTwins() osrm implementation.

routing/cross_mwm_index_graph.cpp

@@ -1,20 +1,24 @@
 #include "routing/cross_mwm_index_graph.hpp"
 
-#include "platform/country_file.hpp"
-
 #include "base/macros.hpp"
+#include "base/stl_helpers.hpp"
 
+#include <algorithm>
 #include <utility>
 
+using namespace platform;
+using namespace routing;
+using namespace std;
+
 namespace
 {
-void FillTransitionSegments(routing::OsrmFtSegMapping const & segMapping, routing::TWrittenNodeId nodeId,
-                            routing::NumMwmId mwmId, std::set<routing::Segment> & transitionSegments)
+bool GetTransitionSegment(OsrmFtSegMapping const & segMapping, TWrittenNodeId nodeId,
+                          NumMwmId numMwmId, Segment & segment)
 {
   auto const range = segMapping.GetSegmentsRange(nodeId);
   for (size_t segmentIndex = range.first; segmentIndex != range.second; ++segmentIndex)
   {
-    routing::OsrmMappingTypes::FtSeg seg;
+    OsrmMappingTypes::FtSeg seg;
     // The meaning of node id in osrm is an edge between two joints.
     // So, it's possible to consider the first valid segment from the range which returns by GetSegmentsRange().
     segMapping.GetSegmentByIndex(segmentIndex, seg);
@@ -22,11 +26,44 @@ void FillTransitionSegments(routing::OsrmFtSegMapping const & segMapping, routin
       continue;
 
     CHECK_NOT_EQUAL(seg.m_pointStart, seg.m_pointEnd, ());
-    transitionSegments.emplace(seg.m_fid, min(seg.m_pointStart, seg.m_pointEnd), mwmId, seg.IsForward());
-    return;
+    segment = Segment(numMwmId, seg.m_fid, min(seg.m_pointStart, seg.m_pointEnd), seg.IsForward());
+    return true;
   }
   LOG(LERROR, ("No valid segments in the range returned by OsrmFtSegMapping::GetSegmentsRange(", nodeId,
-               "). Num mwm id:", mwmId));
+               "). Num mwm id:", numMwmId));
+  return false;
+}
+
+void AddTransitionSegment(OsrmFtSegMapping const & segMapping, TWrittenNodeId nodeId,
+                          NumMwmId numMwmId, std::vector<Segment> & segments)
+{
+  Segment key;
+  if (GetTransitionSegment(segMapping, nodeId, numMwmId, key))
+    segments.push_back(key);
+}
+
+void FillTransitionSegments(OsrmFtSegMapping const & segMapping, TWrittenNodeId nodeId,
+                            NumMwmId numMwmId, ms::LatLon const & latLon,
+                            std::map<Segment, ms::LatLon> & transitionSegments)
+{
+  Segment key;
+  if (!GetTransitionSegment(segMapping, nodeId, numMwmId, key))
+    return;
+
+  auto const p = transitionSegments.emplace(key, latLon);
+  if (p.second)
+    return;
+
+  // @TODO(bykoianko) It's necessary to investigate this case.
+  LOG(LWARNING, ("Trying to insert a transition segment that was previously inserted. The key:",
+                 *p.first, ", the value:", latLon));
+}
+
+ms::LatLon const & GetLatLon(std::map<Segment, ms::LatLon> const & segMap, Segment const & s)
+{
+  auto it = segMap.find(s);
+  CHECK(it != segMap.cend(), ());
+  return it->second;
 }
 }  // namespace
 
@@ -41,47 +78,99 @@ bool CrossMwmIndexGraph::IsTransition(Segment const & s, bool isOutgoing)
   auto it = m_transitionCache.find(s.GetMwmId());
   if (it == m_transitionCache.cend())
   {
-    platform::CountryFile const & countryFile = m_numMwmIds->GetFile(s.GetMwmId());
-    TRoutingMappingPtr mappingPtr = m_indexManager.GetMappingByName(countryFile.GetName());
-    MappingGuard mappingPtrGuard(mappingPtr);
-
-    CHECK(mappingPtr, ("countryFile:", countryFile));
-    mappingPtr->LoadCrossContext();
-
-    TransitionSegments transitionSegments;
-    mappingPtr->m_crossContext.ForEachOutgoingNode([&](OutgoingCrossNode const & node)
-    {
-      FillTransitionSegments(mappingPtr->m_segMapping, node.m_nodeId, s.GetMwmId(),
-                             transitionSegments.m_outgoing);
-    });
-    mappingPtr->m_crossContext.ForEachIngoingNode([&](IngoingCrossNode const & node)
-    {
-      FillTransitionSegments(mappingPtr->m_segMapping, node.m_nodeId, s.GetMwmId(),
-                             transitionSegments.m_ingoing);
-    });
-    auto const p = m_transitionCache.emplace(s.GetMwmId(), transitionSegments);
-    it = p.first;
-    CHECK(p.second, ("Mwm num id:", s.GetMwmId(), "has been inserted before. countryFile:",
-                     countryFile));
+    InsertWholeMwmTransitionSegments(s.GetMwmId());
+    it = m_transitionCache.find(s.GetMwmId());
   }
+  CHECK(it != m_transitionCache.cend(),
+        ("Mwm ", s.GetMwmId(), "has not been downloaded. s:", s, ". isOutgoing", isOutgoing));
 
   if (isOutgoing)
     return it->second.m_outgoing.count(s) != 0;
   return it->second.m_ingoing.count(s) != 0;
 }
 
-void CrossMwmIndexGraph::GetTwins(Segment const & /* s */, std::vector<Segment> & /* twins */) const
+void CrossMwmIndexGraph::GetTwins(Segment const & s, bool isOutgoing, std::vector<Segment> & twins)
+{
+  CHECK(IsTransition(s, isOutgoing), ("The segment is not a transition segment."));
+  twins.clear();
+  // @TODO(bykoianko) It's necessary to check if mwm of |s| contains an A* cross mwm section
+  // and if so to use it. If not, osrm cross mwm sections should be used.
+
+  auto const getTwins = [&](NumMwmId /* numMwmId */, TRoutingMappingPtr const & segMapping)
+  {
+    vector<string> const & neighboringMwm = segMapping->m_crossContext.GetNeighboringMwmList();
+
+    for (string const & name : neighboringMwm)
+      InsertWholeMwmTransitionSegments(m_numMwmIds->GetId(CountryFile(name)));
+
+    auto it = m_transitionCache.find(s.GetMwmId());
+    CHECK(it != m_transitionCache.cend(), ());
+
+    ms::LatLon const & latLon = isOutgoing ? GetLatLon(it->second.m_outgoing, s)
+                                           : GetLatLon(it->second.m_ingoing, s);
+    for (string const & name : neighboringMwm)
+    {
+      auto const addTransitionSegments = [&](NumMwmId numMwmId, TRoutingMappingPtr const & mapping)
+      {
+        if (isOutgoing)
+        {
+          mapping->m_crossContext.ForEachIngoingNodeNearPoint(latLon, [&](IngoingCrossNode const & node){
+            AddTransitionSegment(mapping->m_segMapping, node.m_nodeId, numMwmId, twins);
+          });
+        }
+        else
+        {
+          mapping->m_crossContext.ForEachOutgoingNodeNearPoint(latLon, [&](OutgoingCrossNode const & node){
+            AddTransitionSegment(mapping->m_segMapping, node.m_nodeId, numMwmId, twins);
+          });
+        }
+      };
+
+      if (!LoadWith(m_numMwmIds->GetId(CountryFile(name)), addTransitionSegments))
+        continue;  // mwm was not loaded.
+    }
+  };
+
+  LoadWith(s.GetMwmId(), getTwins);
+  my::SortUnique(twins);
+}
+
+void CrossMwmIndexGraph::GetEdgeList(Segment const & /* s */,
+                                     bool /* isOutgoing */, std::vector<SegmentEdge> & /* edges */) const
 {
   // @TODO(bykoianko) It's necessary to check if mwm of |s| contains an A* cross mwm section
   // and if so to use it. If not, osrm cross mwm sections should be used.
   NOTIMPLEMENTED();
 }
 
-void CrossMwmIndexGraph::GetEdgeList(Segment const & /* s */,
-                                         bool /* isOutgoing */, std::vector<SegmentEdge> & /* edges */) const
+void CrossMwmIndexGraph::Clear()
 {
-  // @TODO(bykoianko) It's necessary to check if mwm of |s| contains an A* cross mwm section
-  // and if so to use it. If not, osrm cross mwm sections should be used.
-  NOTIMPLEMENTED();
+  m_transitionCache.clear();
+}
+
+void CrossMwmIndexGraph::InsertWholeMwmTransitionSegments(NumMwmId numMwmId)
+{
+  if (m_transitionCache.count(numMwmId) != 0)
+    return;
+
+  auto const fillAllTransitionSegments = [this](NumMwmId numMwmId, TRoutingMappingPtr const & mapping){
+    TransitionSegments transitionSegments;
+    mapping->m_crossContext.ForEachOutgoingNode([&](OutgoingCrossNode const & node)
+    {
+      FillTransitionSegments(mapping->m_segMapping, node.m_nodeId, numMwmId,
+                             node.m_point, transitionSegments.m_outgoing);
+    });
+    mapping->m_crossContext.ForEachIngoingNode([&](IngoingCrossNode const & node)
+    {
+      FillTransitionSegments(mapping->m_segMapping, node.m_nodeId, numMwmId,
+                             node.m_point, transitionSegments.m_ingoing);
+    });
+    auto const p = m_transitionCache.emplace(numMwmId, move(transitionSegments));
+    ASSERT(p.second, ("Mwm num id:", numMwmId, "has been inserted before. Country file name:",
+                      mapping->GetCountryName()));
+  };
+
+  if (!LoadWith(numMwmId, fillAllTransitionSegments))
+    m_transitionCache.emplace(numMwmId, TransitionSegments());
 }
 }  // namespace routing

routing/cross_mwm_index_graph.hpp

@@ -1,15 +1,17 @@
 #pragma once
 
-#include "routing/cross_mwm_index_graph.hpp"
 #include "routing/num_mwm_id.hpp"
 #include "routing/routing_mapping.hpp"
 #include "routing/segment.hpp"
 
+#include "geometry/latlon.hpp"
+
+#include "platform/country_file.hpp"
+
 #include "base/math.hpp"
 
 #include <map>
 #include <memory>
-#include <set>
 #include <vector>
 
 namespace routing
@@ -52,10 +54,14 @@ public:
 
   /// \brief Fills |twins| with duplicates of |s| transition segment in neighbouring mwm.
   /// For most cases there is only one twin for |s|.
-  /// If |s| is an enter transition segment fills |twins| with appropriate exit transition segments.
-  /// If |s| is an exit transition segment fills |twins| with appropriate enter transition segments.
-  /// \note GetTwins(...) shall be called only if IsTransition(s, ...) returns true.
-  void GetTwins(Segment const & s, std::vector<Segment> & twins) const;
+  /// If |isOutgoing| == true |s| should be an exit transition segment and
+  /// the mehtod fills |twins| with appropriate enter transition segments.
+  /// If |isOutgoing| == false |s| should be an enter transition segment and
+  /// the method fills |twins| with appropriate exit transition segments.
+  /// \note GetTwins(s, isOutgoing, ...) shall be called only if IsTransition(s, isOutgoing) returns true.
+  /// \note GetTwins(s, isOutgoing, twins) fills |twins| only if mwm contained |twins| has been downloaded.
+  /// If not, |twins| could be emply after a GetTwins(...) call.
+  void GetTwins(Segment const & s, bool isOutgoing, std::vector<Segment> & twins);
 
   /// \brief Fills |edges| with edges outgoing from |s| (ingoing to |s|).
   /// If |isOutgoing| == true then |s| should be an enter transition segment.
@@ -68,13 +74,35 @@ public:
   /// if |isOutgoing| == true and from |SegmentEdge::m_target| to |s| otherwise.
   void GetEdgeList(Segment const & s, bool isOutgoing, std::vector<SegmentEdge> & edges) const;
 
+  void Clear();
+
 private:
   struct TransitionSegments
   {
-    std::set<Segment> m_ingoing;
-    std::set<Segment> m_outgoing;
+    std::map<Segment, ms::LatLon> m_ingoing;
+    std::map<Segment, ms::LatLon> m_outgoing;
   };
 
+  /// \brief Inserts all ingoing and outgoing transition segments of mwm with |numMwmId|
+  /// to |m_transitionCache|.
+  void InsertWholeMwmTransitionSegments(NumMwmId numMwmId);
+
+  template <class Fn>
+  bool LoadWith(NumMwmId numMwmId, Fn && fn)
+  {
+    platform::CountryFile const & countryFile = m_numMwmIds->GetFile(numMwmId);
+    TRoutingMappingPtr mapping = m_indexManager.GetMappingByName(countryFile.GetName());
+    CHECK(mapping, ("No routing mapping file for countryFile:", countryFile));
+
+    if (!mapping->IsValid())
+      return false; // mwm was not loaded.
+
+    MappingGuard mappingGuard(mapping);
+    mapping->LoadCrossContext();
+    fn(numMwmId, mapping);
+    return true;
+  }
+
   RoutingIndexManager & m_indexManager;
   std::shared_ptr<NumMwmIds> m_numMwmIds;
 

routing/cross_mwm_road_graph.cpp

@@ -6,7 +6,6 @@
 namespace
 {
 inline bool IsValidEdgeWeight(EdgeWeight const & w) { return w != INVALID_EDGE_WEIGHT; }
-double constexpr kMwmCrossingNodeEqualityRadiusDegrees = 0.001;
 }
 
 namespace routing
@@ -159,7 +158,7 @@ bool CrossMwmGraph::ConstructBorderCrossImpl(OutgoingCrossNode const & startNode
   nextMapping->LoadCrossContext();
   nextMapping->m_crossContext.ForEachIngoingNodeNearPoint(startNode.m_point, [&](IngoingCrossNode const & node)
     {
-    if (node.m_point.EqualDxDy(startNode.m_point, kMwmCrossingNodeEqualityRadiusDegrees))
+    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())

routing/cross_routing_context.cpp

@@ -1,13 +1,10 @@
 #include "routing/cross_routing_context.hpp"
 
-#include "geometry/mercator.hpp"
 #include "indexer/point_to_int64.hpp"
 
 namespace
 {
 uint32_t constexpr kCoordBits = POINT_COORD_BITS;
-
-double constexpr kMwmCrossingNodeEqualityRadiusDegrees = 0.001;
 }  // namespace
 
 namespace routing
@@ -74,7 +71,10 @@ void CrossRoutingContextReader::Load(Reader const & r)
   m_outgoingNodes.resize(size);
 
   for (size_t i = 0; i < size; ++i)
+  {
     pos = m_outgoingNodes[i].Load(r, pos, i);
+    m_outgoingIndex.Add(m_outgoingNodes[i]);
+  }
 
   size_t adjacencySize = ingoingSize * m_outgoingNodes.size();
   size_t const adjMatrixSize = sizeof(TWrittenEdgeWeight) * adjacencySize;
@@ -96,21 +96,6 @@ void CrossRoutingContextReader::Load(Reader const & r)
   }
 }
 
-bool CrossRoutingContextReader::ForEachIngoingNodeNearPoint(ms::LatLon const & point, function<void(IngoingCrossNode const & node)> && fn) const
-{
-  bool found = false;
-  m_ingoingIndex.ForEachInRect(m2::RectD(point.lat - kMwmCrossingNodeEqualityRadiusDegrees,
-                                     point.lon - kMwmCrossingNodeEqualityRadiusDegrees,
-                                     point.lat + kMwmCrossingNodeEqualityRadiusDegrees,
-                                     point.lon + kMwmCrossingNodeEqualityRadiusDegrees),
-                               [&found, &fn](IngoingCrossNode const & node)
-                               {
-                                 fn(node);
-                                 found = true;
-                               });
-  return found;
-}
-
 const string & CrossRoutingContextReader::GetOutgoingMwmName(
     OutgoingCrossNode const & outgoingNode) const
 {
@@ -131,6 +116,15 @@ TWrittenEdgeWeight CrossRoutingContextReader::GetAdjacencyCost(IngoingCrossNode
   return cost_index < m_adjacencyMatrix.size() ? m_adjacencyMatrix[cost_index] : kInvalidContextEdgeWeight;
 }
 
+m2::RectD CrossRoutingContextReader::GetMwmCrossingNodeEqualityRect(ms::LatLon const & point) const
+{
+  double constexpr kMwmCrossingNodeEqualityDegrees = kMwmCrossingNodeEqualityMeters * MercatorBounds::degreeInMetres;
+  return m2::RectD(point.lat - kMwmCrossingNodeEqualityDegrees,
+                   point.lon - kMwmCrossingNodeEqualityDegrees,
+                   point.lat + kMwmCrossingNodeEqualityDegrees,
+                   point.lon + kMwmCrossingNodeEqualityDegrees);
+}
+
 void CrossRoutingContextWriter::Save(Writer & w) const
 {
   uint32_t size = static_cast<uint32_t>(m_ingoingNodes.size());

routing/cross_routing_context.hpp

@@ -3,6 +3,7 @@
 #include "coding/file_container.hpp"
 
 #include "geometry/latlon.hpp"
+#include "geometry/mercator.hpp"
 #include "geometry/point2d.hpp"
 #include "geometry/rect2d.hpp"
 #include "geometry/tree4d.hpp"
@@ -19,6 +20,7 @@ using TWrittenEdgeWeight = uint32_t;
 TWrittenEdgeWeight constexpr kInvalidContextEdgeNodeId = std::numeric_limits<uint32_t>::max();
 TWrittenEdgeWeight constexpr kInvalidContextEdgeWeight = std::numeric_limits<TWrittenEdgeWeight>::max();
 size_t constexpr kInvalidAdjacencyIndex = numeric_limits<size_t>::max();
+double constexpr kMwmCrossingNodeEqualityMeters = 80;
 
 struct IngoingCrossNode
 {
@@ -70,6 +72,8 @@ struct OutgoingCrossNode
   void Save(Writer & w) const;
 
   size_t Load(Reader const & r, size_t pos, size_t adjacencyIndex);
+
+  m2::RectD const GetLimitRect() const { return m2::RectD(m_point.lat, m_point.lon, m_point.lat, m_point.lon); }
 };
 
 using IngoingEdgeIteratorT = vector<IngoingCrossNode>::const_iterator;
@@ -82,17 +86,44 @@ class CrossRoutingContextReader
   vector<string> m_neighborMwmList;
   vector<TWrittenEdgeWeight> m_adjacencyMatrix;
   m4::Tree<IngoingCrossNode> m_ingoingIndex;
+  m4::Tree<OutgoingCrossNode> m_outgoingIndex;
 
 public:
   void Load(Reader const & r);
 
   const string & GetOutgoingMwmName(OutgoingCrossNode const & mwmIndex) const;
 
-  bool ForEachIngoingNodeNearPoint(ms::LatLon const & point, function<void(IngoingCrossNode const & node)> && fn) const;
-
   TWrittenEdgeWeight GetAdjacencyCost(IngoingCrossNode const & ingoing,
                                      OutgoingCrossNode const & outgoing) const;
 
+  vector<string> const & GetNeighboringMwmList() const { return m_neighborMwmList; }
+
+  m2::RectD GetMwmCrossingNodeEqualityRect(ms::LatLon const & point) const;
+
+  template <class Fn>
+  bool ForEachIngoingNodeNearPoint(ms::LatLon const & point, Fn && fn) const
+  {
+    bool found = false;
+    m_ingoingIndex.ForEachInRect(GetMwmCrossingNodeEqualityRect(point),
+                                 [&found, &fn](IngoingCrossNode const & node) {
+                                   fn(node);
+                                   found = true;
+                                 });
+    return found;
+  }
+
+  template <class Fn>
+  bool ForEachOutgoingNodeNearPoint(ms::LatLon const & point, Fn && fn) const
+  {
+    bool found = false;
+    m_outgoingIndex.ForEachInRect(GetMwmCrossingNodeEqualityRect(point),
+                                  [&found, &fn](OutgoingCrossNode const & node) {
+                                    fn(node);
+                                    found = true;
+                                  });
+    return found;
+  }
+
   template <class TFunctor>
   void ForEachIngoingNode(TFunctor f) const
   {

routing/routing_tests/cross_routing_tests.cpp

@@ -154,5 +154,15 @@ UNIT_TEST(TestFindingByPoint)
   TEST_EQUAL(node.size(), 2, ());
   TEST_EQUAL(node[0].m_nodeId, 5, ());
   TEST_EQUAL(node[1].m_nodeId, 6, ());
+
+  vector<OutgoingCrossNode> outgoingNode;
+  auto fnOutgoing = [&outgoingNode](OutgoingCrossNode const & nd) {outgoingNode.push_back(nd);};
+  TEST(newContext.ForEachOutgoingNodeNearPoint(ms::LatLon::Zero(), fnOutgoing), ());
+  TEST_EQUAL(outgoingNode.size(), 1, ());
+  TEST_EQUAL(outgoingNode[0].m_nodeId, 4, ());
+
+  outgoingNode.clear();
+  TEST(!newContext.ForEachOutgoingNodeNearPoint(p3, fnOutgoing), ());
+  TEST(outgoingNode.empty(), ());
 }
 }  // namespace