added and::GetShortestDistance and tests for it.

2 files changed, 50 insertions, 0 deletions

diff --git a/geometry/angles.hpp b/geometry/angles.hpp
index 39285b7d27..7aac2b0dd4 100644
--- a/geometry/angles.hpp
+++ b/geometry/angles.hpp
@@ -75,6 +75,37 @@ namespace ang
     return atan2(p2.y - p1.y, p2.x - p1.x);
   }
 
+  inline double RadToDegree(double rad)
+  {
+    return rad / math::pi * 180.0;
+  }
+
+  inline double DegreeToRad(double degree)
+  {
+    return degree / 180.0 * math::pi;
+  }
+
+  inline double GetShortestDistance(double rad1, double rad2)
+  {
+    double period = 2 * math::pi;
+    rad1 = fmod(rad1, period);
+    rad2 = fmod(rad2, period);
+
+    double res = 0;
+
+    if (abs(rad1 - rad2) > math::pi)
+    {
+      if (rad1 > rad2)
+        res = 2 * math::pi - (rad1 - rad2);
+      else
+        res = - 2 * math::pi + (rad2 - rad1);
+    }
+    else
+      res = rad2 - rad1;
+
+    return res;
+  }
+
   inline double GetMiddleAngle(double a1, double a2)
   {
     double ang = (a1 + a2) / 2.0;
diff --git a/geometry/geometry_tests/angle_test.cpp b/geometry/geometry_tests/angle_test.cpp
index c59bd66276..a762aa0a5e 100644
--- a/geometry/geometry_tests/angle_test.cpp
+++ b/geometry/geometry_tests/angle_test.cpp
@@ -52,3 +52,22 @@ UNIT_TEST(Average)
   TEST(is_equal_angle(ang::GetMiddleAngle(arr2[0], arr2[1]), 0.0), ());
   check_avg(arr2, ARRAY_SIZE(arr2), 0.0);
 }
+
+namespace
+{
+  bool is_equal(double val0, double val1, double eps)
+  {
+    return fabs(val0 - val1) < eps;
+  }
+}
+
+UNIT_TEST(ShortestDistance)
+{
+  double const eps = 1.0E-3;
+
+  TEST(is_equal(ang::GetShortestDistance(0, math::pi), math::pi, eps), ());
+  TEST(is_equal(ang::GetShortestDistance(0, math::pi + 1), -math::pi + 1, eps), ());
+
+  TEST(is_equal(ang::GetShortestDistance(math::pi - 1, 0), -math::pi + 1, eps), ());
+  TEST(is_equal(ang::GetShortestDistance(math::pi + 1, 0), math::pi - 1, eps), ());
+}