Merge pull request #5021 from ygorshenin/release-70-uturnsbeta-531

[routing] Added UTurns.

10 files changed, 398 insertions, 37 deletions

diff --git a/routing/base/astar_algorithm.hpp b/routing/base/astar_algorithm.hpp
index 1c753701b3..0dc3c3f806 100644
--- a/routing/base/astar_algorithm.hpp
+++ b/routing/base/astar_algorithm.hpp
@@ -238,7 +238,10 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPath(
       double const piW = graph.HeuristicCostEstimate(stateW.vertex, finalVertex);
       double const reducedLen = len + piW - piV;
 
-      CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon));
+      CHECK_GREATER_OR_EQUAL(
+          reducedLen, -kEpsilon,
+          ("Invariant violation, vertex V:", stateV.vertex, "vertex W:", stateW.vertex,
+           "edge weight:", len, "heuristic V:", piV, "heuristic W:", piW));
       double const newReducedDist = stateV.distance + max(reducedLen, 0.0);
 
       auto const t = bestDistance.find(stateW.vertex);
@@ -351,7 +354,10 @@ typename AStarAlgorithm<TGraph>::Result AStarAlgorithm<TGraph>::FindPathBidirect
       double const pW = cur->ConsistentHeuristic(stateW.vertex);
       double const reducedLen = len + pW - pV;
 
-      CHECK(reducedLen >= -kEpsilon, ("Invariant violated:", reducedLen, "<", -kEpsilon));
+      CHECK_GREATER_OR_EQUAL(
+          reducedLen, -kEpsilon,
+          ("Invariant violation, vertex V:", stateV.vertex, "vertex W:", stateW.vertex,
+           "edge weight:", len, "heuristic V:", pV, "heuristic W:", pW));
       double const newReducedDist = stateV.distance + max(reducedLen, 0.0);
 
       auto const itCur = cur->bestDistance.find(stateW.vertex);
diff --git a/routing/edge_estimator.cpp b/routing/edge_estimator.cpp
index d929ccbc8a..6f3d2382b8 100644
--- a/routing/edge_estimator.cpp
+++ b/routing/edge_estimator.cpp
@@ -40,6 +40,7 @@ public:
   double CalcEdgesWeight(uint32_t featureId, RoadGeometry const & road, uint32_t pointFrom,
                          uint32_t pointTo) const override;
   double CalcHeuristic(m2::PointD const & from, m2::PointD const & to) const override;
+  double GetUTurnPenalty() const override;
 
 private:
   TrafficCache const & m_trafficCache;
