index graph wrapper for start and finish

11 files changed, 360 insertions, 145 deletions

diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp
index 14c711c889..17c1abd2cf 100644
--- a/routing/astar_router.cpp
+++ b/routing/astar_router.cpp
@@ -7,6 +7,7 @@
 #include "routing/car_model.hpp"
 #include "routing/features_road_graph.hpp"
 #include "routing/index_graph.hpp"
+#include "routing/index_graph_starter.hpp"
 #include "routing/pedestrian_model.hpp"
 #include "routing/route.hpp"
 #include "routing/turns_generator.hpp"
@@ -28,15 +29,16 @@ size_t constexpr kMaxRoadCandidates = 6;
 float constexpr kProgressInterval = 2;
 uint32_t constexpr kDrawPointsPeriod = 10;
 
-vector<Junction> ConvertToJunctions(IndexGraph const & graph, vector<Joint::Id> const & joints)
+vector<Junction> ConvertToJunctions(IndexGraphStarter const & starter,
+                                    vector<Joint::Id> const & joints)
 {
   vector<RoadPoint> roadPoints;
-  graph.RedressRoute(joints, roadPoints);
+  starter.RedressRoute(joints, roadPoints);
 
   vector<Junction> junctions;
   junctions.reserve(roadPoints.size());
 
-  Geometry const & geometry = graph.GetGeometry();
+  Geometry const & geometry = starter.GetGraph().GetGeometry();
   // TODO: Use real altitudes for pedestrian and bicycle routing.
   for (RoadPoint const & point : roadPoints)
     junctions.emplace_back(geometry.GetPoint(point), feature::kDefaultAltitudeMeters);
@@ -109,14 +111,15 @@ IRouter::ResultCode AStarRouter::DoCalculateRoute(MwmSet::MwmId const & mwmId,
   if (!LoadIndex(mwmId, country, graph))
     return IRouter::RouteFileNotExist;
 
+  IndexGraphStarter starter(graph, start, finish);
+
   AStarProgress progress(0, 100);
   progress.Initialize(graph.GetGeometry().GetPoint(start), graph.GetGeometry().GetPoint(finish));
 
   uint32_t drawPointsStep = 0;
-  auto onVisitJunction = [&delegate, &progress, &graph, &drawPointsStep](Joint::Id const & from,
-                                                                         Joint::Id const & target) {
-    m2::PointD const & point = graph.GetPoint(from);
-    m2::PointD const & targetPoint = graph.GetPoint(target);
+  auto onVisitJunction = [&](Joint::Id const & from, Joint::Id const & target) {
+    m2::PointD const & point = starter.GetPoint(from);
+    m2::PointD const & targetPoint = starter.GetPoint(target);
     auto const lastValue = progress.GetLastValue();
     auto const newValue = progress.GetProgressForBidirectedAlgo(point, targetPoint);
     if (newValue - lastValue > kProgressInterval)
@@ -126,20 +129,19 @@ IRouter::ResultCode AStarRouter::DoCalculateRoute(MwmSet::MwmId const & mwmId,
     ++drawPointsStep;
   };
 
-  AStarAlgorithm<IndexGraph> algorithm;
+  AStarAlgorithm<IndexGraphStarter> algorithm;
 
   RoutingResult<Joint::Id> routingResult;
-  Joint::Id const startJoint = graph.InsertJoint(start);
-  Joint::Id const finishJoint = graph.InsertJoint(finish);
-  auto const resultCode = algorithm.FindPathBidirectional(graph, startJoint, finishJoint,
-                                                          routingResult, delegate, onVisitJunction);
+  auto const resultCode =
+      algorithm.FindPathBidirectional(starter, starter.GetStartJoint(), starter.GetFinishJoint(),
+                                      routingResult, delegate, onVisitJunction);
 
   switch (resultCode)
   {
-  case AStarAlgorithm<IndexGraph>::Result::NoPath: return IRouter::RouteNotFound;
-  case AStarAlgorithm<IndexGraph>::Result::Cancelled: return IRouter::Cancelled;
-  case AStarAlgorithm<IndexGraph>::Result::OK:
-    vector<Junction> path = ConvertToJunctions(graph, routingResult.path);
+  case AStarAlgorithm<IndexGraphStarter>::Result::NoPath: return IRouter::RouteNotFound;
+  case AStarAlgorithm<IndexGraphStarter>::Result::Cancelled: return IRouter::Cancelled;
+  case AStarAlgorithm<IndexGraphStarter>::Result::OK:
+    vector<Junction> path = ConvertToJunctions(starter, routingResult.path);
     ReconstructRoute(m_directionsEngine.get(), *m_roadGraph, delegate, path, route);
     return IRouter::NoError;
   }
diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp
index 8ae77dfc2f..6e3e0136c3 100644
--- a/routing/index_graph.cpp
+++ b/routing/index_graph.cpp
@@ -11,19 +11,11 @@ IndexGraph::IndexGraph(unique_ptr<GeometryLoader> loader, shared_ptr<EdgeEstimat
   ASSERT(m_estimator, ());
 }
 
-void IndexGraph::GetOutgoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges) const
+void IndexGraph::GetEdgesList(Joint::Id jointId, bool isOutgoing, vector<JointEdge> & edges) const
 {
-  GetEdgesList(jointId, true, edges);
-}
-
-void IndexGraph::GetIngoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges) const
-{
-  GetEdgesList(jointId, false, edges);
-}
-
-double IndexGraph::HeuristicCostEstimate(Joint::Id jointFrom, Joint::Id jointTo) const
-{
-  return m_estimator->CalcHeuristic(GetPoint(jointFrom), GetPoint(jointTo));
+  m_jointIndex.ForEachPoint(jointId, [this, &edges, isOutgoing](RoadPoint const & rp) {
+    AddNeighboringEdges(rp, isOutgoing, edges);
+  });
 }
 
 m2::PointD const & IndexGraph::GetPoint(Joint::Id jointId) const
@@ -48,54 +40,34 @@ Joint::Id IndexGraph::InsertJoint(RoadPoint const & rp)
   return jointId;
 }
 
-void IndexGraph::RedressRoute(vector<Joint::Id> const & route, vector<RoadPoint> & roadPoints) const
+bool IndexGraph::JointLaysOnRoad(Joint::Id jointId, uint32_t featureId) const
 {
-  if (route.size() < 2)
-  {
-    if (route.size() == 1)
-      roadPoints.emplace_back(m_jointIndex.GetPoint(route[0]));
+  bool result = false;
+  m_jointIndex.ForEachPoint(jointId, [&result, featureId](RoadPoint const & rp) {
+    if (rp.GetFeatureId() == featureId)
+      result = true;
+  });
+
+  return result;
+}
+
+inline void IndexGraph::AddNeighboringEdges(RoadPoint rp, bool isOutgoing,
+                                            vector<JointEdge> & edges) const
+{
+  RoadGeometry const & road = m_geometry.GetRoad(rp.GetFeatureId());
+  if (!road.IsRoad())
     return;
-  }
 
-  roadPoints.reserve(route.size() * 2);
+  bool const bidirectional = !road.IsOneWay();
+  if (!isOutgoing || bidirectional)
+    AddNeighboringEdge(road, rp, false /* forward */, edges);
 
-  for (size_t i = 0; i < route.size() - 1; ++i)
-  {
-    Joint::Id const prevJoint = route[i];
-    Joint::Id const nextJoint = route[i + 1];
-
-    RoadPoint rp0;
-    RoadPoint rp1;
-    m_jointIndex.FindPointsWithCommonFeature(prevJoint, nextJoint, rp0, rp1);
-    if (i == 0)
-      roadPoints.emplace_back(rp0);
-
-    uint32_t const featureId = rp0.GetFeatureId();
-    uint32_t const pointFrom = rp0.GetPointId();
-    uint32_t const pointTo = rp1.GetPointId();
-
-    if (pointFrom < pointTo)
-    {
-      for (uint32_t pointId = pointFrom + 1; pointId < pointTo; ++pointId)
-        roadPoints.emplace_back(featureId, pointId);
-    }
-    else if (pointFrom > pointTo)
-    {
-      for (uint32_t pointId = pointFrom - 1; pointId > pointTo; --pointId)
-        roadPoints.emplace_back(featureId, pointId);
-    }
-    else
-    {
-      MYTHROW(RootException,
-              ("Wrong equality pointFrom = pointTo =", pointFrom, ", featureId = ", featureId));
-    }
-
-    roadPoints.emplace_back(rp1);
-  }
+  if (isOutgoing || bidirectional)
+    AddNeighboringEdge(road, rp, true /* forward */, edges);
 }
 
 inline void IndexGraph::AddNeighboringEdge(RoadGeometry const & road, RoadPoint rp, bool forward,
-                                           vector<TEdgeType> & edges) const
+                                           vector<JointEdge> & edges) const
 {
   pair<Joint::Id, uint32_t> const & neighbor = m_roadIndex.FindNeighbor(rp, forward);
   if (neighbor.first != Joint::kInvalidId)
@@ -105,22 +77,17 @@ inline void IndexGraph::AddNeighboringEdge(RoadGeometry const & road, RoadPoint
   }
 }
 
