From 4796f801c854bb443998ca111915a04d635d168b Mon Sep 17 00:00:00 2001
From: Vladimir Byko-Ianko <v.bykoianko@corp.mail.ru>
Date: Thu, 18 Dec 2014 16:12:56 +0300
Subject: [PATCH] Matching bearing while route matching

---
 platform/location.hpp                      | 17 +++++++++
 platform/platform_tests/location_test.cpp  | 40 ++++++++++++++++++++++
 platform/platform_tests/platform_tests.pro |  1 +
 routing/route.cpp                          | 24 +++++++++++++
 routing/route.hpp                          |  1 +
 5 files changed, 83 insertions(+)
 create mode 100644 platform/platform_tests/location_test.cpp

diff --git a/platform/location.hpp b/platform/location.hpp
index 978bf8aeab..576c95a3ee 100644
--- a/platform/location.hpp
+++ b/platform/location.hpp
@@ -5,6 +5,7 @@
 #include "../routing/turns.hpp"
 
 #include "../std/string.hpp"
+#include "../std/cmath.hpp"
 
 
 namespace location
@@ -73,6 +74,22 @@ namespace location
     return lon != 0. && lon < 180. && lon > -180.;
   }
 
+
+  // Convert angle (in degrees counterclockwise from X) to bearing ([0, 360) clockwise from the north)
+  static inline double AngleToBearing(double a)
+  {
+    double reverseAng = fmod(-a + 90, 360.);
+    if (reverseAng < 0)
+      reverseAng += 360.;
+    return reverseAng;
+  }
+
+  // Convert bearing (in degrees clockwise from the north) to angle ([0, 360) counterclockwise from X)
+  static inline double BearingToAngle(double a)
+  {
+    return AngleToBearing(a);
+  }
+
   class FollowingInfo
   {
   public:
diff --git a/platform/platform_tests/location_test.cpp b/platform/platform_tests/location_test.cpp
new file mode 100644
index 0000000000..3c79dba77c
--- /dev/null
+++ b/platform/platform_tests/location_test.cpp
@@ -0,0 +1,40 @@
+#include "../../testing/testing.hpp"
+
+#include "../location.hpp"
+
+
+UNIT_TEST(IsLatValid)
+{
+  TEST(location::IsLatValid(35.), ());
+  TEST(location::IsLatValid(-35.), ());
+  TEST(!location::IsLatValid(0.), ());
+  TEST(!location::IsLatValid(100.), ());
+  TEST(!location::IsLatValid(-99.), ());
+}
+
+UNIT_TEST(IsLonValid)
+{
+  TEST(location::IsLonValid(135.), ());
+  TEST(location::IsLonValid(-35.), ());
+  TEST(!location::IsLonValid(0.), ());
+  TEST(!location::IsLonValid(200.), ());
+  TEST(!location::IsLonValid(-199.), ());
+}
+
+UNIT_TEST(AngleToBearing)
+{
+  TEST_ALMOST_EQUAL(location::AngleToBearing(0.), 90., ());
+  TEST_ALMOST_EQUAL(location::AngleToBearing(30.), 60., ());
+  TEST_ALMOST_EQUAL(location::AngleToBearing(100.), 350., ());
+  TEST_ALMOST_EQUAL(location::AngleToBearing(370.), 80., ());
+  TEST_ALMOST_EQUAL(location::AngleToBearing(-370.), 100., ());
+}
+
+UNIT_TEST(BearingToAngle)
+{
+  TEST_ALMOST_EQUAL(location::BearingToAngle(0.), 90., ());
+  TEST_ALMOST_EQUAL(location::BearingToAngle(30.), 60., ());
+  TEST_ALMOST_EQUAL(location::BearingToAngle(100.), 350., ());
+  TEST_ALMOST_EQUAL(location::BearingToAngle(370.), 80., ());
+  TEST_ALMOST_EQUAL(location::AngleToBearing(-370.), 100., ());
+}
diff --git a/platform/platform_tests/platform_tests.pro b/platform/platform_tests/platform_tests.pro
index 1f4a5a5054..df71478ffc 100644
--- a/platform/platform_tests/platform_tests.pro
+++ b/platform/platform_tests/platform_tests.pro
@@ -34,3 +34,4 @@ SOURCES += \
     downloader_test.cpp \
     video_timer_test.cpp \
     apk_test.cpp \
+    location_test.cpp \
diff --git a/routing/route.cpp b/routing/route.cpp
index 1b185e6a0a..1ce41365c2 100644
--- a/routing/route.cpp
+++ b/routing/route.cpp
@@ -5,6 +5,8 @@
 #include "../platform/location.hpp"
 
 #include "../geometry/distance_on_sphere.hpp"
+#include "../geometry/angles.hpp"
+#include "../geometry/point2d.hpp"
 
 #include "../base/logging.hpp"
 
@@ -172,6 +174,27 @@ double Route::GetCurrentSqDistance(m2::PointD const & pt) const
   return pt.SquareLength(m_current.m_pt);
 }
 
+double Route::GetPolySegAngle(size_t ind) const
+{
+  size_t const polySz = m_poly.GetSize();
+
+  if (ind + 1 >= polySz)
+  {
+    ASSERT(false, ());
+    return 0;
+  }
+
+  m2::PointD const p1 = m_poly.GetPoint(ind);
+  m2::PointD p2;
+  size_t i = ind + 1;
+  do
+  {
+    p2 = m_poly.GetPoint(i);
+  }
+  while (m2::AlmostEqual(p1, p2) && ++i < polySz);
+  return (i == polySz) ? 0 : my::RadToDeg(ang::AngleTo(p1, p2));
+}
+
 void Route::MatchLocationToRoute(location::GpsInfo & location) const
 {
   if (m_current.IsValid())
@@ -183,6 +206,7 @@ void Route::MatchLocationToRoute(location::GpsInfo & location) const
     {
       location.m_latitude = MercatorBounds::YToLat(m_current.m_pt.y);
       location.m_longitude = MercatorBounds::XToLon(m_current.m_pt.x);
+      location.m_bearing = location::AngleToBearing(GetPolySegAngle(m_current.m_ind));
     }
   }
 }
diff --git a/routing/route.hpp b/routing/route.hpp
index aaed16efe1..3329006dd0 100644
--- a/routing/route.hpp
+++ b/routing/route.hpp
@@ -113,6 +113,7 @@ private:
 
   /// Call this fucnction when geometry have changed.
   void Update();
+  double GetPolySegAngle(size_t ind) const;
 
 private:
   friend string DebugPrint(Route const & r);