@@ -66,16 +67,17 @@ void CarEdgeEstimator::Finish()
 double CarEdgeEstimator::CalcEdgesWeight(uint32_t featureId, RoadGeometry const & road,
                                          uint32_t pointFrom, uint32_t pointTo) const
 {
+  // Current time estimation is too optimistic.  Need more accurate
+  // tuning: traffic lights, traffic jams, road models and so on.  Add
+  // some penalty to make estimation more realistic.
+  //
+  // TODO: make accurate tuning, remove penalty.
+  double constexpr kTimePenalty = 1.8;
+
   uint32_t const start = min(pointFrom, pointTo);
   uint32_t const finish = max(pointFrom, pointTo);
   ASSERT_LESS(finish, road.GetPointsCount(), ());
 
-  // Current time estimation are too optimistic.
-  // Need more accurate tuning: traffic lights, traffic jams, road models and so on.
-  // Add some penalty to make estimation of a more realistic.
-  // TODO: make accurate tuning, remove penalty.
-  double constexpr kTimePenalty = 1.8;
-
   double result = 0.0;
   double const speedMPS = road.GetSpeed() * kKMPH2MPS;
   auto const dir = pointFrom < pointTo ? TrafficInfo::RoadSegmentId::kForwardDirection
@@ -102,6 +104,14 @@ double CarEdgeEstimator::CalcHeuristic(m2::PointD const & from, m2::PointD const
 {
   return TimeBetweenSec(from, to, m_maxSpeedMPS);
 }
+
+double CarEdgeEstimator::GetUTurnPenalty() const
+{
+  // Adds 2 minutes penalty for U-turn. The value is quite arbitrary
+  // and needs to be properly selected after a number of real-world
+  // experiments.
+  return 2 * 60;  // seconds
+}
 }  // namespace
 
 namespace routing
diff --git a/routing/edge_estimator.hpp b/routing/edge_estimator.hpp
index f78f2a7c25..bafc5cb186 100644
--- a/routing/edge_estimator.hpp
+++ b/routing/edge_estimator.hpp
@@ -24,7 +24,7 @@ public:
   virtual double CalcEdgesWeight(uint32_t featureId, RoadGeometry const & road, uint32_t pointFrom,
                                  uint32_t pointTo) const = 0;
   virtual double CalcHeuristic(m2::PointD const & from, m2::PointD const & to) const = 0;
-
+  virtual double GetUTurnPenalty() const = 0;
 
   static shared_ptr<EdgeEstimator> CreateForCar(IVehicleModel const & vehicleModel,
                                                 traffic::TrafficCache const & trafficCache);
diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp
index 601a592dea..2d84a6523d 100644
--- a/routing/index_graph.cpp
+++ b/routing/index_graph.cpp
@@ -670,6 +670,7 @@ void IndexGraph::InsertToEdgeMapping(DirectedEdge const & key, DirectedEdge cons
     return;
   }
   edges.push_back(value);
+  m_parentMapping[value] = key;
 }
 
 string DebugPrint(DirectedEdge const & directedEdge)
diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp
index 0235a67315..279bb975d8 100644
--- a/routing/index_graph.hpp
+++ b/routing/index_graph.hpp
@@ -76,11 +76,11 @@ public:
   }
 
 private:
-  Joint::Id const m_from = Joint::kInvalidId;
-  Joint::Id const m_to = Joint::kInvalidId;
+  Joint::Id m_from = Joint::kInvalidId;
+  Joint::Id m_to = Joint::kInvalidId;
   // Note. It's important to store feature id because two |m_from| and |m_to| may be
   // connected with several features.
-  uint32_t const m_featureId = 0;
+  uint32_t m_featureId = 0;
 };
 
 string DebugPrint(DirectedEdge const & directedEdge);
@@ -211,6 +211,16 @@ public:
     f(directedEdge);
   }
 
+  DirectedEdge GetParent(DirectedEdge const & e) const
+  {
+    // TODO (@bykoianko): get rid of static restrictions and use them
+    // dynamically in IndexGraphStarter::ApplyPenalties().
+    auto const it = m_parentMapping.find(e);
+    if (it == m_parentMapping.end())
+      return e;
+    return GetParent(it->second);
+  }
+
 private:
   friend struct routing_test::RestrictionTest;
 
@@ -356,5 +366,6 @@ private:
   // * be copied. If so the mapping is kept in |m_edgeMapping| and the source edge is not blocked.
   // See ApplyRestriction* method for a detailed comments about trasformation rules.
   map<DirectedEdge, vector<DirectedEdge>> m_edgeMapping;
+  map<DirectedEdge, DirectedEdge> m_parentMapping;
 };
 }  // namespace routing
diff --git a/routing/index_graph_starter.cpp b/routing/index_graph_starter.cpp
index 31b696f7c3..00ba9403af 100644
--- a/routing/index_graph_starter.cpp
+++ b/routing/index_graph_starter.cpp
@@ -2,8 +2,51 @@
 
 #include "routing/routing_exceptions.hpp"
 