-inline void IndexGraph::GetEdgesList(Joint::Id jointId, bool isOutgoing,
-                                     vector<TEdgeType> & edges) const
+void IndexGraph::AddDirectEdge(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo,
+                               Joint::Id target, bool forward, vector<JointEdge> & edges) const
 {
-  edges.clear();
-
-  m_jointIndex.ForEachPoint(jointId, [this, &edges, isOutgoing](RoadPoint const & rp) {
-    RoadGeometry const & road = m_geometry.GetRoad(rp.GetFeatureId());
-    if (!road.IsRoad())
-      return;
+  RoadGeometry const & road = m_geometry.GetRoad(featureId);
+  if (!road.IsRoad())
+    return;
 
-    bool const bidirectional = !road.IsOneWay();
-    if (!isOutgoing || bidirectional)
-      AddNeighboringEdge(road, rp, false /* forward */, edges);
+  if (road.IsOneWay() && forward != (pointFrom < pointTo))
+    return;
 
-    if (isOutgoing || bidirectional)
-      AddNeighboringEdge(road, rp, true /* forward */, edges);
-  });
+  double const distance = m_estimator->CalcEdgesWeight(road, pointFrom, pointTo);
+  edges.emplace_back(target, distance);
 }
 }  // namespace routing
diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp
index 38aae8410e..8c535fb94f 100644
--- a/routing/index_graph.hpp
+++ b/routing/index_graph.hpp
@@ -36,30 +36,32 @@ private:
 class IndexGraph final
 {
 public:
-  // AStarAlgorithm types aliases:
-  using TVertexType = Joint::Id;
-  using TEdgeType = JointEdge;
-
   IndexGraph() = default;
   explicit IndexGraph(unique_ptr<GeometryLoader> loader, shared_ptr<EdgeEstimator> estimator);
 
-  // AStarAlgorithm<TGraph> overloads:
-  void GetOutgoingEdgesList(Joint::Id vertex, vector<JointEdge> & edges) const;
-  void GetIngoingEdgesList(Joint::Id vertex, vector<JointEdge> & edges) const;
-  double HeuristicCostEstimate(Joint::Id from, Joint::Id to) const;
-
+  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; }
   m2::PointD const & GetPoint(Joint::Id jointId) const;
