From b967eb4f071051b5fc8bb5fcc6f6d2fc99816c24 Mon Sep 17 00:00:00 2001
From: mitukou1109 <mitukou1109@gmail.com>
Date: Fri, 27 Sep 2024 16:03:05 +0900
Subject: [PATCH] make covered_by() support concave polygon

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>
---
 .../universe_utils/geometry/alt_geometry.hpp  |  2 +
 .../src/geometry/alt_geometry.cpp             | 13 ++++
 .../test/src/geometry/test_alt_geometry.cpp   | 68 +++++++++++++++++++
 3 files changed, 83 insertions(+)

diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp
index 59f965ebef205..0203f7a54e3df 100644
--- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp
+++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp
@@ -161,6 +161,8 @@ void correct(alt::Polygon2d & poly);
 
 bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
 
+bool covered_by(const alt::Point2d & point, const alt::Polygon2d & poly);
+
 bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
 
 double distance(
diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp
index bf0711838bf3f..22e21e6265b56 100644
--- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp
+++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/universe_utils/geometry/alt_geometry.hpp"
 
+#include "autoware/universe_utils/geometry/ear_clipping.hpp"
+
 namespace autoware::universe_utils
 {
 // Alternatives for Boost.Geometry ----------------------------------------------------------------
@@ -294,6 +296,17 @@ bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
   return winding_number != 0;
 }
 
+bool covered_by(const alt::Point2d & point, const alt::Polygon2d & poly)
+{
+  for (const auto & triangle : triangulate(poly)) {
+    if (covered_by(point, triangle)) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2)
 {
   if (equals(poly1, poly2)) {
diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp
index 5d069c599fc2e..6d8fa3695c4e9 100644
--- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp
+++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "autoware/universe_utils/geometry/alt_geometry.hpp"
+#include "autoware/universe_utils/geometry/ear_clipping.hpp"
 #include "autoware/universe_utils/geometry/random_concave_polygon.hpp"
 #include "autoware/universe_utils/geometry/random_convex_polygon.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
@@ -915,6 +916,73 @@ TEST(alt_geometry, coveredByRand)
   }
 }
 
+TEST(alt_geometry, coveredByConcaveRand)
+{
+  std::vector<autoware::universe_utils::Polygon2d> polygons;
+  constexpr auto polygons_nb = 500;
+  constexpr auto max_vertices = 10;
+  constexpr auto max_values = 1000;
+
+  autoware::universe_utils::StopWatch<std::chrono::nanoseconds, std::chrono::nanoseconds> sw;
+  for (auto vertices = 4UL; vertices < max_vertices; ++vertices) {
+    double ground_truth_covered_ns = 0.0;
+    double ground_truth_not_covered_ns = 0.0;
+    double alt_covered_ns = 0.0;
+    double alt_not_covered_ns = 0.0;
+    int covered_count = 0;
+
+    polygons.clear();
+    for (auto i = 0; i < polygons_nb; ++i) {
+      polygons.push_back(autoware::universe_utils::random_concave_polygon(vertices, max_values));
+    }
+    for (auto i = 0UL; i < polygons.size(); ++i) {
+      for (const auto & point : polygons[i].outer()) {
+        for (auto j = 0UL; j < polygons.size(); ++j) {
+          sw.tic();
+          const auto ground_truth = boost::geometry::covered_by(point, polygons[j]);
+          if (ground_truth) {
+            ++covered_count;
+            ground_truth_covered_ns += sw.toc();
+          } else {
+            ground_truth_not_covered_ns += sw.toc();
+          }
+
+          const auto alt_point = autoware::universe_utils::alt::Point2d(point);
+          const auto alt_poly =
+            autoware::universe_utils::alt::Polygon2d::create(polygons[j]).value();
+          sw.tic();
+          const auto alt = autoware::universe_utils::covered_by(alt_point, alt_poly);
+          if (alt) {
+            alt_covered_ns += sw.toc();
+          } else {
+            alt_not_covered_ns += sw.toc();
+          }
+
+          if (ground_truth != alt) {
+            std::cout << "Alt failed for the point and polygon: ";
+            std::cout << boost::geometry::wkt(point) << boost::geometry::wkt(polygons[j])
+                      << std::endl;
+          }
+          EXPECT_EQ(ground_truth, alt);
+        }
+      }
+    }
+    std::printf(
+      "polygons_nb = %d, vertices = %ld, %d / %ld covered pairs\n", polygons_nb, vertices,
+      covered_count, polygons_nb * vertices * polygons_nb);
+    std::printf(
+      "\tCovered:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      ground_truth_covered_ns / 1e6, alt_covered_ns / 1e6);
+    std::printf(
+      "\tNot covered:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      ground_truth_not_covered_ns / 1e6, alt_not_covered_ns / 1e6);
+    std::printf(
+      "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      (ground_truth_not_covered_ns + ground_truth_covered_ns) / 1e6,
+      (alt_not_covered_ns + alt_covered_ns) / 1e6);
+  }
+}
+
 TEST(alt_geometry, disjointRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;