+#include "std/sstream.hpp"
+
 namespace routing
 {
+namespace
+{
+int Compare(uint32_t a, uint32_t b) noexcept
+{
+  if (a < b)
+    return -1;
+  if (a > b)
+    return 1;
+  return 0;
+}
+}  // namespace
+
+// IndexGraphStarter::TVertexType ------------------------------------------------------------------
+IndexGraphStarter::TVertexType::TVertexType(Joint::Id prev, Joint::Id curr, double weight) noexcept
+  : m_prev(prev), m_curr(curr), m_weight(weight)
+{
+}
+
+bool IndexGraphStarter::TVertexType::operator==(TVertexType const & rhs) const noexcept
+{
+  return m_prev == rhs.m_prev && m_curr == rhs.m_curr && m_weight == rhs.m_weight;
+}
+
+bool IndexGraphStarter::TVertexType::operator<(TVertexType const & rhs) const noexcept
+{
+  if (m_curr != rhs.m_curr)
+    return m_curr < rhs.m_curr;
+  if (m_prev != rhs.m_prev)
+    return m_prev < rhs.m_prev;
+  return m_weight < rhs.m_weight;
+}
+
+string DebugPrint(IndexGraphStarter::TVertexType const & u)
+{
+  ostringstream os;
+  os << "Vertex [ prev:" << u.GetPrev() << ", curr:" << u.GetCurr() << " , weight:" << u.GetWeight()
+     << " ]";
+  return os.str();
+}
+
+// IndexGraphStarter -------------------------------------------------------------------------------
 IndexGraphStarter::IndexGraphStarter(IndexGraph & graph, RoadPoint const & startPoint,
                                      RoadPoint const & finishPoint)
   : m_fakeNextFeatureId(graph.GetNextFakeFeatureId())
@@ -62,6 +105,61 @@ m2::PointD const & IndexGraphStarter::GetPoint(RoadPoint const & rp)
   return GetPoint(FindFakeEdge(rp.GetFeatureId()).GetFrom());
 }
 
+void IndexGraphStarter::GetOutgoingEdgesList(TVertexType const & u, vector<TEdgeType> & edges)
+{
+  vector<JointEdge> jes;
+  GetEdgesList(u.GetCurr(), true /* isOutgoing */, jes);
+
+  edges.clear();
+  edges.reserve(jes.size());
+  for (auto const & je : jes)
+  {
+    TVertexType const v(u.GetCurr(), je.GetTarget(), je.GetWeight());
+    double const weight = v.GetWeight() + GetPenalties(u, v);
+    edges.emplace_back(v, weight);
+  }
+
+  if (u.GetCurr() == GetFinishJoint())
+    edges.emplace_back(GetFinishVertex(), 0 /* weight */);
+}
+
+void IndexGraphStarter::GetIngoingEdgesList(TVertexType const & u, vector<TEdgeType> & edges)
+{
+  vector<JointEdge> jes;
+  GetEdgesList(u.GetPrev(), false /* isOutgoing */, jes);
+
+  edges.clear();
+  edges.reserve(jes.size());
+  for (auto const & je : jes)
+  {
+    TVertexType const v(je.GetTarget(), u.GetPrev(), je.GetWeight());
+    double const weight = u.GetWeight() + GetPenalties(v, u);
+    edges.emplace_back(v, weight);
+  }
+
+  if (u.GetPrev() == GetStartJoint())
+    edges.emplace_back(GetStartVertex(), u.GetWeight());
+}
+
+uint32_t IndexGraphStarter::GetOriginalFeature(TVertexType const & vertex)
+{
+  RoadPoint rp0;
+  RoadPoint rp1;
+  FindPointsWithCommonFeature(vertex.GetPrev(), vertex.GetCurr(), rp0, rp1);
+  return rp0.GetFeatureId();
+}
+
+pair<RoadPoint, RoadPoint> IndexGraphStarter::GetOriginalEndPoints(TVertexType const & vertex)
+{
+  DirectedEdge const e(vertex.GetPrev(), vertex.GetCurr(), GetOriginalFeature(vertex));
+  DirectedEdge const p = m_graph.GetParent(e);
+
+  RoadPoint rp0;
+  RoadPoint rp1;
+  FindPointsWithCommonFeature(p.GetFrom(), p.GetTo(), rp0, rp1);
+  return make_pair(rp0, rp1);
+}
+
 RoadGeometry IndexGraphStarter::GetFakeRoadGeometry(uint32_t fakeFeatureId)
 {
   DirectedEdge const & e = FindFakeEdge(fakeFeatureId);
@@ -310,6 +408,34 @@ void IndexGraphStarter::AddFakeZeroLengthEdges(FakeJoint const & fj, EndPointTyp
   }
 }
 
