diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_concave_polygon.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_concave_polygon.hpp index 297201e925399..63c55377bd956 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_concave_polygon.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/random_concave_polygon.hpp @@ -17,7 +17,9 @@ #include +#include #include + namespace autoware::universe_utils { /// @brief generate a random non-convex polygon @@ -25,7 +27,7 @@ namespace autoware::universe_utils /// @param max points will be generated in the range [-max, max] /// @details algorithm from /// https://digitalscholarship.unlv.edu/cgi/viewcontent.cgi?article=3183&context=thesesdissertations -Polygon2d random_concave_polygon(const size_t vertices, const double max); +std::optional random_concave_polygon(const size_t vertices, const double max); /// @brief checks for collisions between two vectors of convex polygons using a specified collision /// detection algorithm 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..aa3c2afab321a 100644 --- a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp +++ b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -138,30 +139,23 @@ 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 is_valid(const Edge & e, const Polygon2d & P, const std::list & 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 -Point2d get_nearest_node(const std::vector & Q, const Edge & e) +Point2d get_nearest_node(const std::list & Q, const Edge & e) { double min_distance = std::numeric_limits::max(); Point2d nearest_node(0, 0); @@ -178,7 +172,7 @@ Point2d get_nearest_node(const std::vector & Q, const Edge & e) } /// @brief finds the edge that is closest to the given set of points -Edge get_breaking_edge(const PolygonWithEdges & polygon_with_edges, const std::vector & Q) +Edge get_breaking_edge(const PolygonWithEdges & polygon_with_edges, const std::list & Q) { double min_distance = std::numeric_limits::max(); Edge e_breaking; @@ -229,7 +223,7 @@ void insert_node(PolygonWithEdges & polygon_with_edges, const Point2d & w, const } /// @brief removes a node from a set of points -void remove_node(std::vector & Q, const Point2d & w) +void remove_node(std::list & Q, const Point2d & w) { const double epsilon = 1e-9; @@ -243,7 +237,7 @@ void remove_node(std::vector & Q, const Point2d & w) } /// @brief marks edges as valid if they are valid according to the polygon and points -void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::vector & Q) +void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::list & Q) { for (auto & edge : polygon_with_edges.edges) { if (is_valid(edge, polygon_with_edges.polygon, Q)) { @@ -256,8 +250,7 @@ void mark_valid_edges(PolygonWithEdges & polygon_with_edges, const std::vector

q; - q.reserve(ring.size()); + std::list q; boost::geometry::strategy::convex_hull::graham_andrew strategy; boost::geometry::convex_hull(ring, convex_ring, strategy); PolygonWithEdges polygon_with_edges; @@ -342,7 +335,7 @@ bool test_intersection( return false; } -Polygon2d random_concave_polygon(const size_t vertices, const double max) +std::optional random_concave_polygon(const size_t vertices, const double max) { if (vertices < 4) { return Polygon2d(); @@ -376,7 +369,7 @@ Polygon2d random_concave_polygon(const size_t vertices, const double max) // apply inward denting algorithm poly = inward_denting(points); // check for convexity - if (!is_convex(poly)) { + if (!is_convex(poly) && boost::geometry::is_valid(poly) && poly.outer().size() != vertices) { is_non_convex = true; } LinearRing2d poly_outer = poly.outer(); 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 af90ab1f32c57..3202347cc8ac4 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; @@ -2246,7 +2345,10 @@ TEST(geometry, intersectConcavePolygonRand) triangulations.clear(); for (auto i = 0; i < polygons_nb; ++i) { - polygons.push_back(autoware::universe_utils::random_concave_polygon(vertices, max_values)); + auto polygon_opt = autoware::universe_utils::random_concave_polygon(vertices, max_values); + if (polygon_opt.has_value()) { + polygons.push_back(polygon_opt.value()); + } } for (const auto & polygon : polygons) {