diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 6c060b03b4543..0d6e608d8cd6d 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -68,6 +68,9 @@ std::optional Polygon2d::create( std::vector inners; for (const auto & inner : polygon.inners()) { PointList2d _inner; + if (inner.empty()) { + continue; + } for (const auto & point : inner) { _inner.push_back(Point2d(point)); } diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 595c93d073ee3..edc17b2dab120 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -420,6 +420,9 @@ std::size_t eliminate_holes( std::vector queue; for (const auto & ring : inners) { + if (ring.empty()) { + continue; + } auto inner_index = linked_list(ring, false, vertices, points); if (points[inner_index].next_index.value() == inner_index) { @@ -617,10 +620,6 @@ std::vector triangulate(const alt::Polygon2d & poly) std::vector triangulate(const Polygon2d & poly) { const auto alt_poly = alt::Polygon2d::create(poly); - if (!alt_poly.has_value()) { - return {}; - } - const auto alt_triangles = triangulate(alt_poly.value()); std::vector triangles; for (const auto & alt_triangle : alt_triangles) { diff --git a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp index 5cf5dbb3bfdfd..44c0dc131fd7d 100644 --- a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp +++ b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp @@ -140,24 +140,17 @@ bool intersecting(const Edge & e, const Polygon2d & polygon) /// @brief checks if an edge is valid for a given polygon and set of points bool is_valid(const Edge & e, const Polygon2d & P, const std::vector & Q) { - bool valid = false; - size_t i = 0; - - while (!valid && i < Q.size()) { - const Point2d & q = Q[i]; + for (const Point2d & q : Q) { Edge e1 = {e.first, q}; Edge e2 = {q, e.second}; bool intersects_e1 = intersecting(e1, P); bool intersects_e2 = intersecting(e2, P); - - if (!intersects_e1 && !intersects_e2) { - valid = true; + if (intersects_e1 || intersects_e2) { + return false; } - - ++i; } - return valid; + return true; } /// @brief finds the nearest node from a set of points to an edge diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index ab5e9f4236bad..db5840693a39f 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -2021,6 +2021,105 @@ TEST(geometry, intersectPolygonRand) } } +double calculate_total_polygon_area( + const std::vector & polygons) +{ + double totalArea = 0.0; + for (const auto & polygon : polygons) { + totalArea += boost::geometry::area(polygon); + } + return totalArea; +} + +TEST(geometry, PolygonTriangulation) +{ + using autoware::universe_utils::Polygon2d; + using autoware::universe_utils::triangulate; + + { // concave polygon + Polygon2d poly; + + poly.outer().emplace_back(0.0, 0.0); + poly.outer().emplace_back(4.0, 0.0); + poly.outer().emplace_back(4.0, 4.0); + poly.outer().emplace_back(2.0, 2.0); + poly.outer().emplace_back(0.0, 4.0); + boost::geometry::correct(poly); + + const auto triangles = triangulate(poly); + + const auto triangle_area = calculate_total_polygon_area(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } + + { // concave polygon with empty inners + Polygon2d poly; + + poly.outer().emplace_back(0.0, 0.0); + poly.outer().emplace_back(4.0, 0.0); + poly.outer().emplace_back(4.0, 4.0); + poly.outer().emplace_back(2.0, 2.0); + poly.outer().emplace_back(0.0, 4.0); + boost::geometry::correct(poly); + + poly.inners().emplace_back(); + + const auto triangles = triangulate(poly); + + const auto triangle_area = calculate_total_polygon_area(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } + + { // concave polygon with hole + Polygon2d poly; + + poly.outer().emplace_back(0.0, 0.0); + poly.outer().emplace_back(4.0, 0.0); + poly.outer().emplace_back(4.0, 4.0); + poly.outer().emplace_back(2.0, 2.0); + poly.outer().emplace_back(0.0, 4.0); + + poly.inners().emplace_back(); + poly.inners().back().emplace_back(1.0, 1.0); + poly.inners().back().emplace_back(1.5, 1.0); + poly.inners().back().emplace_back(1.5, 1.5); + poly.inners().back().emplace_back(1.0, 1.5); + boost::geometry::correct(poly); + + const auto triangles = triangulate(poly); + + const auto triangle_area = calculate_total_polygon_area(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } + + { // concave polygon with one empty inner followed by one hole + Polygon2d poly; + + poly.outer().emplace_back(0.0, 0.0); + poly.outer().emplace_back(4.0, 0.0); + poly.outer().emplace_back(4.0, 4.0); + poly.outer().emplace_back(2.0, 2.0); + poly.outer().emplace_back(0.0, 4.0); + + poly.inners().emplace_back(); + poly.inners().emplace_back(); + poly.inners().back().emplace_back(1.0, 1.0); + poly.inners().back().emplace_back(1.5, 1.0); + poly.inners().back().emplace_back(1.5, 1.5); + poly.inners().back().emplace_back(1.0, 1.5); + boost::geometry::correct(poly); + + const auto triangles = triangulate(poly); + + const auto triangle_area = calculate_total_polygon_area(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } +} + TEST(geometry, intersectPolygonWithHoles) { using autoware::universe_utils::Polygon2d;