+bool IndexGraphStarter::IsUTurn(TVertexType const & u, TVertexType const & v)
+{
+  if (u.GetCurr() != v.GetPrev())
+    return false;
+
+  auto const pu = GetOriginalEndPoints(u);
+  auto const pv = GetOriginalEndPoints(v);
+  if (pu.first.GetFeatureId() != pv.first.GetFeatureId())
+    return false;
+
+  auto const su = Compare(pu.first.GetPointId(), pu.second.GetPointId());
+  auto const sv = Compare(pv.first.GetPointId(), pv.second.GetPointId());
+  return su != 0 && sv != 0 && su != sv;
+}
+
+double IndexGraphStarter::GetPenalties(TVertexType const & u, TVertexType const & v)
+{
+  if (u == GetStartVertex() || u == GetFinishVertex())
+    return 0;
+  if (v == GetStartVertex() || v == GetFinishVertex())
+    return 0;
+
+  double penalty = 0;
+  if (IsUTurn(u, v))
+    penalty += m_graph.GetEstimator().GetUTurnPenalty();
+  return penalty;
+}
+
 // IndexGraphStarter::FakeJoint --------------------------------------------------------------------
 IndexGraphStarter::FakeJoint::FakeJoint(RoadPoint const & point, Joint::Id fakeId)
   : m_point(point), m_fakeId(fakeId), m_jointId(Joint::kInvalidId)
diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp
index 33759f569c..588019bcc8 100644
--- a/routing/index_graph_starter.hpp
+++ b/routing/index_graph_starter.hpp
@@ -5,6 +5,7 @@
 #include "routing/route_point.hpp"
 
 #include "std/set.hpp"
+#include "std/type_traits.hpp"
 #include "std/utility.hpp"
 #include "std/vector.hpp"
 
