From 95632ac11fbc2f2aa16795d8c7f7e72aea9cfa16 Mon Sep 17 00:00:00 2001
From: Lev Dragunov <l.dragunov@corp.mail.ru>
Date: Tue, 17 Nov 2015 18:58:00 +0300
Subject: [PATCH] UTurn checker.

---
 routing/osrm_router.cpp     | 11 +++++++--
 routing/turns_generator.cpp | 48 +++++++++++++++++++++++++++++++++++++
 routing/turns_generator.hpp |  6 +++++
 3 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp
index feb77dca24..5497d0f79f 100644
--- a/routing/osrm_router.cpp
+++ b/routing/osrm_router.cpp
@@ -407,6 +407,7 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation(
     }
 
     // Annotate turns.
+    size_t skipTurnSegments = 0;
     for (size_t segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex)
     {
       auto const & loadedSegment = loadedSegments[segmentIndex];
@@ -415,13 +416,17 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation(
       double const nodeTimeSeconds = loadedSegment.m_weight * kOSRMWeightToSecondsMultiplier;
 
       // Turns information.
-      if (segmentIndex > 0 && !points.empty())
+      if (segmentIndex > 0 && !points.empty() && skipTurnSegments == 0)
       {
         turns::TurnItem turnItem;
         turnItem.m_index = static_cast<uint32_t>(points.size() - 1);
 
+        skipTurnSegments = CheckUTurnOnRoute(loadedSegments, segmentIndex, turnItem);
+
         turns::TurnInfo turnInfo(loadedSegments[segmentIndex - 1], loadedSegments[segmentIndex]);
-        turns::GetTurnDirection(*m_pIndex, *mapping, turnInfo, turnItem);
+
+        if (turnItem.m_turn == turns::TurnDirection::NoTurn)
+          turns::GetTurnDirection(*m_pIndex, *mapping, turnInfo, turnItem);
 
 #ifdef DEBUG
         double distMeters = 0.0;
@@ -443,6 +448,8 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation(
       }
 
       estimatedTime += nodeTimeSeconds;
+      if (skipTurnSegments > 0)
+        --skipTurnSegments;
 
       // Path geometry.
       points.insert(points.end(), loadedSegment.m_path.begin(), loadedSegment.m_path.end());
diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp
index 155b975d65..786c83b0f1 100644
--- a/routing/turns_generator.cpp
+++ b/routing/turns_generator.cpp
@@ -887,5 +887,53 @@ void GetTurnDirection(Index const & index, RoutingMapping & mapping, TurnInfo &
     return;
   }
 }
+
+size_t CheckUTurnOnRoute(vector<LoadedPathSegment> const & segments, size_t currentSegment, TurnItem & turn)
+{
+  size_t constexpr kUTurnLookAhead = 5;
+  double const kUTurnHeadingSensitivity = math::pi / 10.0;
+
+  ASSERT_GREATER(segments.size(), 1, ());
+  ASSERT_GREATER(currentSegment, 0, ());
+  auto const & masterSegment = segments[currentSegment - 1];
+  // Roundabout is not the UTurn.
+  if (masterSegment.m_onRoundabout)
+    return 0;
+  for (size_t i = 0; i< min(kUTurnLookAhead, segments.size()); ++i)
+  {
+    auto const & checkedSegment = segments[currentSegment + i];
+    if (checkedSegment.m_name == masterSegment.m_name &&
+        checkedSegment.m_highwayClass == masterSegment.m_highwayClass &&
+        checkedSegment.m_isLink == masterSegment.m_isLink && !checkedSegment.m_onRoundabout)
+    {
+      m2::PointD p1 = masterSegment.m_path.back() - masterSegment.m_path[masterSegment.m_path.size() - 2];
+      m2::PointD p2 = checkedSegment.m_path[1] -checkedSegment.m_path.front();
+      auto angle = ang::TwoVectorsAngle(m2::PointD::Zero(), p1, p2);
+      if (my::AlmostEqualAbs(angle, math::pi, kUTurnHeadingSensitivity))
+      {
+        if (i == 0)
+        {
+          turn.m_turn = TurnDirection::UTurnLeft;
+          return 0;
+        }
+        // Determine turn direction.
+        m2::PointD const junctionPoint = masterSegment.m_path.back();
+        m2::PointD const ingoingPoint = GetPointForTurn(masterSegment.m_path, junctionPoint,
+                                                        kMaxPointsCount, kMinDistMeters,
+                                                        GetIngoingPointIndex);
+        m2::PointD const outgoingPoint = GetPointForTurn(segments[currentSegment].m_path, junctionPoint,
+                                                         kMaxPointsCount, kMinDistMeters,
+                                                         GetOutgoingPointIndex);
+        if (PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, outgoingPoint) < 0)
+          turn.m_turn = TurnDirection::UTurnLeft;
+        else
+          turn.m_turn = TurnDirection::UTurnRight;
+        return ++i;
+      }
+    }
+  }
+
+  return 0;
+}
 }  // namespace turns
 }  // namespace routing
diff --git a/routing/turns_generator.hpp b/routing/turns_generator.hpp
index a53871b793..6c5f7fcec6 100644
--- a/routing/turns_generator.hpp
+++ b/routing/turns_generator.hpp
@@ -143,5 +143,11 @@ TurnDirection GetRoundaboutDirection(bool isIngoingEdgeRoundabout, bool isOutgoi
 void GetTurnDirection(Index const & index, RoutingMapping & mapping, turns::TurnInfo & turnInfo,
                       TurnItem & turn);
 
+/*!
+ * \brief Finds UTurn started from current segment and returns how many segments it lasts.
+ * Returns 0 otherwise.
+ */
+size_t CheckUTurnOnRoute(vector<LoadedPathSegment> const & segments, size_t currentSegment, TurnItem & turn);
+
 }  // namespace routing
 }  // namespace turns