make area() support concave polygon

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

common/autoware_universe_utils/include/autoware/universe_utils/geometry/alt_geometry.hpp

@@ -151,7 +151,9 @@ class ConvexPolygon2d : public Polygon2d
 };
 }  // namespace alt
 
-double area(const alt::ConvexPolygon2d & poly);
+double area(const alt::PointList2d & vertices);
+
+double area(const alt::Polygon2d & poly);
 
 std::optional<alt::ConvexPolygon2d> convex_hull(const alt::Points2d & points);
 

common/autoware_universe_utils/src/geometry/alt_geometry.cpp

@@ -140,16 +140,26 @@ std::optional<ConvexPolygon2d> ConvexPolygon2d::create(
 }
 }  // namespace alt
 
-double area(const alt::ConvexPolygon2d & poly)
+double area(const alt::PointList2d & vertices)
 {
-  const auto & vertices = poly.vertices();
+  double area_2 = 0.;
+  for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
+    area_2 += (*std::next(it)).cross(*it);
+  }
+
+  return area_2 / 2;
+}
+
+double area(const alt::Polygon2d & poly)
+{
+  const auto outer_area = area(poly.outer());
 
-  double area = 0.;
-  for (auto it = std::next(vertices.cbegin()); it != std::prev(vertices.cend(), 2); ++it) {
-    area += (*std::next(it) - vertices.front()).cross(*it - vertices.front()) / 2;
+  double inner_area = 0.;
+  for (const auto & inner : poly.inners()) {
+    inner_area += area(inner);
   }
 
-  return area;
+  return outer_area - inner_area;
 }
 
 std::optional<alt::ConvexPolygon2d> convex_hull(const alt::Points2d & points)

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/random_concave_polygon.hpp"
 #include "autoware/universe_utils/geometry/random_convex_polygon.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
 
@@ -760,6 +761,45 @@ TEST(alt_geometry, areaRand)
   }
 }
 
+TEST(alt_geometry, areaConcaveRand)
+{
+  std::vector<autoware::universe_utils::Polygon2d> polygons;
+  constexpr auto polygons_nb = 100;
+  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_area_ns = 0.0;
+    double alt_area_ns = 0.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) {
+      sw.tic();
+      const auto ground_truth = boost::geometry::area(polygons[i]);
+      ground_truth_area_ns += sw.toc();
+
+      const auto alt_poly = autoware::universe_utils::alt::Polygon2d::create(polygons[i]).value();
+      sw.tic();
+      const auto alt = autoware::universe_utils::area(alt_poly);
+      alt_area_ns += sw.toc();
+
+      if (std::abs(alt - ground_truth) > epsilon) {
+        std::cout << "Alt failed for the polygon: ";
+        std::cout << boost::geometry::wkt(polygons[i]) << std::endl;
+      }
+      EXPECT_NEAR(ground_truth, alt, epsilon);
+    }
+    std::printf("polygons_nb = %d, vertices = %ld\n", polygons_nb, vertices);
+    std::printf(
+      "\tArea:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n", ground_truth_area_ns / 1e6,
+      alt_area_ns / 1e6);
+  }
+}
+
 TEST(alt_geometry, convexHullRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;