-  size_t GetNumRoads() const { return m_roadIndex.GetSize(); }
-  size_t GetNumJoints() const { return m_jointIndex.GetNumJoints(); }
-  size_t GetNumPoints() const { return m_jointIndex.GetNumPoints(); }
+  uint32_t GetNumRoads() const { return m_roadIndex.GetSize(); }
+  uint32_t GetNumJoints() const { return m_jointIndex.GetNumJoints(); }
+  uint32_t GetNumPoints() const { return m_jointIndex.GetNumPoints(); }
   void Import(vector<Joint> const & joints);
   Joint::Id InsertJoint(RoadPoint const & rp);
+  bool JointLaysOnRoad(Joint::Id jointId, uint32_t featureId) const;
 
-  // Add intermediate points to route (those don't correspond to any joint).
-  //
-  // Also convert joint ids to RoadPoints.
-  void RedressRoute(vector<Joint::Id> const & route, vector<RoadPoint> & roadPoints) const;
+  template <typename F>
+  void ForEachPoint(Joint::Id jointId, F && f) const
+  {
+    m_jointIndex.ForEachPoint(jointId, f);
+  }
 
   template <class Sink>
   void Serialize(Sink & sink) const
@@ -78,8 +80,7 @@ public:
 
 private:
   void AddNeighboringEdge(RoadGeometry const & road, RoadPoint rp, bool forward,
-                          vector<TEdgeType> & edges) const;
-  void GetEdgesList(Joint::Id jointId, bool forward, vector<TEdgeType> & edges) const;
+                          vector<JointEdge> & edges) const;
 
   Geometry m_geometry;
   shared_ptr<EdgeEstimator> m_estimator;
