make disjoint() support concave polygons

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

Author: mitukou1109

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

@@ -163,6 +163,8 @@ bool covered_by(const alt::Point2d & point, const alt::PointList2d & ring);
 
 bool covered_by(const alt::Point2d & point, const alt::Polygon2d & poly);
 
+bool disjoint(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2);
+
 bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
 
 double distance(

common/autoware_universe_utils/src/geometry/alt_geometry.cpp

@@ -306,23 +306,14 @@ bool covered_by(const alt::Point2d & point, const alt::Polygon2d & poly)
   return covered_by(point, poly.outer());
 }
 
-bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2)
+bool disjoint(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2)
 {
-  if (equals(poly1, poly2)) {
-    return false;
-  }
-
-  if (intersects(poly1, poly2)) {
-    return false;
-  }
-
-  for (const auto & vertex : poly1.vertices()) {
-    if (touches(vertex, poly2)) {
-      return false;
-    }
-  }
+  return !intersects(poly1, poly2);
+}
 
-  return true;
+bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2)
+{
+  return !intersects(poly1, poly2);
 }
 
 double distance(

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

@@ -238,6 +238,7 @@ TEST(alt_geometry, disjoint)
   using autoware::universe_utils::disjoint;
   using autoware::universe_utils::alt::ConvexPolygon2d;
   using autoware::universe_utils::alt::PointList2d;
+  using autoware::universe_utils::alt::Polygon2d;
 
   {  // Disjoint
     PointList2d vertices1;
@@ -276,6 +277,56 @@ TEST(alt_geometry, disjoint)
 
     EXPECT_FALSE(result);
   }
+
+  {  // Disjoint (one concave polygon is within the hole of the other)
+    PointList2d outer1;
+    outer1.push_back({-1.0, -1.0});
+    outer1.push_back({-1.0, 3.0});
+    outer1.push_back({3.0, 3.0});
+    outer1.push_back({3.0, -1.0});
+
+    PointList2d inner1;
+    inner1.push_back({0.0, 0.0});
+    inner1.push_back({0.0, 2.0});
+    inner1.push_back({1.0, 1.0});
+    inner1.push_back({2.0, 2.0});
+    inner1.push_back({2.0, 0.0});
+
+    PointList2d outer2;
+    outer2.push_back({0.5, 0.5});
+    outer2.push_back({0.5, 1.0});
+    outer2.push_back({1.0, 0.5});
+
+    const auto result =
+      disjoint(Polygon2d::create(outer1, {inner1}).value(), Polygon2d::create(outer2, {}).value());
+
+    EXPECT_TRUE(result);
+  }
+
+  {  // Not disjoint (one concave polygon and the hole of the other share a vertex)
+    PointList2d outer1;
+    outer1.push_back({-1.0, -1.0});
+    outer1.push_back({-1.0, 3.0});
+    outer1.push_back({3.0, 3.0});
+    outer1.push_back({3.0, -1.0});
+
+    PointList2d inner1;
+    inner1.push_back({0.0, 0.0});
+    inner1.push_back({0.0, 2.0});
+    inner1.push_back({1.0, 1.0});
+    inner1.push_back({2.0, 2.0});
+    inner1.push_back({2.0, 0.0});
+
+    PointList2d outer2;
+    outer2.push_back({1.0, 1.0});
+    outer2.push_back({1.5, 0.5});
+    outer2.push_back({0.5, 0.5});
+
+    const auto result =
+      disjoint(Polygon2d::create(outer1, {inner1}).value(), Polygon2d::create(outer2, {}).value());
+
+    EXPECT_FALSE(result);
+  }
 }
 
 TEST(alt_geometry, distance)
@@ -1422,6 +1473,72 @@ TEST(alt_geometry, disjointRand)
   }
 }
 
+TEST(alt_geometry, disjointConcaveRand)
+{
+  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_disjoint_ns = 0.0;
+    double ground_truth_not_disjoint_ns = 0.0;
+    double alt_disjoint_ns = 0.0;
+    double alt_not_disjoint_ns = 0.0;
+    int disjoint_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 (auto j = 0UL; j < polygons.size(); ++j) {
+        sw.tic();
+        const auto ground_truth = boost::geometry::disjoint(polygons[i], polygons[j]);
+        if (ground_truth) {
+          ++disjoint_count;
+          ground_truth_disjoint_ns += sw.toc();
+        } else {
+          ground_truth_not_disjoint_ns += sw.toc();
+        }
+
+        const auto alt_poly1 =
+          autoware::universe_utils::alt::Polygon2d::create(polygons[i]).value();
+        const auto alt_poly2 =
+          autoware::universe_utils::alt::Polygon2d::create(polygons[j]).value();
+        sw.tic();
+        const auto alt = autoware::universe_utils::disjoint(alt_poly1, alt_poly2);
+        if (alt) {
+          alt_disjoint_ns += sw.toc();
+        } else {
+          alt_not_disjoint_ns += sw.toc();
+        }
+
+        if (ground_truth != alt) {
+          std::cout << "Failed for the 2 polygons: ";
+          std::cout << boost::geometry::wkt(polygons[i]) << boost::geometry::wkt(polygons[j])
+                    << std::endl;
+        }
+        EXPECT_EQ(ground_truth, alt);
+      }
+    }
+    std::printf(
+      "polygons_nb = %d, vertices = %ld, %d / %d disjoint pairs\n", polygons_nb, vertices,
+      disjoint_count, polygons_nb * polygons_nb);
+    std::printf(
+      "\tDisjoint:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      ground_truth_disjoint_ns / 1e6, alt_disjoint_ns / 1e6);
+    std::printf(
+      "\tNot disjoint:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      ground_truth_not_disjoint_ns / 1e6, alt_not_disjoint_ns / 1e6);
+    std::printf(
+      "\tTotal:\n\t\tBoost::geometry = %2.2f ms\n\t\tAlt = %2.2f ms\n",
+      (ground_truth_not_disjoint_ns + ground_truth_disjoint_ns) / 1e6,
+      (alt_not_disjoint_ns + alt_disjoint_ns) / 1e6);
+  }
+}
+
 TEST(alt_geometry, intersectsRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;