feat(universe_utils): add Triangulation (ear clipping) implementation…

… for 2D concave polygon with/without holes (#8609)

* added random_concave_polygon and triangulation

* disable some test with GJK

* pre-commit fix

* fully fixed convexHull issue and  styling fix

* fix conflict

* cleaning up the code

Signed-off-by: Maxime CLEMENT <[email protected]>

* cleanup the code

Signed-off-by: Maxime CLEMENT <[email protected]>

* cleanup the code

Signed-off-by: Maxime CLEMENT <[email protected]>

* fix spelling

Signed-off-by: Maxime CLEMENT <[email protected]>

* last cleanup

Signed-off-by: Maxime CLEMENT <[email protected]>

* more spellcheck fix

Signed-off-by: Maxime CLEMENT <[email protected]>

* more spellcheck fixes

Signed-off-by: Maxime CLEMENT <[email protected]>

---------

Signed-off-by: Maxime CLEMENT <[email protected]>
Co-authored-by: Maxime CLEMENT <[email protected]>

common/autoware_universe_utils/CMakeLists.txt

@@ -16,6 +16,7 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/geometry/pose_deviation.cpp
   src/geometry/boost_polygon_utils.cpp
   src/geometry/random_convex_polygon.cpp
+  src/geometry/random_concave_polygon.cpp
   src/geometry/gjk_2d.cpp
   src/geometry/sat_2d.cpp
   src/math/sin_table.cpp
@@ -25,6 +26,7 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/ros/logger_level_configure.cpp
   src/system/backtrace.cpp
   src/system/time_keeper.cpp
+  src/geometry/ear_clipping.cpp
 )
 
 target_link_libraries(autoware_universe_utils

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

@@ -0,0 +1,107 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_
+
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+
+#include <utility>
+#include <vector>
+
+namespace autoware::universe_utils
+{
+
+class EarClipping
+{
+public:
+  std::vector<std::size_t> indices;
+  std::size_t vertices = 0;
+  using Polygon2d = autoware::universe_utils::Polygon2d;
+  using Point2d = autoware::universe_utils::Point2d;
+  using LinearRing2d = autoware::universe_utils::LinearRing2d;
+
+  void operator()(const Polygon2d & polygon);
+
+  ~EarClipping()
+  {
+    for (auto * p : points_) {
+      delete p;
+    }
+  }
+
+private:
+  struct Point
+  {
+    Point(const std::size_t index, Point2d point) : i(index), pt(std::move(point)) {}
+
+    const std::size_t i;  // Index of the point in the original polygon
+    const Point2d pt;     // The Point2d object representing the coordinates
+
+    // Previous and next vertices (Points) in the polygon ring
+    Point * prev = nullptr;
+    Point * next = nullptr;
+    bool steiner = false;
+
+    [[nodiscard]] double x() const { return pt.x(); }
+    [[nodiscard]] double y() const { return pt.y(); }
+  };
+
+  std::vector<Point *> points_;
+
+  Point * linked_list(const LinearRing2d & points, bool clockwise);
+  static Point * filter_points(Point * start, Point * end = nullptr);
+  Point * cure_local_intersections(Point * start);
+  static Point * get_leftmost(Point * start);
+  Point * split_polygon(Point * a, Point * b);
+  Point * insert_point(std::size_t i, const Point2d & p, Point * last);
+  Point * eliminate_holes(
+    const std::vector<LinearRing2d> & inners, EarClipping::Point * outer_point);
+  Point * eliminate_hole(Point * hole, Point * outer_point);
+  static Point * find_hole_bridge(Point * hole, Point * outer_point);
+  void ear_clipping_linked(Point * ear, int pass = 0);
+  void split_ear_clipping(Point * start);
+  static void remove_point(Point * p);
+  static bool is_ear(Point * ear);
+  static bool sector_contains_sector(const Point * m, const Point * p);
+  [[nodiscard]] static bool point_in_triangle(
+    double ax, double ay, double bx, double by, double cx, double cy, double px, double py);
+  static bool is_valid_diagonal(Point * a, Point * b);
+  static bool equals(const Point * p1, const Point * p2);
+  static bool intersects(const Point * p1, const Point * q1, const Point * p2, const Point * q2);
+  static bool on_segment(const Point * p, const Point * q, const Point * r);
+  static bool intersects_polygon(const Point * a, const Point * b);
+  static bool locally_inside(const Point * a, const Point * b);
+  static bool middle_inside(const Point * a, const Point * b);
+  static int sign(double val);
+  static double area(const Point * p, const Point * q, const Point * r);
+
+  // Function to construct a new Point object
+  EarClipping::Point * construct_point(std::size_t index, const Point2d & point)
+  {
+    auto * new_point = new Point(index, point);
+    points_.push_back(new_point);
+    return new_point;
+  }
+};
+
+/// @brief Triangulate based on ear clipping algorithm
+/// @param polygon concave/convex polygon with/without holes
+/// @details algorithm based on https://github.com/mapbox/earclipping with modification
+std::vector<autoware::universe_utils::Polygon2d> triangulate(
+  const autoware::universe_utils::Polygon2d & poly);
+
+}  // namespace autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_

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

@@ -0,0 +1,45 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__UNIVERSE_UTILS__GEOMETRY__RANDOM_CONCAVE_POLYGON_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__RANDOM_CONCAVE_POLYGON_HPP_
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+
+#include <vector>
+namespace autoware::universe_utils
+{
+/// @brief generate a random non-convex polygon
+/// @param vertices number of vertices for the desired polygon
+/// @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);
+
+/// @brief checks for collisions between two vectors of convex polygons using a specified collision
+/// detection algorithm
+/// @param polygons1 A vector of convex polygons to check for collisions.
+/// @param polygons2 A vector of convex polygons to check for collisions.
+/// @param intersection_func A function that takes two polygons and returns true if they intersect,
+/// otherwise false.
+/// @return True if at least one pair of polygons intersects, otherwise false.
+bool test_intersection(
+  const std::vector<autoware::universe_utils::Polygon2d> & polygons1,
+  const std::vector<autoware::universe_utils::Polygon2d> & polygons2,
+  const std::function<bool(
+    const autoware::universe_utils::Polygon2d &, const autoware::universe_utils::Polygon2d &)> &);
+
+}  // namespace autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__RANDOM_CONCAVE_POLYGON_HPP_