diff --git a/routing/index_graph_starter.cpp b/routing/index_graph_starter.cpp
new file mode 100644
index 0000000000..7b1f5658cd
--- /dev/null
+++ b/routing/index_graph_starter.cpp
@@ -0,0 +1,156 @@
+#include "routing/index_graph_starter.hpp"
+
+namespace routing
+{
+IndexGraphStarter::IndexGraphStarter(IndexGraph const & graph, RoadPoint startPoint,
+                                     RoadPoint finishPoint)
+  : m_graph(graph)
+  , m_startPoint(startPoint)
+  , m_finishPoint(finishPoint)
+  , m_startImplant(graph.GetNumJoints())
+  , m_finishImplant(graph.GetNumJoints() + 1)
+  , m_startJoint(CalcStartJoint())
+  , m_finishJoint(CalcFinishJoint())
+{
+}
+
+Joint::Id IndexGraphStarter::CalcStartJoint() const
+{
+  Joint::Id const jointId = m_graph.GetJointId(m_startPoint);
+  if (jointId == Joint::kInvalidId)
+    return m_startImplant;
+
+  return jointId;
+}
+
+Joint::Id IndexGraphStarter::CalcFinishJoint() const
+{
+  if (m_startPoint == m_finishPoint)
+    return CalcStartJoint();
+
+  Joint::Id const jointId = m_graph.GetJointId(m_finishPoint);
+  if (jointId == Joint::kInvalidId)
+    return m_finishImplant;
+
+  return jointId;
+}
+
+void IndexGraphStarter::GetEdgesList(Joint::Id jointId, bool isOutgoing,
+                                     vector<JointEdge> & edges) const
+{
+  edges.clear();
+
+  if (jointId == m_startImplant)
+  {
+    m_graph.AddNeighboringEdges(m_startPoint, isOutgoing, edges);
+
+    if (FinishIsImplant() && m_startPoint.GetFeatureId() == m_finishPoint.GetFeatureId())
+      m_graph.AddDirectEdge(m_startPoint.GetFeatureId(), m_startPoint.GetPointId(),
+                            m_finishPoint.GetPointId(), m_finishJoint, isOutgoing, edges);
+
+    return;
+  }
+
+  if (jointId == m_finishImplant)
+  {
+    m_graph.AddNeighboringEdges(m_finishPoint, isOutgoing, edges);
+
+    if (StartIsImplant() && m_startPoint.GetFeatureId() == m_finishPoint.GetFeatureId())
+      m_graph.AddDirectEdge(m_finishPoint.GetFeatureId(), m_finishPoint.GetPointId(),
+                            m_startPoint.GetPointId(), m_startJoint, isOutgoing, edges);
+
+    return;
+  }
+
+  m_graph.GetEdgesList(jointId, isOutgoing, edges);
+
+  if (StartIsImplant() && m_graph.JointLaysOnRoad(jointId, m_startPoint.GetFeatureId()))
+  {
+    vector<JointEdge> startEdges;
+    m_graph.AddNeighboringEdges(m_startPoint, !isOutgoing, startEdges);
+    for (JointEdge const & edge : startEdges)
+    {
+      if (edge.GetTarget() == jointId)
+        edges.emplace_back(m_startJoint, edge.GetWeight());
+    }
+  }
+
+  if (FinishIsImplant() && m_graph.JointLaysOnRoad(jointId, m_finishPoint.GetFeatureId()))
+  {
+    vector<JointEdge> finishEdges;
+    m_graph.AddNeighboringEdges(m_finishPoint, !isOutgoing, finishEdges);
+    for (JointEdge const & edge : finishEdges)
+    {
+      if (edge.GetTarget() == jointId)
+        edges.emplace_back(m_finishJoint, edge.GetWeight());
+    }
+  }
+}
+
+void IndexGraphStarter::RedressRoute(vector<Joint::Id> const & route,
+                                     vector<RoadPoint> & roadPoints) const
+{
+  if (route.size() < 2)
+  {
+    if (route.size() == 1)
+      roadPoints.emplace_back(m_startPoint);
+    return;
+  }
+
+  roadPoints.reserve(route.size() * 2);
+
+  for (size_t i = 0; i < route.size() - 1; ++i)
+  {
+    Joint::Id const prevJoint = route[i];
+    Joint::Id const nextJoint = route[i + 1];
+
+    RoadPoint rp0;
+    RoadPoint rp1;
+    FindPointsWithCommonFeature(prevJoint, nextJoint, rp0, rp1);
+    if (i == 0)
+      roadPoints.emplace_back(rp0);
+
+    uint32_t const featureId = rp0.GetFeatureId();
+    uint32_t const pointFrom = rp0.GetPointId();
+    uint32_t const pointTo = rp1.GetPointId();
+
+    if (pointFrom < pointTo)
+    {
+      for (uint32_t pointId = pointFrom + 1; pointId < pointTo; ++pointId)
+        roadPoints.emplace_back(featureId, pointId);
+    }
+    else if (pointFrom > pointTo)
+    {
+      for (uint32_t pointId = pointFrom - 1; pointId > pointTo; --pointId)
+        roadPoints.emplace_back(featureId, pointId);
+    }
+    else
+    {
+      MYTHROW(RootException,
+              ("Wrong equality pointFrom = pointTo =", pointFrom, ", featureId = ", featureId));
+    }
+
+    roadPoints.emplace_back(rp1);
+  }
+}
+
+void IndexGraphStarter::FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1,
+                                                    RoadPoint & result0, RoadPoint & result1) const
+{
+  bool found = false;
+
+  ForEachPoint(jointId0, [&](RoadPoint const & rp0) {
+    ForEachPoint(jointId1, [&](RoadPoint const & rp1) {
+      if (rp0.GetFeatureId() == rp1.GetFeatureId() && !found)
+      {
+        result0 = rp0;
+        result1 = rp1;
+        found = true;
+      }
+    });
+  });
+
+  if (!found)
+    MYTHROW(RootException, ("Can't find common feature for joints", jointId0, jointId1));
+}
+}  // namespace routing
diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp
new file mode 100644
index 0000000000..104b4bd30a
--- /dev/null
+++ b/routing/index_graph_starter.hpp
@@ -0,0 +1,96 @@
+#pragma once
+
+#include "routing/index_graph.hpp"
+#include "routing/joint.hpp"
+
+namespace routing
+{
+// The problem:
+// IndexGraph contains only road points connected in joints.
+// So it is possible IndexGraph doesn't contain start and finish.
+//
+// IndexGraphStarter implants start and finish for AStarAlgorithm.
+//
+class IndexGraphStarter final
+{
+public:
+  // AStarAlgorithm types aliases:
+  using TVertexType = Joint::Id;
+  using TEdgeType = JointEdge;
+
+  IndexGraphStarter(IndexGraph const & graph, RoadPoint startPoint, RoadPoint finishPoint);
+
+  IndexGraph const & GetGraph() const { return m_graph; }
+  Joint::Id GetStartJoint() const { return m_startJoint; }
+  Joint::Id GetFinishJoint() const { return m_finishJoint; }
+
+  m2::PointD const & GetPoint(Joint::Id jointId) const
+  {
+    if (jointId == m_startImplant)
+      return m_graph.GetGeometry().GetPoint(m_startPoint);
+
+    if (jointId == m_finishImplant)
+      return m_graph.GetGeometry().GetPoint(m_finishPoint);
+
+    return m_graph.GetPoint(jointId);
+  }
+
+  void GetOutgoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges) const
+  {
+    GetEdgesList(jointId, true, edges);
+  }
+
+  void GetIngoingEdgesList(Joint::Id jointId, vector<JointEdge> & edges) const
+  {
+    GetEdgesList(jointId, false, edges);
+  }
+
+  void GetEdgesList(Joint::Id jointId, bool isOutgoing, vector<JointEdge> & edges) const;
+
+  double HeuristicCostEstimate(Joint::Id from, Joint::Id to) const
+  {
+    return m_graph.CalcHeuristic(GetPoint(from), GetPoint(to));
+  }
+
+  // Add intermediate points to route (those don't correspond to any joint).
+  //
+  // Also convert joint ids to RoadPoints.
+  void RedressRoute(vector<Joint::Id> const & route, vector<RoadPoint> & roadPoints) const;
+
+private:
+  Joint::Id CalcStartJoint() const;
+  Joint::Id CalcFinishJoint() const;
+
+  bool StartIsImplant() const { return m_startJoint == m_startImplant; }
+  bool FinishIsImplant() const { return m_finishJoint == m_finishImplant; }
+
+  template <typename F>
+  void ForEachPoint(Joint::Id jointId, F && f) const
+  {
+    if (jointId == m_startImplant)
+    {
+      f(m_startPoint);
+      return;
+    }
+
+    if (jointId == m_finishImplant)
+    {
+      f(m_finishPoint);
+      return;
+    }
+
+    m_graph.ForEachPoint(jointId, f);
+  }
+
+  void FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, RoadPoint & result0,
+                                   RoadPoint & result1) const;
+
+  IndexGraph const & m_graph;
+  RoadPoint const m_startPoint;
+  RoadPoint const m_finishPoint;
+  Joint::Id const m_startImplant;
+  Joint::Id const m_finishImplant;
+  Joint::Id const m_startJoint;
+  Joint::Id const m_finishJoint;
+};
+}  // namespace routing
diff --git a/routing/joint_index.cpp b/routing/joint_index.cpp
index f8bbb2a680..2efd1c5bb8 100644
--- a/routing/joint_index.cpp
+++ b/routing/joint_index.cpp
@@ -15,34 +15,12 @@ void JointIndex::AppendToJoint(Joint::Id jointId, RoadPoint rp)
   m_dynamicJoints[jointId].AddPoint(rp);
 }
 
