bugfix RedressRoute race

5 files changed, 88 insertions, 10 deletions

diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp
index 8c535fb94f..77ec8bb2a4 100644
--- a/routing/index_graph.hpp
+++ b/routing/index_graph.hpp
@@ -42,13 +42,10 @@ public:
   void AddDirectEdge(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo, Joint::Id target,
                      bool forward, vector<JointEdge> & edges) const;
   void AddNeighboringEdges(RoadPoint rp, bool isOutgoing, vector<JointEdge> & edges) const;
-  double CalcHeuristic(m2::PointD const & from, m2::PointD const & to) const
-  {
-    return m_estimator->CalcHeuristic(from, to);
-  }
   void GetEdgesList(Joint::Id jointId, bool forward, vector<JointEdge> & edges) const;
   Joint::Id GetJointId(RoadPoint rp) const { return m_roadIndex.GetJointId(rp); }
   Geometry const & GetGeometry() const { return m_geometry; }
+  EdgeEstimator const & GetEstimator() const { return *m_estimator; }
   m2::PointD const & GetPoint(Joint::Id jointId) const;
   uint32_t GetNumRoads() const { return m_roadIndex.GetSize(); }
   uint32_t GetNumJoints() const { return m_jointIndex.GetNumJoints(); }
diff --git a/routing/index_graph_starter.cpp b/routing/index_graph_starter.cpp
index 7b1f5658cd..53f64a7b1b 100644
--- a/routing/index_graph_starter.cpp
+++ b/routing/index_graph_starter.cpp
@@ -138,14 +138,45 @@ void IndexGraphStarter::FindPointsWithCommonFeature(Joint::Id jointId0, Joint::I
                                                     RoadPoint & result0, RoadPoint & result1) const
 {
   bool found = false;
+  double minWeight = -1.0;
 
   ForEachPoint(jointId0, [&](RoadPoint const & rp0) {
     ForEachPoint(jointId1, [&](RoadPoint const & rp1) {
-      if (rp0.GetFeatureId() == rp1.GetFeatureId() && !found)
+      if (rp0.GetFeatureId() == rp1.GetFeatureId())
       {
-        result0 = rp0;
-        result1 = rp1;
-        found = true;
+        RoadGeometry const & road = m_graph.GetGeometry().GetRoad(rp0.GetFeatureId());
+        if (!road.IsRoad())
+          return;
+
+        if (road.IsOneWay() && rp0.GetPointId() > rp1.GetPointId())
+          return;
+
+        if (found)
+        {
+          if (minWeight < 0.0)
+          {
+            // CalcEdgesWeight is very expensive.
+            // So calculate it only if second common feature found.
+            RoadGeometry const & prevRoad = m_graph.GetGeometry().GetRoad(result0.GetFeatureId());
+            minWeight = m_graph.GetEstimator().CalcEdgesWeight(prevRoad, result0.GetPointId(),
+                                                               result1.GetPointId());
+          }
+
+          double const weight =
+              m_graph.GetEstimator().CalcEdgesWeight(road, rp0.GetPointId(), rp1.GetPointId());
+          if (weight < minWeight)
+          {
+            minWeight = weight;
+            result0 = rp0;
+            result1 = rp1;
+          }
+        }
+        else
+        {
+          result0 = rp0;
+          result1 = rp1;
+          found = true;
+        }
       }
     });
   });
diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp
index 104b4bd30a..c036c0bd14 100644
--- a/routing/index_graph_starter.hpp
+++ b/routing/index_graph_starter.hpp
@@ -49,7 +49,7 @@ public:
 
   double HeuristicCostEstimate(Joint::Id from, Joint::Id to) const
   {
-    return m_graph.CalcHeuristic(GetPoint(from), GetPoint(to));
+    return m_graph.GetEstimator().CalcHeuristic(GetPoint(from), GetPoint(to));
   }
 
   // Add intermediate points to route (those don't correspond to any joint).
diff --git a/routing/road_point.hpp b/routing/road_point.hpp
index c613bca31f..654f89c675 100644
--- a/routing/road_point.hpp
+++ b/routing/road_point.hpp
@@ -1,6 +1,8 @@
 #pragma once
 
 #include "std/cstdint.hpp"
+#include "std/sstream.hpp"
+#include "std/string.hpp"
 
 namespace routing
 {
@@ -28,4 +30,12 @@ private:
   uint32_t m_featureId;
   uint32_t m_pointId;
 };
+
+inline string DebugPrint(RoadPoint const & rp)
+{
+  ostringstream out;
+  out << "rp("
+      << "(" << rp.GetFeatureId() << ", " << rp.GetPointId() << ")";
+  return out.str();
+}
 }  // namespace routing
