From 3e35f7cabd8d485e05eb38892faeae9ce1f1432a Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Tue, 1 Oct 2024 15:20:50 +0900 Subject: [PATCH 1/8] fixed self-intersecting issue and empty inners --- .../src/geometry/ear_clipping.cpp | 7 ++++++- .../src/geometry/random_concave_polygon.cpp | 15 ++++----------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 595c93d073ee3..53ccde2897134 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -582,7 +582,12 @@ std::vector perform_triangulation( } if (!polygon.inners().empty()) { - outer_point_index = eliminate_holes(polygon.inners(), outer_point_index, vertices, points); + const auto & inner_rings = polygon.inners(); + bool has_non_empty_holes = std::any_of( + inner_rings.begin(), inner_rings.end(), [](const auto & ring) { return !ring.empty(); }); + if (has_non_empty_holes) { + outer_point_index = eliminate_holes(inner_rings, outer_point_index, vertices, points); + } } ear_clipping_linked(outer_point_index, indices, points); 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 From 5960d287973aa5046907df133ba3d5364ef957a1 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Tue, 1 Oct 2024 18:09:16 +0900 Subject: [PATCH 2/8] fixed empty inners and add some unit test cases --- .../universe_utils/geometry/ear_clipping.hpp | 5 + .../src/geometry/alt_geometry.cpp | 3 + .../src/geometry/ear_clipping.cpp | 34 +++++-- .../test/src/geometry/test_geometry.cpp | 91 +++++++++++++++++++ 4 files changed, 123 insertions(+), 10 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index e111504795861..826172c17bca8 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -83,6 +83,11 @@ std::size_t eliminate_holes( const std::vector & inners, std::size_t outer_index, std::size_t & vertices, std::vector & points); +/** + * @brief helper function to calcuate total area of triangles + */ +double calculate_total_triangle(const std::vector& triangles); + /** * @brief triangulates a polygon into convex triangles * @details simplifies a concave polygon, with or without holes, into a set of triangles diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 6c060b03b4543..633ec094da50c 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 53ccde2897134..93469137ce462 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) { @@ -561,6 +564,26 @@ void ear_clipping_linked( } } +double calculate_triangle_area(const autoware::universe_utils::Polygon2d& triangle) { + const auto& points = triangle.outer(); + double x1 = points[0].x(); + double y1 = points[0].y(); + double x2 = points[1].x(); + double y2 = points[1].y(); + double x3 = points[2].x(); + double y3 = points[2].y(); + + return std::abs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); +} + +double calculate_total_triangle(const std::vector& triangles) { + double totalArea = 0.0; + for (const auto& triangle : triangles) { + totalArea += calculate_triangle_area(triangle); + } + return totalArea; +} + std::vector perform_triangulation( const alt::Polygon2d & polygon, std::vector & indices) { @@ -582,12 +605,7 @@ std::vector perform_triangulation( } if (!polygon.inners().empty()) { - const auto & inner_rings = polygon.inners(); - bool has_non_empty_holes = std::any_of( - inner_rings.begin(), inner_rings.end(), [](const auto & ring) { return !ring.empty(); }); - if (has_non_empty_holes) { - outer_point_index = eliminate_holes(inner_rings, outer_point_index, vertices, points); - } + outer_point_index = eliminate_holes(polygon.inners(), outer_point_index, vertices, points); } ear_clipping_linked(outer_point_index, indices, points); @@ -622,10 +640,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/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index ab5e9f4236bad..061d531c3ee71 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,97 @@ TEST(geometry, intersectPolygonRand) } } +TEST(geometry, PolygonTriangulation) +{ + using autoware::universe_utils::Polygon2d; + using autoware::universe_utils::triangulate; + using autoware::universe_utils::calculate_total_triangle; + + + { // 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_triangle(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_triangle(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_triangle(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } + + { // concave polygon with one empty inners and 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_triangle(triangles); + const auto poly_area = boost::geometry::area(poly); + EXPECT_NEAR(triangle_area, poly_area, epsilon); + } +} + TEST(geometry, intersectPolygonWithHoles) { using autoware::universe_utils::Polygon2d; From 4449916fcc044f26e3e75b3b51ad53036a108d15 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 1 Oct 2024 09:12:43 +0000 Subject: [PATCH 3/8] style(pre-commit): autofix --- .../universe_utils/geometry/ear_clipping.hpp | 2 +- .../src/geometry/alt_geometry.cpp | 2 +- .../src/geometry/ear_clipping.cpp | 36 ++++++++++--------- .../test/src/geometry/test_geometry.cpp | 3 +- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index 826172c17bca8..bafe2c4fae1b8 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -86,7 +86,7 @@ std::size_t eliminate_holes( /** * @brief helper function to calcuate total area of triangles */ -double calculate_total_triangle(const std::vector& triangles); +double calculate_total_triangle(const std::vector & triangles); /** * @brief triangulates a polygon into convex triangles diff --git a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp index 633ec094da50c..0d6e608d8cd6d 100644 --- a/common/autoware_universe_utils/src/geometry/alt_geometry.cpp +++ b/common/autoware_universe_utils/src/geometry/alt_geometry.cpp @@ -69,7 +69,7 @@ std::optional Polygon2d::create( for (const auto & inner : polygon.inners()) { PointList2d _inner; if (inner.empty()) { - continue; + 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 93469137ce462..77226be4c1ece 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -421,7 +421,7 @@ std::size_t eliminate_holes( for (const auto & ring : inners) { if (ring.empty()) { - continue; + continue; } auto inner_index = linked_list(ring, false, vertices, points); @@ -564,24 +564,26 @@ void ear_clipping_linked( } } -double calculate_triangle_area(const autoware::universe_utils::Polygon2d& triangle) { - const auto& points = triangle.outer(); - double x1 = points[0].x(); - double y1 = points[0].y(); - double x2 = points[1].x(); - double y2 = points[1].y(); - double x3 = points[2].x(); - double y3 = points[2].y(); - - return std::abs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); +double calculate_triangle_area(const autoware::universe_utils::Polygon2d & triangle) +{ + const auto & points = triangle.outer(); + double x1 = points[0].x(); + double y1 = points[0].y(); + double x2 = points[1].x(); + double y2 = points[1].y(); + double x3 = points[2].x(); + double y3 = points[2].y(); + + return std::abs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); } -double calculate_total_triangle(const std::vector& triangles) { - double totalArea = 0.0; - for (const auto& triangle : triangles) { - totalArea += calculate_triangle_area(triangle); - } - return totalArea; +double calculate_total_triangle(const std::vector & triangles) +{ + double totalArea = 0.0; + for (const auto & triangle : triangles) { + totalArea += calculate_triangle_area(triangle); + } + return totalArea; } std::vector perform_triangulation( 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 061d531c3ee71..a0a169a728999 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -2023,11 +2023,10 @@ TEST(geometry, intersectPolygonRand) TEST(geometry, PolygonTriangulation) { + using autoware::universe_utils::calculate_total_triangle; using autoware::universe_utils::Polygon2d; using autoware::universe_utils::triangulate; - using autoware::universe_utils::calculate_total_triangle; - { // concave polygon Polygon2d poly; From b93d4bd14927212fc83ce7a09d6a74d04c4fe07c Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Tue, 1 Oct 2024 18:15:40 +0900 Subject: [PATCH 4/8] minor fix on test description --- .../autoware_universe_utils/test/src/geometry/test_geometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a0a169a728999..e91f108858b8f 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -2086,7 +2086,7 @@ TEST(geometry, PolygonTriangulation) EXPECT_NEAR(triangle_area, poly_area, epsilon); } - { // concave polygon with one empty inners and one hole + { // concave polygon with one empty inner followed by one hole Polygon2d poly; poly.outer().emplace_back(0.0, 0.0); From e14191421c36be45588313eacf03409ac5ae2c1d Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Thu, 3 Oct 2024 13:24:15 +0900 Subject: [PATCH 5/8] move area calculation to test --- .../universe_utils/geometry/ear_clipping.hpp | 5 ----- .../src/geometry/ear_clipping.cpp | 22 ------------------- .../test/src/geometry/test_geometry.cpp | 19 +++++++++++----- 3 files changed, 14 insertions(+), 32 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp index bafe2c4fae1b8..e111504795861 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/ear_clipping.hpp @@ -83,11 +83,6 @@ std::size_t eliminate_holes( const std::vector & inners, std::size_t outer_index, std::size_t & vertices, std::vector & points); -/** - * @brief helper function to calcuate total area of triangles - */ -double calculate_total_triangle(const std::vector & triangles); - /** * @brief triangulates a polygon into convex triangles * @details simplifies a concave polygon, with or without holes, into a set of triangles diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 77226be4c1ece..edc17b2dab120 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -564,28 +564,6 @@ void ear_clipping_linked( } } -double calculate_triangle_area(const autoware::universe_utils::Polygon2d & triangle) -{ - const auto & points = triangle.outer(); - double x1 = points[0].x(); - double y1 = points[0].y(); - double x2 = points[1].x(); - double y2 = points[1].y(); - double x3 = points[2].x(); - double y3 = points[2].y(); - - return std::abs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0); -} - -double calculate_total_triangle(const std::vector & triangles) -{ - double totalArea = 0.0; - for (const auto & triangle : triangles) { - totalArea += calculate_triangle_area(triangle); - } - return totalArea; -} - std::vector perform_triangulation( const alt::Polygon2d & polygon, std::vector & indices) { 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 e91f108858b8f..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,9 +2021,18 @@ 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::calculate_total_triangle; using autoware::universe_utils::Polygon2d; using autoware::universe_utils::triangulate; @@ -2039,7 +2048,7 @@ TEST(geometry, PolygonTriangulation) const auto triangles = triangulate(poly); - const auto triangle_area = calculate_total_triangle(triangles); + const auto triangle_area = calculate_total_polygon_area(triangles); const auto poly_area = boost::geometry::area(poly); EXPECT_NEAR(triangle_area, poly_area, epsilon); } @@ -2058,7 +2067,7 @@ TEST(geometry, PolygonTriangulation) const auto triangles = triangulate(poly); - const auto triangle_area = calculate_total_triangle(triangles); + const auto triangle_area = calculate_total_polygon_area(triangles); const auto poly_area = boost::geometry::area(poly); EXPECT_NEAR(triangle_area, poly_area, epsilon); } @@ -2081,7 +2090,7 @@ TEST(geometry, PolygonTriangulation) const auto triangles = triangulate(poly); - const auto triangle_area = calculate_total_triangle(triangles); + const auto triangle_area = calculate_total_polygon_area(triangles); const auto poly_area = boost::geometry::area(poly); EXPECT_NEAR(triangle_area, poly_area, epsilon); } @@ -2105,7 +2114,7 @@ TEST(geometry, PolygonTriangulation) const auto triangles = triangulate(poly); - const auto triangle_area = calculate_total_triangle(triangles); + const auto triangle_area = calculate_total_polygon_area(triangles); const auto poly_area = boost::geometry::area(poly); EXPECT_NEAR(triangle_area, poly_area, epsilon); } From e6dfc0a9cfffa03b1ed500687ddfd2b865920de9 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Mon, 7 Oct 2024 16:33:14 +0900 Subject: [PATCH 6/8] make sure polygon is_valid --- .../src/geometry/random_concave_polygon.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 44c0dc131fd7d..2195776d5c4ac 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 @@ -369,7 +370,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)) { is_non_convex = true; } LinearRing2d poly_outer = poly.outer(); From af68b95c9fb40c74bb58dfa595a8382cba0f8386 Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Tue, 15 Oct 2024 15:54:05 +0900 Subject: [PATCH 7/8] fixed different size issue --- .../src/geometry/random_concave_polygon.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2195776d5c4ac..8d13ba19fdd14 100644 --- a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp +++ b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp @@ -370,7 +370,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) && boost::geometry::is_valid(poly)) { + if (!is_convex(poly) && boost::geometry::is_valid(poly) && poly.outer().size() != vertices) { is_non_convex = true; } LinearRing2d poly_outer = poly.outer(); From 61a9fe4034ba502e99b233d1a80755d7059bc98f Mon Sep 17 00:00:00 2001 From: mraditya01 Date: Mon, 11 Nov 2024 13:56:00 +0900 Subject: [PATCH 8/8] change vector to list and use optional --- .../geometry/random_concave_polygon.hpp | 4 +++- .../src/geometry/random_concave_polygon.cpp | 15 +++++++-------- .../test/src/geometry/test_geometry.cpp | 5 ++++- 3 files changed, 14 insertions(+), 10 deletions(-) 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/random_concave_polygon.cpp b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp index 8d13ba19fdd14..aa3c2afab321a 100644 --- a/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp +++ b/common/autoware_universe_utils/src/geometry/random_concave_polygon.cpp @@ -139,7 +139,7 @@ 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) { for (const Point2d & q : Q) { Edge e1 = {e.first, q}; @@ -155,7 +155,7 @@ bool is_valid(const Edge & e, const Polygon2d & P, const std::vector & } /// @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); @@ -172,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; @@ -223,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; @@ -237,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)) { @@ -250,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; @@ -336,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(); 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 f7a3785109047..3202347cc8ac4 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -2345,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) {