-void JointIndex::FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1,
-                                             RoadPoint & result0, RoadPoint & result1) const
-{
-  bool found = false;
-
-  ForEachPoint(jointId0, [&](RoadPoint const & rp0) {
-    ForEachPoint(jointId1, [&](RoadPoint const & rp1) {
-      if (rp0.GetFeatureId() == rp1.GetFeatureId() && !found)
-      {
-        result0 = rp0;
-        result1 = rp1;
-        found = true;
-      }
-    });
-  });
-
-  if (!found)
-    MYTHROW(RootException, ("Can't find common feature for joints", jointId0, jointId1));
-}
-
 void JointIndex::Build(RoadIndex const & roadIndex, uint32_t numJoints)
 {
-  // +2 is reserved space for start and finish.
   // + 1 is protection for 'End' method from out of bounds.
   // Call End(numJoints-1) requires more size, so add one more item.
   // Therefore m_offsets.size() == numJoints + 1,
   // And m_offsets.back() == m_points.size()
-  m_offsets.reserve(numJoints + 1 + 2);
   m_offsets.assign(numJoints + 1, 0);
 
   // Calculate shifted sizes.
@@ -68,8 +46,6 @@ void JointIndex::Build(RoadIndex const & roadIndex, uint32_t numJoints)
     prevSum = sum;
   }
 