@@ -24,9 +25,79 @@ namespace routing
 class IndexGraphStarter final
 {
 public:
-  // AStarAlgorithm types aliases:
-  using TVertexType = Joint::Id;
-  using TEdgeType = JointEdge;
+  // IndexGraph is a G = (V, E), where V is a Joint, and E is a pair
+  // of joints (JointEdge). But this space is too restrictive, as it's
+  // hard to add penalties for left turns, U-turns, restrictions and
+  // so on. Therefore, we run A* algorithm on a different graph G' =
+  // (V', E'), where each vertex from V' is a pair of vertices
+  // (previous vertex, current vertex) from V.
+  //
+  // Following laws hold:
+  //
+  // 1. (s, s) in V', where s is the start joint.
+  //
+  // 2. (t, t) in V', where t is the finish joint.
+  //
+  // 3. If there is an edge (u, v) in E and edge (v, w) in E, then
+  // there is an edge from (u, v) to (v, w) in E', and the weight of
+  // this edge is the same as the weight of (v, w) from E.
+  //
+  // 4. If there is an edge (s, u) in E then there is an edge from (s,
+  // s) to (s, u) in E' and the weight of this edge is the same as the
+  // weigth of (s, u) from E.
+  //
+  // 5. If there is an edge (u, t) in E then there is an edge from (u,
+  // t) to (t, t) in E' of zero length.
+  //
+  // 6. (s, s) from V' and (t, t) from V' are start and finish
+  // vertices correspondingly.
+  //
+  // As one can see, vertices in the new space are actually edges from
+  // the original space.
+  //
+  // TODO (@bykoianko): replace this after join of DirectedEdge and
+  // JointEdge by a merged edge type.
+  class TVertexType final
+  {
+  public:
+    static_assert(is_integral<Joint::Id>::value, "Please, revisit noexcept specifiers below.");
+
+    TVertexType() = default;
+    TVertexType(Joint::Id prev, Joint::Id curr, double weight) noexcept;
+
+    bool operator==(TVertexType const & rhs) const noexcept;
+    bool operator<(TVertexType const & rhs) const noexcept;
+
+    inline Joint::Id GetPrev() const noexcept { return m_prev; }
+    inline Joint::Id GetCurr() const noexcept { return m_curr; }
+    inline double GetWeight() const noexcept { return m_weight; }
+
+  private:
+    // A joint we came from.
+    Joint::Id m_prev = Joint::kInvalidId;
+
+    // A joint we are currently on.
+    Joint::Id m_curr = Joint::kInvalidId;
+
+    // A weight between m_prev and m_curr.
+    double m_weight = 0.0;
+  };
+
+  class TEdgeType final
+  {
+  public:
+    TEdgeType(TVertexType const & target, double weight) noexcept
+      : m_target(target), m_weight(weight)
+    {
+    }
+
+    TVertexType GetTarget() const noexcept { return m_target; }
+    double GetWeight() const noexcept { return m_weight; }
+
+  private:
+    TVertexType const m_target;
+    double const m_weight;
+  };
 
   enum class EndPointType
   {
@@ -42,22 +113,30 @@ public:
   Joint::Id GetStartJoint() const { return m_start.m_jointId; }
   Joint::Id GetFinishJoint() const { return m_finish.m_jointId; }
 
-  m2::PointD const & GetPoint(Joint::Id jointId);
-  m2::PointD const & GetPoint(RoadPoint const & rp);
-
-  void GetOutgoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges)
+  TVertexType GetStartVertex() const
   {
-    GetEdgesList(jointId, true /* isOutgoing */, edges);
+    auto const s = GetStartJoint();
+    return TVertexType(s, s, 0.0 /* weight */);
   }
 
-  void GetIngoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges)
+  TVertexType GetFinishVertex() const
   {
-    GetEdgesList(jointId, false /* isOutgoing */, edges);
+    auto const t = GetFinishJoint();
+    return TVertexType(t, t, 0.0 /* weight */);
   }
 
-  double HeuristicCostEstimate(Joint::Id from, Joint::Id to)
+  m2::PointD const & GetPoint(Joint::Id jointId);
+  m2::PointD const & GetPoint(RoadPoint const & rp);
+
+  // Clears |edges| and fills with all outgoing edges from |u|.
+  void GetOutgoingEdgesList(TVertexType const & u, vector<TEdgeType> & edges);
+
+  // Clears |edges| and fills with all ingoing edges to |u|.
+  void GetIngoingEdgesList(TVertexType const & u, vector<TEdgeType> & edges);
+
+  double HeuristicCostEstimate(TVertexType const & from, TVertexType const & to)
   {
-    return m_graph.GetEstimator().CalcHeuristic(GetPoint(from), GetPoint(to));
+    return m_graph.GetEstimator().CalcHeuristic(GetPoint(from.GetCurr()), GetPoint(to.GetCurr()));
   }
 
   // Add intermediate points to route (those don't correspond to any joint).
@@ -134,6 +213,19 @@ private:
 
   void AddFakeZeroLengthEdges(FakeJoint const & fp, EndPointType endPointType);
 
