[geometry] Circle on earth

8 files changed, 283 insertions, 0 deletions

diff --git a/geometry/CMakeLists.txt b/geometry/CMakeLists.txt
index 3ddcd864e5..dfcb0b2c5c 100644
--- a/geometry/CMakeLists.txt
+++ b/geometry/CMakeLists.txt
@@ -15,6 +15,8 @@ set(
   calipers_box.cpp
   calipers_box.hpp
   cellid.hpp
+  circle_on_earth.cpp
+  circle_on_earth.hpp
   clipping.cpp
   clipping.hpp
   convex_hull.cpp
diff --git a/geometry/circle_on_earth.cpp b/geometry/circle_on_earth.cpp
new file mode 100644
index 0000000000..300aa068bf
--- /dev/null
+++ b/geometry/circle_on_earth.cpp
@@ -0,0 +1,73 @@
+#include "geometry/circle_on_earth.hpp"
+
+#include "geometry/distance_on_sphere.hpp"
+#include "geometry/mercator.hpp"
+#include "geometry/point3d.hpp"
+
+#include "base/assert.hpp"
+
+namespace
+{
+std::vector<m3::PointD> CreateCircleOnNorth(double radiusMeters, double angleStepDegree)
+{
+  double const angle = radiusMeters / ms::kEarthRadiusMeters;
+  double const circleRadiusMeters = ms::kEarthRadiusMeters * sin(angle);
+
+  double const z = ms::kEarthRadiusMeters * cos(angle);
+
+  std::vector<m3::PointD> result;
+  double const stepRad = base::DegToRad(angleStepDegree);
+  for (double angleRad = 0; angleRad < 2 * math::pi; angleRad += stepRad)
+  {
+    result.emplace_back(circleRadiusMeters * cos(angleRad),
+                        circleRadiusMeters * sin(angleRad),
+                        z);
+  }
+  return result;
+}
+
+ms::LatLon FromEarth3dToSpherical(m3::PointD const & vec)
+{
+  ASSERT(base::AlmostEqualAbs(vec.Length(), ms::kEarthRadiusMeters, 1e-5),
+         (vec.Length(), ms::kEarthRadiusMeters));
+
+  double sinLatRad = vec.z / ms::kEarthRadiusMeters;
+  sinLatRad = base::Clamp(sinLatRad, -1.0, 1.0);
+  double const cosLatRad = std::sqrt(1 - sinLatRad * sinLatRad);
+  CHECK(-1.0 <= cosLatRad && cosLatRad <= 1.0, (cosLatRad));
+
+  double const latRad = asin(sinLatRad);
+  double sinLonRad = vec.y / ms::kEarthRadiusMeters / cosLatRad;
+  sinLonRad = base::Clamp(sinLonRad, -1.0, 1.0);
+  double lonRad = asin(sinLonRad);
+  if (vec.y > 0 && vec.x < 0)
+    lonRad = math::pi - lonRad;
+  else if (vec.y < 0 && vec.x < 0)
+    lonRad = -(math::pi - fabs(lonRad));
+
+  auto const lat = base::RadToDeg(latRad);
+  auto const lon = base::RadToDeg(lonRad);
+
+  return {lat, lon};
+}
+}  // namespace
+
+namespace ms
+{
+std::vector<m2::PointD> CreateCircleGeometryOnEarth(ms::LatLon const & center, double radiusMeters,
+                                                    double angleStepDegree)
+{
+  auto const circleOnNorth = CreateCircleOnNorth(radiusMeters, angleStepDegree);
+  std::vector<m2::PointD> result;
+  for (auto const & point3d : circleOnNorth)
+  {
+    auto const rotateByLat = point3d.RotateAroundY(90.0 - center.m_lat);
+    auto const rotateByLon = rotateByLat.RotateAroundZ(center.m_lon);
+
+    auto const latlonRotated = FromEarth3dToSpherical(rotateByLon);
+    result.emplace_back(mercator::FromLatLon(latlonRotated));
+  }
+
+  return result;
+}
+}  // namespace ms
diff --git a/geometry/circle_on_earth.hpp b/geometry/circle_on_earth.hpp
new file mode 100644
index 0000000000..e366394b89
--- /dev/null
+++ b/geometry/circle_on_earth.hpp
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "geometry/latlon.hpp"
+#include "geometry/point2d.hpp"
+
+#include <vector>
+
+namespace ms
+{
+std::vector<m2::PointD> CreateCircleGeometryOnEarth(ms::LatLon const & center, double radiusMeters,
+                                                    double angleStepDegree);
+}  // namespace ms
diff --git a/geometry/geometry_tests/CMakeLists.txt b/geometry/geometry_tests/CMakeLists.txt
index 6e7f43ad2e..a223b5082f 100644
--- a/geometry/geometry_tests/CMakeLists.txt
+++ b/geometry/geometry_tests/CMakeLists.txt
@@ -11,6 +11,7 @@ set(
   bounding_box_tests.cpp
   calipers_box_tests.cpp
   cellid_test.cpp
+  circle_on_earth_tests.cpp
   clipping_test.cpp
   common_test.cpp
   convex_hull_tests.cpp
diff --git a/geometry/geometry_tests/circle_on_earth_tests.cpp b/geometry/geometry_tests/circle_on_earth_tests.cpp
new file mode 100644
index 0000000000..2f4f5fa702
--- /dev/null
+++ b/geometry/geometry_tests/circle_on_earth_tests.cpp
@@ -0,0 +1,82 @@
+#include "testing/testing.hpp"
+
+#include "geometry/distance_on_sphere.hpp"
+#include "geometry/point2d.hpp"
+#include "geometry/mercator.hpp"
+#include "geometry/circle_on_earth.hpp"
+
+#include "base/math.hpp"
+
+#include <sstream>
+#include <vector>
+
+namespace
+{
+void TestGeometryAlmostEqualAbs(std::vector<m2::PointD> const & geometry,
+                                std::vector<m2::PointD> const & answer,
+                                double eps)
+{
+  TEST_EQUAL(geometry.size(), answer.size(), ());
+
+  for (size_t i = 0; i < geometry.size(); ++i)
+    TEST_ALMOST_EQUAL_ABS(geometry[i], answer[i], eps, ());
+}
+
+void TestGeometryAlmostEqualMeters(std::vector<m2::PointD> const & geometry,
+                                      std::vector<m2::PointD> const & answer,
+                                      double epsMeters)
+{
+  TEST_EQUAL(geometry.size(), answer.size(), ());
+
+  for (size_t i = 0; i < geometry.size(); ++i)
+    TEST_LESS(mercator::DistanceOnEarth(geometry[i], answer[i]), epsMeters, ());
+}
+}  // namespace
+
+UNIT_TEST(CircleOnEarth)
+{
+  ms::LatLon const center(90.0 /* lat */, 0.0 /* lon */);
+  double const radiusMeters = 2.0 * math::pi * ms::kEarthRadiusMeters / 4.0;
+  auto const geometry =
+      ms::CreateCircleGeometryOnEarth(center, radiusMeters, 90.0 /* angleStepDegree */);
+
+  std::vector<m2::PointD> const result = {
+      mercator::FromLatLon(0.0, 0.0),
+      mercator::FromLatLon(0.0, 90.0),
+      mercator::FromLatLon(0.0, 180.0),
+      mercator::FromLatLon(0.0, -90.0)
+  };
+
+  TestGeometryAlmostEqualAbs(geometry, result, 1e-9 /* eps */);
+}
+
+UNIT_TEST(CircleOnEarthEquator)
+{
+  ms::LatLon const center(0.0 /* lat */, 0.0 /* lon */);
+  auto const point = mercator::FromLatLon(center);
+
+  auto constexpr kRadiusMeters = 10000.0;
+  double const kRadiusMercator = mercator::MetersToMercator(kRadiusMeters);
+  auto constexpr kAngleStepDegree = 30.0;
+  auto constexpr kN = static_cast<size_t>(360.0 / kAngleStepDegree);
+
+  std::vector<m2::PointD> result;
+  result.reserve(kN);
+  auto constexpr kStepRad = base::DegToRad(kAngleStepDegree);
+  double angleSumRad = 0.0;
+  double angleRad = -math::pi2;
+  while (angleSumRad < 2 * math::pi)
+  {
+    angleSumRad += kStepRad;
+    result.emplace_back(point.x + kRadiusMercator * cos(angleRad),
+                        point.y + kRadiusMercator * sin(angleRad));
+    angleRad += kStepRad;
+    if (angleRad > 2 * math::pi)
+      angleRad -= 2 * math::pi;
+  }
+
+  auto const geometry =
+      ms::CreateCircleGeometryOnEarth(center, kRadiusMeters, kAngleStepDegree);
+
+  TestGeometryAlmostEqualMeters(geometry, result, 20.0 /* epsMeters */);
+}
diff --git a/geometry/geometry_tests/point3d_tests.cpp b/geometry/geometry_tests/point3d_tests.cpp
index 713ba361ce..8898d2c5d5 100644
--- a/geometry/geometry_tests/point3d_tests.cpp
+++ b/geometry/geometry_tests/point3d_tests.cpp
@@ -42,3 +42,59 @@ UNIT_TEST(Point3d_CrossProduct_3)
 
   TEST_EQUAL(m3::CrossProduct(p1, p2), m3::Point<int>(61, -21, -36), ());
 }
+
+UNIT_TEST(Point3d_RotateX_1)
+{
+  m3::PointD p(0.0, 1.0, 0.0);
+  auto const rotated = p.RotateAroundX(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(0.0, 0.0, 1.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateX_2)
+{
+  m3::PointD p(1.0, 2.0, 3.0);
+  auto const rotated = p.RotateAroundX(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(1.0, -3.0, 2.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateY_1)
+{
+  m3::PointD p(1.0, 0.0, 0.0);
+  auto const rotated = p.RotateAroundY(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(0.0, 0.0, -1.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateY_2)
+{
+  m3::PointD p(1.0, 2.0, 3.0);
+  auto const rotated = p.RotateAroundY(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(3.0, 2.0, -1.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateZ_1)
+{
+  m3::PointD p(1.0, 0.0, 0.0);
+  auto const rotated = p.RotateAroundZ(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(0.0, 1.0, 0.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateZ_2)
+{
+  m3::PointD p(1.0, 2.0, 3.0);
+  auto const rotated = p.RotateAroundZ(90.0);
+  TEST_ALMOST_EQUAL_ABS(rotated, m3::PointD(-2.0, 1.0, 3.0), 1e-10, ());
+}
+
+UNIT_TEST(Point3d_RotateXYZ)
+{
+  m3::PointD p(1.0, 1.0, 1.0);
+  auto const rotatedFirst = p.RotateAroundZ(-45.0);
+
+  TEST_ALMOST_EQUAL_ABS(rotatedFirst, m3::PointD(std::sqrt(2.0), 0.0, 1.0), 1e-10, ());
+
+  double const angleDegree = base::RadToDeg(acos(rotatedFirst.z / rotatedFirst.Length()));
+
+  auto const north = rotatedFirst.RotateAroundY(-angleDegree);
+  TEST_ALMOST_EQUAL_ABS(north, m3::PointD(0.0, 0.0, std::sqrt(3.0)), 1e-10, ());
+
+}
diff --git a/geometry/point3d.hpp b/geometry/point3d.hpp
index 470c8807e4..f7551e6f9d 100644
--- a/geometry/point3d.hpp
+++ b/geometry/point3d.hpp
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "base/math.hpp"
+
 #include <cmath>
 #include <sstream>
 #include <string>
@@ -15,6 +17,10 @@ public:
 
   T Length() const { return std::sqrt(x * x + y * y + z * z); }
 
+  Point RotateAroundX(double angleDegree) const;
+  Point RotateAroundY(double angleDegree) const;
+  Point RotateAroundZ(double angleDegree) const;
+
   bool operator==(Point<T> const & rhs) const;
 
   T x;
@@ -23,6 +29,39 @@ public:
 };
 
 template <typename T>
+Point<T> Point<T>::RotateAroundX(double angleDegree) const
+{
+  double const angleRad = base::DegToRad(angleDegree);
+  Point<T> res;
+  res.x = x;
+  res.y = y * cos(angleRad) - z * sin(angleRad);
+  res.z = y * sin(angleRad) + z * cos(angleRad);
+  return res;
+}
+
+template <typename T>
+Point<T> Point<T>::RotateAroundY(double angleDegree) const
+{
+  double const angleRad = base::DegToRad(angleDegree);
+  Point<T> res;
+  res.x = x * cos(angleRad) + z * sin(angleRad);
+  res.y = y;
+  res.z = -x * sin(angleRad) + z * cos(angleRad);
+  return res;
+}
+
+template <typename T>
+Point<T> Point<T>::RotateAroundZ(double angleDegree) const
+{
+  double const angleRad = base::DegToRad(angleDegree);
+  Point<T> res;
+  res.x = x * cos(angleRad) - y * sin(angleRad);
+  res.y = x * sin(angleRad) + y * cos(angleRad);
+  res.z = z;
+  return res;
+}
+
+template <typename T>
 bool Point<T>::operator==(m3::Point<T> const & rhs) const
 {
   return x == rhs.x && y == rhs.y && z == rhs.z;
@@ -54,3 +93,13 @@ std::string DebugPrint(Point<T> const & p)
   return out.str();
 }
 }  // namespace m3
+
+namespace base
+{
+template <typename T>
+bool AlmostEqualAbs(m3::Point<T> const & p1, m3::Point<T> const & p2, double const & eps)
+{
+  return base::AlmostEqualAbs(p1.x, p2.x, eps) && base::AlmostEqualAbs(p1.y, p2.y, eps) &&
+         base::AlmostEqualAbs(p1.z, p2.z, eps);
+}
+}  // namespace base
diff --git a/xcode/geometry/geometry.xcodeproj/project.pbxproj b/xcode/geometry/geometry.xcodeproj/project.pbxproj
index 26eab53446..7194a5eee4 100644
--- a/xcode/geometry/geometry.xcodeproj/project.pbxproj
+++ b/xcode/geometry/geometry.xcodeproj/project.pbxproj
@@ -25,7 +25,9 @@
 		4403990E2359CA8D0098EB7F /* mercator_test.cpp in Sources */ = {isa = PBXBuildFile; fileRef = E92D8BD01C15AF9100A98D17 /* mercator_test.cpp */; };
 		440CF0C2236734820017C2A8 /* area_on_earth.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 440CF0C0236734820017C2A8 /* area_on_earth.cpp */; };
 		440CF0C3236734820017C2A8 /* area_on_earth.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 440CF0C1236734820017C2A8 /* area_on_earth.hpp */; };
+		4435C7882372BB5600B4358C /* circle_on_earth.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 4435C7872372BB5500B4358C /* circle_on_earth.hpp */; };
 		446531E2234DEEF3000C348F /* point3d.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 446531E1234DEEF3000C348F /* point3d.hpp */; };
+		448425732372BB3F00C6AD04 /* circle_on_earth.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 448425722372BB3F00C6AD04 /* circle_on_earth.cpp */; };
 		45A2D9BE1F7526E6003310A0 /* bounding_box.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 45A2D9B71F7526E6003310A0 /* bounding_box.cpp */; };
 		45A2D9BF1F7526E6003310A0 /* bounding_box.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 45A2D9B81F7526E6003310A0 /* bounding_box.hpp */; };
 		45A2D9C01F7526E6003310A0 /* calipers_box.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 45A2D9B91F7526E6003310A0 /* calipers_box.cpp */; };
@@ -139,7 +141,9 @@
 		39B2B9721FB4681400AB85A1 /* line2d.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = line2d.hpp; sourceTree = "<group>"; };
 		440CF0C0236734820017C2A8 /* area_on_earth.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = area_on_earth.cpp; sourceTree = "<group>"; };
 		440CF0C1236734820017C2A8 /* area_on_earth.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = area_on_earth.hpp; sourceTree = "<group>"; };
+		4435C7872372BB5500B4358C /* circle_on_earth.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = circle_on_earth.hpp; sourceTree = "<group>"; };
 		446531E1234DEEF3000C348F /* point3d.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = point3d.hpp; sourceTree = "<group>"; };
+		448425722372BB3F00C6AD04 /* circle_on_earth.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = circle_on_earth.cpp; sourceTree = "<group>"; };
 		44A4BF17235732A5005857C4 /* point3d_tests.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = point3d_tests.cpp; sourceTree = "<group>"; };
 		45A2D9B71F7526E6003310A0 /* bounding_box.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = bounding_box.cpp; sourceTree = "<group>"; };
 		45A2D9B81F7526E6003310A0 /* bounding_box.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = bounding_box.hpp; sourceTree = "<group>"; };
@@ -319,6 +323,8 @@
 		675344931A3F684600A0A8C3 /* geometry */ = {
 			isa = PBXGroup;
 			children = (
+				4435C7872372BB5500B4358C /* circle_on_earth.hpp */,
+				448425722372BB3F00C6AD04 /* circle_on_earth.cpp */,
 				D53836332366DAF3007E7EDB /* oblate_spheroid.cpp */,
 				D53836342366DAF3007E7EDB /* oblate_spheroid.hpp */,
 				440CF0C0236734820017C2A8 /* area_on_earth.cpp */,
@@ -417,6 +423,7 @@
 				675344BF1A3F687400A0A8C3 /* avg_vector.hpp in Headers */,
 				675344D31A3F687400A0A8C3 /* simplification.hpp in Headers */,
 				675344CC1A3F687400A0A8C3 /* rect_intersect.hpp in Headers */,
+				4435C7882372BB5600B4358C /* circle_on_earth.hpp in Headers */,
 				675344C71A3F687400A0A8C3 /* packer.hpp in Headers */,
 				675344C41A3F687400A0A8C3 /* distance_on_sphere.hpp in Headers */,
 				440CF0C3236734820017C2A8 /* area_on_earth.hpp in Headers */,
@@ -571,6 +578,7 @@
 				45A2D9C01F7526E6003310A0 /* calipers_box.cpp in Sources */,
 				344A713C1F3DA07000B8DDB8 /* nearby_points_sweeper.cpp in Sources */,
 				347F33701C454205009758CC /* triangle2d.cpp in Sources */,
+				448425732372BB3F00C6AD04 /* circle_on_earth.cpp in Sources */,
 				675344BC1A3F687400A0A8C3 /* angles.cpp in Sources */,
 				6741AAA01BF35476002C974C /* latlon.cpp in Sources */,
 				E92D8BD41C15AFAA00A98D17 /* mercator.cpp in Sources */,