-  // +2 is reserved space for start and finish
-  m_points.reserve(sum + 2);
   m_points.resize(sum);
 
   // Now fill points, m_offsets[nextId] is current incrementing begin.
diff --git a/routing/joint_index.hpp b/routing/joint_index.hpp
index d54f4004c0..0bc95a6b3b 100644
--- a/routing/joint_index.hpp
+++ b/routing/joint_index.hpp
@@ -16,8 +16,8 @@ class JointIndex final
 {
 public:
   // Read comments in Build method about -1.
-  size_t GetNumJoints() const { return m_offsets.size() - 1; }
-  size_t GetNumPoints() const { return m_points.size(); }
+  uint32_t GetNumJoints() const { return m_offsets.size() - 1; }
+  uint32_t GetNumPoints() const { return m_points.size(); }
   RoadPoint GetPoint(Joint::Id jointId) const { return m_points[Begin(jointId)]; }
 
   template <typename F>
@@ -36,8 +36,6 @@ public:
   }
 
   void Build(RoadIndex const & roadIndex, uint32_t numJoints);
-  void FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, RoadPoint & result0,
-                                   RoadPoint & result1) const;
   Joint::Id InsertJoint(RoadPoint const & rp);
   void AppendToJoint(Joint::Id jointId, RoadPoint rp);
 
