fix(autoware_universe_utils): fix procedure to check if point is on e…

…dge (#10260) * fix procedure to check if point is on edge Signed-off-by: mitukou1109 <[email protected]> * add test cases Signed-off-by: mitukou1109 <[email protected]> --------- Signed-off-by: mitukou1109 <[email protected]>
autowarefoundation · Mar 12, 2025 · 2d879df · 2d879df
1 parent 60b9108
commit 2d879df
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 30 deletions.
diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp
@@ -263,28 +263,30 @@ bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
     return false;
   }
 
-  double cross;
   for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
     const auto & p1 = *it;
     const auto & p2 = *std::next(it);
 
-    if (p1.y() <= point.y() && p2.y() >= point.y()) {  // upward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross > 0) {  // point is to the left of edge
-        winding_number++;
-        continue;
-      }
-    } else if (p1.y() >= point.y() && p2.y() <= point.y()) {  // downward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross < 0) {  // point is to the left of edge
-        winding_number--;
-        continue;
-      }
-    } else {
+    const auto is_upward_edge = p1.y() <= point.y() && p2.y() >= point.y();
+    const auto is_downward_edge = p1.y() >= point.y() && p2.y() <= point.y();
+
+    if (!is_upward_edge && !is_downward_edge) {
+      continue;
+    }
+
+    const auto start_vec = point - p1;
+    const auto end_vec = point - p2;
+    const auto cross = start_vec.cross(end_vec);
+
+    if (is_upward_edge && cross > 0) {  // point is to the left of edge
+      winding_number++;
+      continue;
+    } else if (is_downward_edge && cross < 0) {  // point is to the left of edge
+      winding_number--;
       continue;
     }
 
-    if (std::abs(cross) < epsilon) {  // point is on edge
+    if (std::abs(cross) < epsilon && start_vec.dot(end_vec) <= 0.) {  // point is on edge
       return true;
     }
   }
@@ -600,28 +602,30 @@ bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly)
     return false;
   }
 
-  double cross;
   for (auto it = vertices.cbegin(); it != std::prev(vertices.cend()); ++it) {
     const auto & p1 = *it;
     const auto & p2 = *std::next(it);
 
-    if (p1.y() < point.y() && p2.y() > point.y()) {  // upward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross > 0) {  // point is to the left of edge
-        winding_number++;
-        continue;
-      }
-    } else if (p1.y() > point.y() && p2.y() < point.y()) {  // downward edge
-      cross = (p2 - p1).cross(point - p1);
-      if (cross < 0) {  // point is to the left of edge
-        winding_number--;
-        continue;
-      }
-    } else {
+    const auto is_upward_edge = p1.y() < point.y() && p2.y() > point.y();
+    const auto is_downward_edge = p1.y() > point.y() && p2.y() < point.y();
+
+    if (!is_upward_edge && !is_downward_edge) {
+      continue;
+    }
+
+    const auto start_vec = point - p1;
+    const auto end_vec = point - p2;
+    const auto cross = start_vec.cross(end_vec);
+
+    if (is_upward_edge && cross > 0) {  // point is to the left of edge
+      winding_number++;
+      continue;
+    } else if (is_downward_edge && cross < 0) {  // point is to the left of edge
+      winding_number--;
       continue;
     }
 
-    if (std::abs(cross) < epsilon) {  // point is on edge
+    if (std::abs(cross) < epsilon && start_vec.dot(end_vec) <= 0.) {  // point is on edge
       return false;
     }
   }

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
@@ -164,6 +164,17 @@ TEST(alt_geometry, coveredBy)
 
     EXPECT_TRUE(result);
   }
+
+  {  // The point is on the extended line of an edge of the polygon
+    const Point2d point = {0.0, 0.0};
+    const Point2d p1 = {3.0, 0.0};
+    const Point2d p2 = {3.0, 1.0};
+    const Point2d p3 = {4.0, 1.0};
+    const Point2d p4 = {4.0, 0.0};
+    const auto result = covered_by(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+    EXPECT_FALSE(result);
+  }
 }
 
 TEST(alt_geometry, disjoint)
@@ -674,6 +685,17 @@ TEST(alt_geometry, within)
     EXPECT_FALSE(result);
   }
 
+  {  // The point is on the extended line of an edge of the polygon
+    const Point2d point = {0.0, 0.0};
+    const Point2d p1 = {3.0, 0.0};
+    const Point2d p2 = {3.0, 1.0};
+    const Point2d p3 = {4.0, 1.0};
+    const Point2d p4 = {4.0, 0.0};
+    const auto result = within(point, ConvexPolygon2d::create({p1, p2, p3, p4}).value());
+
+    EXPECT_FALSE(result);
+  }
+
   {  // One polygon is within the other
     const Point2d p1 = {1.0, 1.0};
     const Point2d p2 = {1.0, -1.0};