make distance() support concave polygon

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

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

@@ -168,7 +168,8 @@ bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & p
 double distance(
   const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
 
-double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
+template <typename PolyT>
+double distance(const alt::Point2d & point, const PolyT & poly);
 
 std::optional<alt::ConvexPolygon2d> envelope(const alt::Polygon2d & poly);
 

common/autoware_universe_utils/src/geometry/alt_geometry.cpp

@@ -351,14 +351,15 @@ double distance(
   }
 }
 
-double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
+template <typename PolyT>
+double distance(const alt::Point2d & point, const PolyT & poly)
 {
   if (covered_by(point, poly)) {
     return 0.0;
   }
 
   // TODO(mitukou1109): Use plane sweep method to improve performance?
-  const auto & vertices = poly.vertices();
+  const auto & vertices = poly.outer();
   double min_distance = std::numeric_limits<double>::max();
   for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
     min_distance = std::min(min_distance, distance(point, *it, *std::next(it)));
@@ -678,4 +679,7 @@ bool within(
 
   return true;
 }
+
+template double distance(const alt::Point2d &, const alt::ConvexPolygon2d &);
+template double distance(const alt::Point2d &, const alt::Polygon2d &);
 }  // namespace autoware::universe_utils

common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp

@@ -209,6 +209,8 @@ TEST(alt_geometry, distance)
   using autoware::universe_utils::distance;
   using autoware::universe_utils::alt::ConvexPolygon2d;
   using autoware::universe_utils::alt::Point2d;
+  using autoware::universe_utils::alt::PointList2d;
+  using autoware::universe_utils::alt::Polygon2d;
 
   {  // Normal setting
     const Point2d p = {0.0, 1.0};
@@ -299,6 +301,44 @@ TEST(alt_geometry, distance)
 
     EXPECT_NEAR(result, 0.0, epsilon);
   }
+
+  {  // The point is outside the concave polygon
+    PointList2d outer;
+    outer.push_back({0.0, 0.0});
+    outer.push_back({0.0, 2.0});
+    outer.push_back({1.0, 1.0});
+    outer.push_back({2.0, 2.0});
+    outer.push_back({2.0, 0.0});
+    outer.push_back({0.0, 0.0});
+
+    const Point2d p = {1.0, 2.0};
+
+    const auto result = distance(p, Polygon2d::create(outer, {}).value());
+
+    EXPECT_NEAR(result, std::pow(2, -0.5), epsilon);
+  }
+
+  {  // The point is inside the hole of the polygon
+    PointList2d outer;
+    outer.push_back({0.0, 0.0});
+    outer.push_back({0.0, 2.0});
+    outer.push_back({2.0, 2.0});
+    outer.push_back({2.0, 0.0});
+    outer.push_back({0.0, 0.0});
+
+    PointList2d inner;
+    inner.push_back({0.5, 0.5});
+    inner.push_back({0.5, 1.5});
+    inner.push_back({1.5, 1.5});
+    inner.push_back({1.5, 0.5});
+    inner.push_back({0.5, 0.5});
+
+    const Point2d p = {1.0, 1.0};
+
+    const auto result = distance(p, Polygon2d::create(outer, {inner}).value());
+
+    EXPECT_NEAR(result, 0.0, epsilon);
+  }
 }
 
 TEST(geometry, envelope)