diff --git a/routing/road_index.hpp b/routing/road_index.hpp
index e64f9bf474..1ceb9214aa 100644
--- a/routing/road_index.hpp
+++ b/routing/road_index.hpp
@@ -5,6 +5,7 @@
 #include "coding/reader.hpp"
 #include "coding/write_to_sink.hpp"
 
+#include "std/algorithm.hpp"
 #include "std/cstdint.hpp"
 #include "std/unordered_map.hpp"
 #include "std/utility.hpp"
@@ -45,13 +46,25 @@ public:
 
   pair<Joint::Id, uint32_t> FindNeighbor(uint32_t pointId, bool forward) const
   {
-    int32_t const step = forward ? 1 : -1;
+    uint32_t const size = static_cast<uint32_t>(m_jointIds.size());
 
-    for (uint32_t i = pointId + step; i < m_jointIds.size(); i += step)
+    if (forward)
     {
-      Joint::Id const jointId = m_jointIds[i];
-      if (jointId != Joint::kInvalidId)
-        return make_pair(jointId, i);
+      for (uint32_t i = pointId + 1; i < size; ++i)
+      {
+        Joint::Id const jointId = m_jointIds[i];
+        if (jointId != Joint::kInvalidId)
+          return make_pair(jointId, i);
+      }
+    }
+    else
+    {
+      for (uint32_t i = min(pointId, size) - 1; i < size; --i)
+      {
+        Joint::Id const jointId = m_jointIds[i];
+        if (jointId != Joint::kInvalidId)
+          return make_pair(jointId, i);
+      }
     }
 
     return make_pair(Joint::kInvalidId, 0);
diff --git a/routing/road_point.hpp b/routing/road_point.hpp
index ca580f0f1a..c613bca31f 100644
--- a/routing/road_point.hpp
+++ b/routing/road_point.hpp
@@ -19,6 +19,11 @@ public:
 
   uint32_t GetPointId() const { return m_pointId; }
 
+  bool operator==(RoadPoint const & rp) const
+  {
+    return m_featureId == rp.m_featureId && m_pointId == rp.m_pointId;
+  }
+
 private:
   uint32_t m_featureId;
   uint32_t m_pointId;
diff --git a/routing/routing.pro b/routing/routing.pro
index 94f52611d7..b3c500cfc7 100644
--- a/routing/routing.pro
+++ b/routing/routing.pro
@@ -28,6 +28,7 @@ SOURCES += \
     features_road_graph.cpp \
     geometry.cpp \
     index_graph.cpp \
+    index_graph_starter.cpp \
     joint.cpp \
     joint_index.cpp \
     nearest_edge_finder.cpp \
@@ -76,6 +77,7 @@ HEADERS += \
     features_road_graph.hpp \
     geometry.hpp \
     index_graph.hpp \
+    index_graph_starter.hpp \
     joint.hpp \
     joint_index.hpp \
     loaded_path_segment.hpp \
diff --git a/routing/routing_tests/index_graph_test.cpp b/routing/routing_tests/index_graph_test.cpp
index c1991977f2..22d1b7ad77 100644
--- a/routing/routing_tests/index_graph_test.cpp
+++ b/routing/routing_tests/index_graph_test.cpp
@@ -4,6 +4,7 @@
 #include "routing/car_model.hpp"
 #include "routing/edge_estimator.hpp"
 #include "routing/index_graph.hpp"
+#include "routing/index_graph_starter.hpp"
 
 #include "geometry/point2d.hpp"
 
@@ -66,21 +67,22 @@ shared_ptr<EdgeEstimator> CreateEstimator()
   return CreateCarEdgeEstimator(make_shared<CarModelFactory>()->GetVehicleModel());
 }
 