+  // Returns true if there is a U-turn from |u| to |v|.
+  bool IsUTurn(TVertexType const & u, TVertexType const & v);
+
+  // Get's all possible penalties when passing from |u| to |v|.
+  double GetPenalties(TVertexType const & u, TVertexType const & v);
+
+  // Returns original (before restrictions) feature for a |vertex|.
+  uint32_t GetOriginalFeature(TVertexType const & vertex);
+
+  // Returns a pair of original (before restrictions) road points for
+  // a |vertex|.
+  pair<RoadPoint, RoadPoint> GetOriginalEndPoints(TVertexType const & vertex);
+
   // Edges which added to IndexGraph in IndexGraphStarter. Size of |m_fakeZeroLengthEdges| should be
   // relevantly small because some methods working with it have linear complexity.
   vector<DirectedEdge> m_fakeZeroLengthEdges;
@@ -144,4 +236,6 @@ private:
   FakeJoint m_start;
   FakeJoint m_finish;
 };
+
+string DebugPrint(IndexGraphStarter::TVertexType const & u);
 }  // namespace routing
diff --git a/routing/routing_tests/index_graph_tools.cpp b/routing/routing_tests/index_graph_tools.cpp
index 89ea259087..b1f32b15f2 100644
--- a/routing/routing_tests/index_graph_tools.cpp
+++ b/routing/routing_tests/index_graph_tools.cpp
@@ -46,13 +46,22 @@ AStarAlgorithm<IndexGraphStarter>::Result CalculateRoute(IndexGraphStarter & sta
                                                          vector<RoadPoint> & roadPoints)
 {
   AStarAlgorithm<IndexGraphStarter> algorithm;
-  RoutingResult<Joint::Id> routingResult;
+  RoutingResult<IndexGraphStarter::TVertexType> routingResult;
 
-  auto const resultCode = algorithm.FindPath(
-      starter, starter.GetStartJoint(), starter.GetFinishJoint(), routingResult, {}, {});
+  auto const resultCode = algorithm.FindPath(starter, starter.GetStartVertex(),
+                                             starter.GetFinishVertex(), routingResult, {}, {});
+
+  vector<Joint::Id> path;
+  for (auto const & u : routingResult.path)
+    path.emplace_back(u.GetCurr());
+  if (resultCode == AStarAlgorithm<IndexGraphStarter>::Result::OK && path.size() >= 2)
+  {
+    CHECK_EQUAL(path[path.size() - 1], path[path.size() - 2], ());
+    path.pop_back();
+  }
 
   vector<RoutePoint> routePoints;
-  starter.RedressRoute(routingResult.path, routePoints);
+  starter.RedressRoute(path, routePoints);
 
   roadPoints.clear();
   roadPoints.reserve(routePoints.size());
diff --git a/routing/routing_tests/restriction_test.cpp b/routing/routing_tests/restriction_test.cpp
index a5312785ae..c5b320fa4d 100644
--- a/routing/routing_tests/restriction_test.cpp
+++ b/routing/routing_tests/restriction_test.cpp
@@ -5,6 +5,8 @@
 #include "routing/car_model.hpp"
 #include "routing/geometry.hpp"
 
+#include "indexer/classificator_loader.hpp"
+
 #include "geometry/point2d.hpp"
 
 #include "std/unique_ptr.hpp"
@@ -30,11 +32,11 @@ void TestRoutes(vector<RoadPoint> const & starts, vector<RoadPoint> const & fini
 void EdgeTest(Joint::Id vertex, size_t expectedIntgoingNum, size_t expectedOutgoingNum,
               bool graphWithoutRestrictions, IndexGraph & graph)
 {
-  vector<IndexGraphStarter::TEdgeType> ingoing;
+  vector<routing::JointEdge> ingoing;
   graph.GetEdgeList(vertex, false /* is outgoing */, graphWithoutRestrictions, ingoing);
   TEST_EQUAL(ingoing.size(), expectedIntgoingNum, ());
 
-  vector<IndexGraphStarter::TEdgeType> outgoing;
+  vector<routing::JointEdge> outgoing;
   graph.GetEdgeList(vertex, true /* is outgoing */, graphWithoutRestrictions, outgoing);
   TEST_EQUAL(outgoing.size(), expectedOutgoingNum, ());
 }
@@ -86,6 +88,79 @@ UNIT_TEST(TriangularGraph)
   TestRouteSegments(starter, AStarAlgorithm<IndexGraphStarter>::Result::OK, expectedRoute);
 }
 
+//                             Finish
+//                               *
+//                               ^
+//                               |
+//                               F7
+//                               |
+//                               *
+//                               ^
+//                               |
+//                               F6
+//                               |
+// Start * -- F0 --> * -- F1 --> * <-- F2 --> * -- F3 --> *
+//                              | ^
+//                              | |
+//                             F4 F5
+//                              | |
+//                              ⌄ |
+//                               *
+unique_ptr<IndexGraph> BuildCrossGraph()
+{
+  unique_ptr<TestGeometryLoader> loader = make_unique<TestGeometryLoader>();
+  loader->AddRoad(0 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{-1.0, 0.0}, {0.0, 0.0}}));
+  loader->AddRoad(1 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{0.0, 0.0}, {1.0, 0.0}}));
+  loader->AddRoad(2 /* featureId */, false /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.0, 0.0}, {1.9999, 0.0}}));
+  loader->AddRoad(3 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.9999, 0.0}, {3.0, 0.0}}));
+  loader->AddRoad(4 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.0, 0.0}, {1.0, -1.0}}));
+  loader->AddRoad(5 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.0, -1.0}, {1.0, 0.0}}));
+  loader->AddRoad(6 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.0, 0.0}, {1.0, 1.0}}));
+  loader->AddRoad(7 /* featureId */, true /* oneWay */, 1.0 /* speed */,
+                  RoadGeometry::Points({{1.0, 1.0}, {1.0, 2.0}}));
+
+  vector<Joint> const joints = {
+      MakeJoint({{0, 1}, {1, 0}}), MakeJoint({{1, 1}, {2, 0}, {4, 0}, {5, 1}, {6, 0}}),
+      MakeJoint({{2, 1}, {3, 0}}), MakeJoint({{4, 1}, {5, 0}}), MakeJoint({{6, 1}, {7, 0}})};
+
+  traffic::TrafficCache const trafficCache;
+  unique_ptr<IndexGraph> graph =
+      make_unique<IndexGraph>(move(loader), CreateEstimator(trafficCache));
+  graph->Import(joints);
+  return graph;
+}
+
+UNIT_TEST(CrossGraph_UTurn)
+{
+  classificator::Load();
+  unique_ptr<IndexGraph> graph = BuildCrossGraph();
+
+  {
+    IndexGraphStarter starter(*graph, RoadPoint(0, 0) /* start */, RoadPoint(7, 1) /* finish */);
+    vector<m2::PointD> const expectedGeom = {
+        {-1.0 /* x */, 0.0 /* y */}, {0.0, 0.0}, {1.0, 0.0}, {1.0, 1.0}, {1.0, 2.0}};
+    TestRouteGeometry(starter, AStarAlgorithm<IndexGraphStarter>::Result::OK, expectedGeom);
+  }
+
+  RestrictionVec const restrictions = {
+      {Restriction::Type::No, {1 /* feature from */, 6 /* feature to */}}};
+  graph->ApplyRestrictions(restrictions);
+
+  {
+    IndexGraphStarter starter(*graph, RoadPoint(0, 0) /* start */, RoadPoint(7, 1) /* finish */);
+    vector<m2::PointD> const expectedGeom = {{-1.0, 0.0}, {0.0, 0.0}, {1.0, 0.0}, {1.0, -1.0},
+                                             {1.0, 0.0},  {1.0, 1.0}, {1.0, 2.0}};
+    TestRouteGeometry(starter, AStarAlgorithm<IndexGraphStarter>::Result::OK, expectedGeom);
+  }
+}
+
 // Route through triangular graph with feature 2 disabled.
 UNIT_CLASS_TEST(RestrictionTest, TriangularGraph_DisableF2)
 {
diff --git a/routing/single_mwm_router.cpp b/routing/single_mwm_router.cpp
index 7aae783f50..36f625df69 100644
--- a/routing/single_mwm_router.cpp
+++ b/routing/single_mwm_router.cpp
@@ -111,9 +111,11 @@ IRouter::ResultCode SingleMwmRouter::DoCalculateRoute(MwmSet::MwmId const & mwmI
   progress.Initialize(starter.GetPoint(start), starter.GetPoint(finish));
 
   uint32_t drawPointsStep = 0;
-  auto onVisitJunction = [&](Joint::Id const & from, Joint::Id const & to) {
-    m2::PointD const & pointFrom = starter.GetPoint(from);
-    m2::PointD const & pointTo = starter.GetPoint(to);
+  auto onVisitVertex = [&](IndexGraphStarter::TVertexType const & from,
+                           IndexGraphStarter::TVertexType const & to) {
+    m2::PointD const & pointFrom = starter.GetPoint(from.GetCurr());
+    m2::PointD const & pointTo = starter.GetPoint(to.GetCurr());
+
     auto const lastValue = progress.GetLastValue();
     auto const newValue = progress.GetProgressForBidirectedAlgo(pointFrom, pointTo);
     if (newValue - lastValue > kProgressInterval)
@@ -125,17 +127,44 @@ IRouter::ResultCode SingleMwmRouter::DoCalculateRoute(MwmSet::MwmId const & mwmI
 
   AStarAlgorithm<IndexGraphStarter> algorithm;
 
-  RoutingResult<Joint::Id> routingResult;
+  RoutingResult<IndexGraphStarter::TVertexType> routingResult;
   auto const resultCode =
-      algorithm.FindPathBidirectional(starter, starter.GetStartJoint(), starter.GetFinishJoint(),
-                                      routingResult, delegate, onVisitJunction);
+      algorithm.FindPathBidirectional(starter, starter.GetStartVertex(), starter.GetFinishVertex(),
+                                      routingResult, delegate, onVisitVertex);
 
   switch (resultCode)
   {
   case AStarAlgorithm<IndexGraphStarter>::Result::NoPath: return IRouter::RouteNotFound;
   case AStarAlgorithm<IndexGraphStarter>::Result::Cancelled: return IRouter::Cancelled;
   case AStarAlgorithm<IndexGraphStarter>::Result::OK:
-    if (!BuildRoute(mwmId, routingResult.path, delegate, starter, route))
+    // Because A* works in another space, where each vertex is
+    // actually a pair (previous vertex, current vertex), and start
+    // and finish vertices are (start joint, start joint), (finish
+    // joint, finish joint) correspondingly, we need to get back to
+    // the original space.
+
+    vector<Joint::Id> joints;
+    for (auto const & u : routingResult.path)
+      joints.emplace_back(u.GetCurr());
+
+    // If there are at least two points on the shortest path, then the
+    // last point is duplicated.  Imagine that the shortest path in
+    // the original space is: [s, u, v, t].  Then, in the another
+    // space, the shortest path will be: [(s, s), (s, u), (u, v), (v,
+    // t), (t, t)]. After taking second part of the vertices, the
+    // sequence is: [s, u, v, t, t], therefore, the last vertex is
+    // duplicated. On the other hand, if the shortest path in the
+    // original space is a single vertex [s] - a case when start and
+    // finish vertices are the same, in the extended space the
+    // shortest path is [(s, s)], and after taking the second part the
+    // sequence is [s] - exactly what do we need.
+    if (joints.size() >= 2)
+    {
+      CHECK_EQUAL(joints[joints.size() - 1], joints[joints.size() - 2], ());
+      joints.pop_back();
+    }
+
+    if (!BuildRoute(mwmId, joints, delegate, starter, route))
       return IRouter::InternalError;
     if (delegate.IsCancelled())
       return IRouter::Cancelled;