diff --git a/routing/routing_tests/index_graph_test.cpp b/routing/routing_tests/index_graph_test.cpp
index 22d1b7ad77..4a853baf8a 100644
--- a/routing/routing_tests/index_graph_test.cpp
+++ b/routing/routing_tests/index_graph_test.cpp
@@ -68,7 +68,7 @@ shared_ptr<EdgeEstimator> CreateEstimator()
 }
 
 void TestRoute(IndexGraph const & graph, RoadPoint const & start, RoadPoint const & finish,
-               size_t expectedLength)
+               size_t expectedLength, vector<RoadPoint> const * expectedRoute = nullptr)
 {
   LOG(LINFO, ("Test route", start.GetFeatureId(), ",", start.GetPointId(), "=>",
               finish.GetFeatureId(), ",", finish.GetPointId()));
@@ -84,6 +84,9 @@ void TestRoute(IndexGraph const & graph, RoadPoint const & start, RoadPoint cons
   vector<RoadPoint> roadPoints;
   starter.RedressRoute(routingResult.path, roadPoints);
   TEST_EQUAL(roadPoints.size(), expectedLength, ());
+
+  if (expectedRoute != nullptr)
+    TEST_EQUAL(roadPoints, *expectedRoute, ());
 }
 
 void TestEdges(IndexGraph const & graph, Joint::Id jointId,
@@ -268,4 +271,41 @@ UNIT_TEST(FindPathManhattan)
     }
   }
 }
+
+// Roads                          y:
+//
+//  R0:         * - * - *         -1
+//            /           \
+//  R1:   J0 * -* - * - *- * J1    0
+//            \           /
+//  R2:         * - * - *          1
+//
+//     x:    0  1   2   3  4
+//
+UNIT_TEST(RedressRace)
+{
+  unique_ptr<TestGeometryLoader> loader = make_unique<TestGeometryLoader>();
+
+  loader->AddRoad(
+      0 /* featureId */, false,
+      RoadGeometry::Points({{0.0, 0.0}, {1.0, -1.0}, {2.0, -1.0}, {3.0, -1.0}, {4.0, 0.0}}));
+
+  loader->AddRoad(
+      1 /* featureId */, false,
+      RoadGeometry::Points({{0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}, {3.0, 0.0}, {4.0, 0.0}}));
+
+  loader->AddRoad(
+      2 /* featureId */, false,
+      RoadGeometry::Points({{0.0, 0.0}, {1.0, 1.0}, {2.0, 1.0}, {3.0, 1.0}, {4.0, 0.0}}));
+
+  IndexGraph graph(move(loader), CreateEstimator());
+
+  vector<Joint> joints;
+  joints.emplace_back(MakeJoint({{0, 0}, {1, 0}, {2, 0}}));  // J0
+  joints.emplace_back(MakeJoint({{0, 4}, {1, 4}, {2, 4}}));  // J1
+  graph.Import(joints);
+
+  vector<RoadPoint> const expectedRoute({{1, 0}, {1, 1}, {1, 2}, {1, 3}, {1, 4}});
+  TestRoute(graph, {0, 0}, {0, 4}, 5, &expectedRoute);
+}
 }  // namespace routing_test