-void TestRoute(IndexGraph & graph, RoadPoint const & start, RoadPoint const & finish,
+void TestRoute(IndexGraph const & graph, RoadPoint const & start, RoadPoint const & finish,
                size_t expectedLength)
 {
   LOG(LINFO, ("Test route", start.GetFeatureId(), ",", start.GetPointId(), "=>",
               finish.GetFeatureId(), ",", finish.GetPointId()));
 
-  AStarAlgorithm<IndexGraph> algorithm;
+  AStarAlgorithm<IndexGraphStarter> algorithm;
   RoutingResult<Joint::Id> routingResult;
 
-  AStarAlgorithm<IndexGraph>::Result const resultCode = algorithm.FindPath(
-      graph, graph.InsertJoint(start), graph.InsertJoint(finish), routingResult, {}, {});
-  vector<RoadPoint> roadPoints;
-  graph.RedressRoute(routingResult.path, roadPoints);
+  IndexGraphStarter starter(graph, start, finish);
+  auto const resultCode = algorithm.FindPath(starter, starter.GetStartJoint(),
+                                             starter.GetFinishJoint(), routingResult, {}, {});
+  TEST_EQUAL(resultCode, AStarAlgorithm<IndexGraphStarter>::Result::OK, ());
 
-  TEST_EQUAL(resultCode, AStarAlgorithm<IndexGraph>::Result::OK, ());
+  vector<RoadPoint> roadPoints;
+  starter.RedressRoute(routingResult.path, roadPoints);
   TEST_EQUAL(roadPoints.size(), expectedLength, ());
 }
 
@@ -88,10 +90,7 @@ void TestEdges(IndexGraph const & graph, Joint::Id jointId,
                vector<Joint::Id> const & expectedTargets, bool forward)
 {
   vector<JointEdge> edges;
-  if (forward)
-    graph.GetOutgoingEdgesList(jointId, edges);
-  else
-    graph.GetIngoingEdgesList(jointId, edges);
+  graph.GetEdgesList(jointId, forward, edges);
 
   vector<Joint::Id> targets;
   for (JointEdge const & edge : edges)