Improve turns calculation

3 files changed, 141 insertions, 14 deletions

diff --git a/routing/osrm_data_facade.hpp b/routing/osrm_data_facade.hpp
index 6c166d1e57..f70d8eeb0d 100644
--- a/routing/osrm_data_facade.hpp
+++ b/routing/osrm_data_facade.hpp
@@ -111,11 +111,9 @@ public:
     return res;
   }
 
-  //! TODO: Remove static variable
-  EdgeDataT & GetEdgeData(const EdgeID e, NodeID node)
+  EdgeDataT GetEdgeData(const EdgeID e, NodeID node)
   {
-    static EdgeDataT res;
-
+    EdgeDataT res;
 
     res.shortcut = m_shortcuts[e];
     res.id = res.shortcut ? (node - bits::ZigZagDecode(m_edgeId[m_shortcuts.rank(e)])) : 0;
diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp
index f1e1dbd0d3..9a0a2ef363 100644
--- a/routing/osrm_router.cpp
+++ b/routing/osrm_router.cpp
@@ -684,9 +684,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt
 
   return NoError;
 }
-m2::PointD OsrmRouter::GetPointForTurnAngle(const OsrmFtSegMapping::FtSeg &seg,
-    const FeatureType &ft, const m2::PointD &turnPnt,
-    size_t (*GetPndInd)(const size_t, const size_t, const size_t)) const
+m2::PointD OsrmRouter::GetPointForTurnAngle(OsrmFtSegMapping::FtSeg const &seg,
+                                            FeatureType const &ft, m2::PointD const &turnPnt,
+                                            size_t (*GetPndInd)(const size_t, const size_t, const size_t)) const
 {
   const size_t maxPntsNum = 5;
   const double maxDistMeter = 250.f;
@@ -698,10 +698,12 @@ m2::PointD OsrmRouter::GetPointForTurnAngle(const OsrmFtSegMapping::FtSeg &seg,
               ("GetPntForTurnAngle(). The start and the end pnt of a segment are too far from each other"));
   const size_t usedFtPntNum = min(maxPntsNum, segDist);
 
-  for(size_t i=1; i<= usedFtPntNum; ++i){
+  for (size_t i = 1; i <= usedFtPntNum; ++i)
+  {
     nextPnt = ft.GetPoint(GetPndInd(seg.m_pointStart, seg.m_pointEnd, i));
     curDist += MercatorBounds::DistanceOnEarth(pnt, nextPnt);
-    if(curDist > maxDistMeter){
+    if (curDist > maxDistMeter)
+    {
       return nextPnt;
     }
     pnt = nextPnt;
@@ -709,6 +711,93 @@ m2::PointD OsrmRouter::GetPointForTurnAngle(const OsrmFtSegMapping::FtSeg &seg,
   return nextPnt;
 }
 
+NodeID OsrmRouter::GetTurnTargetNode(NodeID src, NodeID trg, QueryEdge::EdgeData const & edgeData)
+{
+  ASSERT_NOT_EQUAL(src, SPECIAL_NODEID, ());
+  ASSERT_NOT_EQUAL(trg, SPECIAL_NODEID, ());
+  if (!edgeData.shortcut)
+    return trg;
+
+  ASSERT_LESS(edgeData.id, m_dataFacade.GetNumberOfNodes(), ());
+  EdgeID edge = SPECIAL_EDGEID;
+  QueryEdge::EdgeData d;
+  for (EdgeID e : m_dataFacade.GetAdjacentEdgeRange(edgeData.id))
+  {
+    if (m_dataFacade.GetTarget(e) == src )
+    {
+      d = m_dataFacade.GetEdgeData(e, edgeData.id);
+      if (d.backward)
+      {
+        edge = e;
+        break;
+      }
+    }
+  }
+
+  if (edge == SPECIAL_EDGEID)
+  {
+    for (EdgeID e : m_dataFacade.GetAdjacentEdgeRange(src))
+    {
+      if (m_dataFacade.GetTarget(e) == edgeData.id)
+      {
+        d = m_dataFacade.GetEdgeData(e, src);
+        if (d.forward)
+        {
+          edge = e;
+          break;
+        }
+      }
+    }
+  }
+  ASSERT_NOT_EQUAL(edge, SPECIAL_EDGEID, ());
+
+  if (d.shortcut)
+    return GetTurnTargetNode(src, edgeData.id, d);
+
+  return edgeData.id;
+}
+
+void OsrmRouter::GetPossibleTurns(NodeID node,
+                                  m2::PointD const & p1,
+                                  m2::PointD const & p,
+                                  uint32_t mwmId,
+                                  OsrmRouter::TurnCandidatesT & candidates)
+{
+
+  for (EdgeID e : m_dataFacade.GetAdjacentEdgeRange(node))
+  {
+    QueryEdge::EdgeData const data = m_dataFacade.GetEdgeData(e, node);
+    if (!data.forward)
+      continue;
+
+    NodeID trg = GetTurnTargetNode(node, m_dataFacade.GetTarget(e), data);
+    ASSERT_NOT_EQUAL(trg, SPECIAL_NODEID, ());
+
+    auto const range = m_mapping.GetSegmentsRange(trg);
+    OsrmFtSegMapping::FtSeg seg;
+    m_mapping.GetSegmentByIndex(range.first, seg);
+
+    FeatureType ft;
+    Index::FeaturesLoaderGuard loader(*m_pIndex, mwmId);
+    loader.GetFeature(seg.m_fid, ft);
+    ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
+
+    m2::PointD const p2 = ft.GetPoint(seg.m_pointStart < seg.m_pointEnd ? seg.m_pointStart + 1 : seg.m_pointStart - 1);
+    ASSERT_EQUAL(p, ft.GetPoint(seg.m_pointStart), ());
+
+    double a = my::RadToDeg(ang::AngleTo(p, p2) - ang::AngleTo(p, p1));
+    while (a < 0)
+      a += 360;
+
+    candidates.emplace_back(a, trg);
+  }
+
+  sort(candidates.begin(), candidates.end(), [](TurnCandidate const & t1, TurnCandidate const & t2) { return t1.m_angle < t2.m_angle; });
+  auto last = unique(candidates.begin(), candidates.end());
+  candidates.erase(last, candidates.end());
+}
+
+
 void OsrmRouter::GetTurnDirection(PathData const & node1,
                                   PathData const & node2,
                                   uint32_t mwmId, Route::TurnItem & turn)
@@ -737,11 +826,13 @@ void OsrmRouter::GetTurnDirection(PathData const & node1,
 
   m2::PointD p = ft1.GetPoint(seg1.m_pointEnd);
   m2::PointD p1 = GetPointForTurnAngle(seg1, ft1, p,
-                    [](const size_t start, const size_t end, const size_t i){
+                    [](const size_t start, const size_t end, const size_t i)
+                    {
                       return end > start ? end - i : end + i;
                     });
   m2::PointD p2 = GetPointForTurnAngle(seg2, ft2, p,
-                    [](const size_t start, const size_t end, const size_t i){
+                    [](const size_t start, const size_t end, const size_t i)
+                    {
                       return end > start ? start + i : start - i;
                     });
 
@@ -749,6 +840,18 @@ void OsrmRouter::GetTurnDirection(PathData const & node1,
   while (a < 0)
     a += 360;
 
+#ifdef _DEBUG
+  TurnCandidatesT nodes;
+  GetPossibleTurns(node1.node, p1, p, mwmId, nodes);
+
+  LOG(LDEBUG, ("Possible turns: ", nodes.size()));
+  for (size_t i = 0; i < nodes.size(); ++i)
+  {
+    TurnCandidate const &t = nodes[i];
+    LOG(LDEBUG, ("Angle:", t.m_angle, "Node:", t.m_node));
+  }
+#endif
+
   turn.m_turn = turns::NoTurn;
   if (a >= 23 && a < 67)
     turn.m_turn = turns::TurnSharpRight;
diff --git a/routing/osrm_router.hpp b/routing/osrm_router.hpp
index 942b806132..95c56aba0f 100644
--- a/routing/osrm_router.hpp
+++ b/routing/osrm_router.hpp
@@ -37,6 +37,24 @@ public:
   };
   typedef vector<FeatureGraphNode> FeatureGraphNodeVecT;
 
+  struct TurnCandidate
+  {
+    double m_angle;
+    NodeID m_node;
+
+    TurnCandidate(double a, NodeID n)
+      : m_angle(a), m_node(n)
+    {
+    }
+
+    bool operator == (TurnCandidate const & other) const
+    {
+      return m_node == other.m_node;
+    }
+  };
+  typedef vector<TurnCandidate> TurnCandidatesT;
+
+
   OsrmRouter(Index const * index, CountryFileFnT const & fn);
 
   virtual string GetName() const;
@@ -53,13 +71,21 @@ protected:
   void CalculateRouteAsync(ReadyCallback const & callback);
   ResultCode CalculateRouteImpl(m2::PointD const & startPt, m2::PointD const & startDr, m2::PointD const & finalPt, Route & route);
 
+private:
+  NodeID GetTurnTargetNode(NodeID src, NodeID trg, QueryEdge::EdgeData const & edgeData);
+
+  void GetPossibleTurns(NodeID node,
+                        m2::PointD const & p1,
+                        m2::PointD const & p,
+                        uint32_t mwmId,
+                        TurnCandidatesT & candidates);
+
   void GetTurnDirection(PathData const & node1,
                         PathData const & node2,
                         uint32_t mwmId, Route::TurnItem & turn);
   void FixupTurns(vector<m2::PointD> const & points, Route::TurnsT & turnsDir) const;
-private:
-  m2::PointD GetPointForTurnAngle(const OsrmFtSegMapping::FtSeg &seg,
-                                  const FeatureType &ft, const m2::PointD &turnPnt,
+  m2::PointD GetPointForTurnAngle(OsrmFtSegMapping::FtSeg const &seg,
+                                  FeatureType const &ft, m2::PointD const &turnPnt,
                                   size_t (*GetPndInd)(const size_t, const size_t, const size_t)) const;
 
   Index const * m_pIndex;