From 59838b6e7552ef1bc61867f9dc65de4dfa472529 Mon Sep 17 00:00:00 2001 From: Giovanni Muhammad Raditya Date: Fri, 20 Sep 2024 10:23:23 +0900 Subject: [PATCH] refactor(universe_utils): remove raw pointers from the triangulation function (#8893) --- .../universe_utils/geometry/ear_clipping.hpp | 146 ++- .../src/geometry/ear_clipping.cpp | 834 ++++++++++-------- 2 files changed, 530 insertions(+), 450 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 5ba6501a1ec18..cf93740e956d8 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 @@ -17,91 +17,89 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" -#include +#include #include namespace autoware::universe_utils { -class EarClipping -{ -public: - std::vector 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); +using Polygon2d = autoware::universe_utils::Polygon2d; +using Point2d = autoware::universe_utils::Point2d; +using LinearRing2d = autoware::universe_utils::LinearRing2d; - ~EarClipping() +struct LinkedPoint +{ + explicit LinkedPoint(const Point2d & point) + : pt(point), steiner(false), prev_index(std::nullopt), next_index(std::nullopt) { - 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 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 & 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; - } + Point2d pt; + bool steiner; + std::optional prev_index; + std::optional next_index; + [[nodiscard]] double x() const { return pt.x(); } + [[nodiscard]] double y() const { return pt.y(); } }; -/// @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 triangulate( - const autoware::universe_utils::Polygon2d & poly); - +/** + * @brief main ear slicing loop which triangulates a polygon using linked list + * @details iterates over the linked list of polygon points, cutting off triangular ears one by one + * handles different stages for fixing intersections and splitting if necessary + */ +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass = 0); +void split_ear_clipping( + std::vector & points, std::size_t start_index, std::vector & indices); + +/** + * @brief creates a linked list from a ring of points + * @details converts a polygon ring into a doubly linked list with optional clockwise ordering + * @return the last index of the created linked list + */ +std::size_t linked_list( + const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + std::vector & points); + +/** + * @brief David Eberly's algorithm for finding a bridge between hole and outer polygon + * @details connects a hole to the outer polygon by finding the closest bridge point + * @return index of the bridge point + */ +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points); + +/** + * @brief eliminates a single hole by connecting it to the outer polygon + * @details finds a bridge and modifies the linked list to remove the hole + * @return the updated outer_index after the hole is eliminated + */ +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points); + +/** + * @brief eliminates all holes from a polygon + * @details processes multiple holes by connecting each to the outer polygon in sequence + * @return the updated outer_index after all holes are eliminated + */ +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points); + +/** + * @brief triangulates a polygon into convex triangles + * @details simplifies a concave polygon, with or without holes, into a set of triangles + * the size of the `points` vector at the end of the perform_triangulation algorithm is described as + * follow: + * - `outer_points`: Number of points in the initial outer linked list. + * - `hole_points`: Number of points in all inner polygons. + * - `2 * n_holes`: Additional points for bridging holes, where `n_holes` is the number of holes. + * the final size of `points` vector is: `outer_points + hole_points + 2 * n_holes`. + * @return A vector of convex triangles representing the triangulated polygon. + */ +std::vector triangulate(const Polygon2d & polygon); } // namespace autoware::universe_utils #endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__EAR_CLIPPING_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp index 741f6fb901c07..03b39e9536b62 100644 --- a/common/autoware_universe_utils/src/geometry/ear_clipping.cpp +++ b/common/autoware_universe_utils/src/geometry/ear_clipping.cpp @@ -14,525 +14,607 @@ #include "autoware/universe_utils/geometry/ear_clipping.hpp" -#ifndef EAR_CUT_IMPL_HPP -#define EAR_CUT_IMPL_HPP - namespace autoware::universe_utils { -void EarClipping::operator()(const Polygon2d & polygon) +void remove_point(const std::size_t p_index, std::vector & points) { - indices.clear(); - vertices = 0; - - if (polygon.outer().size() == 0) return; - - std::size_t len = 0; - - const auto & outer_ring = polygon.outer(); - len = outer_ring.size(); - points_.reserve(len * 3 / 2); - indices.reserve(len + outer_ring.size()); + std::size_t prev_index = points[p_index].prev_index.value(); + std::size_t next_index = points[p_index].next_index.value(); - EarClipping::Point * outer_point = linked_list(outer_ring, true); - if (!outer_point || outer_point->prev == outer_point->next) return; - if (polygon.inners().size() > 0) outer_point = eliminate_holes(polygon.inners(), outer_point); - ear_clipping_linked(outer_point); - points_.clear(); + points[prev_index].next_index = next_index; + points[next_index].prev_index = prev_index; } -/// @brief create a circular doubly linked list from polygon points in the specified winding order -EarClipping::Point * EarClipping::linked_list(const LinearRing2d & points, bool clockwise) +std::size_t get_leftmost(const std::size_t start_idx, const std::vector & points) { - double sum = 0; - const std::size_t len = points.size(); - EarClipping::Point * last = nullptr; - - for (size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { - const auto & p1 = points[i]; - const auto & p2 = points[j]; - const double p10 = p1.x(); - const double p11 = p1.y(); - const double p20 = p2.x(); - const double p21 = p2.y(); - sum += (p20 - p10) * (p11 + p21); - } + std::optional p_idx = points[start_idx].next_index; + std::size_t left_most_idx = start_idx; - if (clockwise == (sum > 0)) { - for (size_t i = 0; i < len; i++) { - last = insert_point(vertices + i, points[i], last); - } - } else { - for (size_t i = len; i-- > 0;) { - last = insert_point(vertices + i, points[i], last); + while (p_idx.has_value() && p_idx.value() != start_idx) { + if ( + points[p_idx.value()].x() < points[left_most_idx].x() || + (points[p_idx.value()].x() == points[left_most_idx].x() && + points[p_idx.value()].y() < points[left_most_idx].y())) { + left_most_idx = p_idx.value(); } + p_idx = points[p_idx.value()].next_index; } - if (last && equals(last, last->next)) { - remove_point(last); - last = last->next; - } - - vertices += len; + return left_most_idx; +} - return last; +bool point_in_triangle( + const double ax, const double ay, const double bx, const double by, const double cx, + const double cy, const double px, const double py) +{ + return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && + (ax - px) * (by - py) >= (bx - px) * (ay - py) && + (bx - px) * (cy - py) >= (cx - px) * (by - py); } -/// @brief eliminate colinear or duplicate points -EarClipping::Point * EarClipping::filter_points( - EarClipping::Point * start, EarClipping::Point * end) +double area( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) { - if (!end) end = start; + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return (q.y() - p.y()) * (r.x() - q.x()) - (q.x() - p.x()) * (r.y() - q.y()); +} - EarClipping::Point * p = start; - bool again = false; - do { - again = false; +bool middle_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) +{ + std::optional p_idx = a_idx; + bool inside = false; + double px = (points[a_idx].x() + points[b_idx].x()) / 2; + double py = (points[a_idx].y() + points[b_idx].y()) / 2; + std::size_t start_idx = a_idx; - if (!p->steiner && (equals(p, p->next) || area(p->prev, p, p->next) == 0)) { - remove_point(p); - p = end = p->prev; + while (p_idx.has_value()) { + std::size_t current_idx = p_idx.value(); + std::size_t next_idx = points[current_idx].next_index.value(); - if (p == p->next) break; - again = true; + if ( + ((points[current_idx].y() > py) != (points[next_idx].y() > py)) && + points[next_idx].y() != points[current_idx].y() && + (px < (points[next_idx].x() - points[current_idx].x()) * (py - points[current_idx].y()) / + (points[next_idx].y() - points[current_idx].y()) + + points[current_idx].x())) { + inside = !inside; + } - } else { - p = p->next; + if (next_idx == start_idx) { + break; // Break the loop if we have cycled back to the start index } - } while (again || p != end); - return end; + p_idx = next_idx; + } + + return inside; } -/// @brief find a bridge between vertices that connects hole with an outer ring and and link it -EarClipping::Point * EarClipping::eliminate_hole( - EarClipping::Point * hole, EarClipping::Point * outer_point) +bool equals( + const std::size_t p1_idx, const std::size_t p2_idx, const std::vector & points) { - EarClipping::Point * bridge = find_hole_bridge(hole, outer_point); - if (!bridge) { - return outer_point; - } - EarClipping::Point * bridge_reverse = split_polygon(bridge, hole); + return points[p1_idx].x() == points[p2_idx].x() && points[p1_idx].y() == points[p2_idx].y(); +} - // filter collinear points around the cuts - filter_points(bridge_reverse, bridge_reverse->next); +int sign(const double val) +{ + return (0.0 < val) - (val < 0.0); +} - // Check if input node was removed by the filtering - return filter_points(bridge, bridge->next); +bool on_segment( + const std::vector & points, const std::size_t p_idx, const std::size_t q_idx, + const std::size_t r_idx) +{ + const LinkedPoint & p = points[p_idx]; + const LinkedPoint & q = points[q_idx]; + const LinkedPoint & r = points[r_idx]; + return q.x() <= std::max(p.x(), r.x()) && q.x() >= std::min(p.x(), r.x()) && + q.y() <= std::max(p.y(), r.y()) && q.y() >= std::min(p.y(), r.y()); } -EarClipping::Point * EarClipping::eliminate_holes( - const std::vector & inners, EarClipping::Point * outer_point) +bool locally_inside( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - const size_t len = inners.size(); - - std::vector queue; - for (size_t i = 0; i < len; i++) { - Point * list = linked_list(inners[i], false); - if (list) { - if (list == list->next) list->steiner = true; - queue.push_back(get_leftmost(list)); - } - } - std::sort( - queue.begin(), queue.end(), [](const Point * a, const Point * b) { return a->x() < b->x(); }); - for (const auto & q : queue) { - outer_point = eliminate_hole(q, outer_point); + const auto & prev_idx = points[a_idx].prev_index; + const auto & next_idx = points[a_idx].next_index; + + if (!prev_idx.has_value() || !next_idx.has_value()) { + return false; } - return outer_point; + double area_prev = area(points, prev_idx.value(), a_idx, next_idx.value()); + double area_a_b_next = area(points, a_idx, b_idx, next_idx.value()); + double area_a_prev_b = area(points, a_idx, prev_idx.value(), b_idx); + double area_a_b_prev = area(points, a_idx, b_idx, prev_idx.value()); + double area_a_next_b = area(points, a_idx, next_idx.value(), b_idx); + + return area_prev < 0 ? area_a_b_next >= 0 && area_a_prev_b >= 0 + : area_a_b_prev < 0 || area_a_next_b < 0; } -// cspell: ignore Eberly -/// @brief David Eberly's algorithm for finding a bridge between hole and outer polygon -EarClipping::Point * EarClipping::find_hole_bridge(Point * hole, Point * outer_point) +bool intersects( + const std::size_t p1_idx, const std::size_t q1_idx, const std::size_t p2_idx, + const std::size_t q2_idx, const std::vector & points) { - Point * p = outer_point; - double hx = hole->x(); - double hy = hole->y(); - double qx = -std::numeric_limits::infinity(); - Point * m = nullptr; - do { - if (hy <= p->y() && hy >= p->next->y() && p->next->y() != p->y()) { - double x = p->x() + (hy - p->y()) * (p->next->x() - p->x()) / (p->next->y() - p->y()); - if (x <= hx && x > qx) { - qx = x; - m = p->x() < p->next->x() ? p : p->next; - if (x == hx) return m; - } - } - p = p->next; - } while (p != outer_point); + int o1 = sign(area(points, p1_idx, q1_idx, p2_idx)); + int o2 = sign(area(points, p1_idx, q1_idx, q2_idx)); + int o3 = sign(area(points, p2_idx, q2_idx, p1_idx)); + int o4 = sign(area(points, p2_idx, q2_idx, q1_idx)); - if (!m) return nullptr; + if (o1 != o2 && o3 != o4) return true; - const Point * stop = m; - double tan_min = std::numeric_limits::infinity(); + if (o1 == 0 && on_segment(points, p1_idx, p2_idx, q1_idx)) return true; + if (o2 == 0 && on_segment(points, p1_idx, q2_idx, q1_idx)) return true; + if (o3 == 0 && on_segment(points, p2_idx, p1_idx, q2_idx)) return true; + if (o4 == 0 && on_segment(points, p2_idx, q1_idx, q2_idx)) return true; - p = m; - double mx = m->x(); - double my = m->y(); + return false; +} - do { - if ( - hx >= p->x() && p->x() >= mx && hx != p->x() && - point_in_triangle(hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, p->x(), p->y())) { - const auto tan_cur = std::abs(hy - p->y()) / (hx - p->x()); +bool intersects_polygon( + const std::vector & points, const std::size_t a_idx, const std::size_t b_idx) +{ + std::size_t p_idx = a_idx; + std::optional p_next_opt = points[p_idx].next_index; - if ( - locally_inside(p, hole) && - (tan_cur < tan_min || - (tan_cur == tan_min && (p->x() > m->x() || sector_contains_sector(m, p))))) { - m = p; - tan_min = tan_cur; + while (p_next_opt.has_value() && p_next_opt.value() != a_idx) { + std::size_t p_next_idx = p_next_opt.value(); + + if (p_idx != a_idx && p_next_idx != a_idx && p_idx != b_idx && p_next_idx != b_idx) { + if (intersects(p_idx, p_next_idx, a_idx, b_idx, points)) { + return true; } } - p = p->next; - } while (p != stop); + p_idx = p_next_idx; + p_next_opt = points[p_idx].next_index; + } - return m; + return false; } -/// @brief main ear slicing loop which triangulates a polygon using linked list -void EarClipping::ear_clipping_linked(EarClipping::Point * ear, int pass) +bool is_valid_diagonal( + const std::size_t a_idx, const std::size_t b_idx, const std::vector & points) { - if (!ear) return; + if ( + !points[a_idx].next_index.has_value() || !points[a_idx].prev_index.has_value() || + !points[b_idx].next_index.has_value() || !points[b_idx].prev_index.has_value()) { + return false; + } - EarClipping::Point * stop = ear; - EarClipping::Point * next = nullptr; + std::size_t a_next_idx = points[a_idx].next_index.value(); + std::size_t a_prev_idx = points[a_idx].prev_index.value(); + std::size_t b_next_idx = points[b_idx].next_index.value(); + std::size_t b_prev_idx = points[b_idx].prev_index.value(); - // Iterate through ears, slicing them one by one - while (ear->prev != ear->next) { - next = ear->next; + if (a_next_idx == b_idx || a_prev_idx == b_idx || intersects_polygon(points, a_idx, b_idx)) { + return false; + } - if (is_ear(ear)) { - // Cut off the triangle - indices.emplace_back(ear->prev->i); - indices.emplace_back(ear->i); - indices.emplace_back(next->i); + bool is_locally_inside_ab = locally_inside(a_idx, b_idx, points); + bool is_locally_inside_ba = locally_inside(b_idx, a_idx, points); + bool is_middle_inside = middle_inside(a_idx, b_idx, points); - remove_point(ear); - ear = next->next; - stop = next->next; + bool is_valid_diagonal = + (is_locally_inside_ab && is_locally_inside_ba && is_middle_inside && + (area(points, a_prev_idx, a_idx, b_prev_idx) != 0.0 || + area(points, a_idx, b_prev_idx, b_idx) != 0.0)) || + (equals(a_idx, b_idx, points) && area(points, a_prev_idx, a_idx, a_next_idx) > 0 && + area(points, b_prev_idx, b_idx, b_next_idx) > 0); - continue; - } + return is_valid_diagonal; +} - ear = next; - if (ear == stop) { - if (!pass) { - ear_clipping_linked(filter_points(ear), 1); - } else if (pass == 1) { - ear = cure_local_intersections(filter_points(ear)); - ear_clipping_linked(ear, 2); - } else if (pass == 2) { - split_ear_clipping(ear); - } - break; +std::size_t insert_point( + const Point2d & pt, std::vector & points, + const std::optional last_index) +{ + std::size_t p_idx = points.size(); + points.push_back(LinkedPoint(pt)); + + // Making sure all next_index and prev_index will always have values + if (!last_index.has_value()) { + points[p_idx].prev_index = p_idx; + points[p_idx].next_index = p_idx; + } else { + std::size_t last = last_index.value(); + std::size_t next = points[last].next_index.value(); + points[p_idx].prev_index = last; + points[p_idx].next_index = next; + points[last].next_index = p_idx; + if (next != p_idx) { + points[next].prev_index = p_idx; } } + + return p_idx; } -/// @brief check whether ear is valid -bool EarClipping::is_ear(EarClipping::Point * ear) +std::size_t linked_list( + const LinearRing2d & ring, const bool clockwise, std::size_t & vertices, + std::vector & points) { - const EarClipping::Point * a = ear->prev; - const EarClipping::Point * b = ear; - const EarClipping::Point * c = ear->next; + double sum = 0; + const std::size_t len = ring.size(); + std::optional last_index = std::nullopt; + + for (std::size_t i = 0, j = len > 0 ? len - 1 : 0; i < len; j = i++) { + const auto & p1 = ring[i]; + const auto & p2 = ring[j]; + sum += (p2.x() - p1.x()) * (p1.y() + p2.y()); + } - if (area(a, b, c) >= 0) return false; // Reflex, can't be an ear + if (clockwise == (sum > 0)) { + for (const auto & point : ring) { + last_index = insert_point(point, points, last_index); + } + } else { + for (auto it = ring.rbegin(); it != ring.rend(); ++it) { + last_index = insert_point(*it, points, last_index); + } + } - EarClipping::Point * p = ear->next->next; + if (last_index.has_value()) { + std::size_t last_idx_value = last_index.value(); + std::optional next_index = points[last_idx_value].next_index; - while (p != ear->prev) { - if ( - point_in_triangle(a->x(), a->y(), b->x(), b->y(), c->x(), c->y(), p->x(), p->y()) && - area(p->prev, p, p->next) >= 0) - return false; - p = p->next; + if (next_index.has_value() && equals(last_idx_value, next_index.value(), points)) { + std::size_t next_idx_value = next_index.value(); + remove_point(last_idx_value, points); + last_index = next_idx_value; + } } - return true; + vertices += len; + return last_index.value(); } -/// @brief go through all polygon Points and cure small local self-intersections -EarClipping::Point * EarClipping::cure_local_intersections(EarClipping::Point * start) +bool sector_contains_sector( + const std::size_t m_idx, const std::size_t p_idx, const std::vector & points) { - EarClipping::Point * p = start; - do { - EarClipping::Point * a = p->prev; - EarClipping::Point * b = p->next->next; + if (!points[m_idx].prev_index.has_value() || !points[m_idx].next_index.has_value()) { + return false; + } - if ( - !equals(a, b) && intersects(a, p, p->next, b) && locally_inside(a, b) && - locally_inside(b, a)) { - indices.emplace_back(a->i); - indices.emplace_back(p->i); - indices.emplace_back(b->i); - remove_point(p); - remove_point(p->next); - - p = start = b; - } - p = p->next; - } while (p != start); + std::size_t m_prev_idx = points[m_idx].prev_index.value(); + std::size_t m_next_idx = points[m_idx].next_index.value(); - return filter_points(p); + return area(points, m_prev_idx, m_idx, p_idx) < 0 && area(points, p_idx, m_next_idx, m_idx) < 0; } -/// @brief splitting polygon into two and triangulate them independently -void EarClipping::split_ear_clipping(EarClipping::Point * start) +std::size_t find_hole_bridge( + const std::size_t hole_index, const std::size_t outer_point_index, + const std::vector & points) { - EarClipping::Point * a = start; - do { - EarClipping::Point * b = a->next->next; - while (b != a->prev) { - if (a->i != b->i && is_valid_diagonal(a, b)) { - EarClipping::Point * c = split_polygon(a, b); + std::size_t p = outer_point_index; + double hx = points[hole_index].x(); + double hy = points[hole_index].y(); + double qx = -std::numeric_limits::infinity(); + std::optional bridge_index = std::nullopt; + std::size_t next_index = points[p].next_index.value(); + + while (p != outer_point_index) { + if ( + hy <= points[p].y() && hy >= points[next_index].y() && + points[next_index].y() != points[p].y()) { + double x = points[p].x() + (hy - points[p].y()) * (points[next_index].x() - points[p].x()) / + (points[next_index].y() - points[p].y()); + if (x <= hx && x > qx) { + qx = x; + bridge_index = (points[p].x() < points[next_index].x()) ? p : next_index; + if (x == hx) return bridge_index.value(); + } + } + p = next_index; + next_index = points[p].next_index.value(); + } - a = filter_points(a, a->next); - c = filter_points(c, c->next); + if (!bridge_index.has_value()) return outer_point_index; - ear_clipping_linked(a); - ear_clipping_linked(c); - return; + const std::size_t stop = bridge_index.value(); + double min_tan = std::numeric_limits::infinity(); + + p = bridge_index.value(); + double mx = points[p].x(); + double my = points[p].y(); + next_index = points[p].next_index.value(); + + while (p != stop) { + if ( + hx >= points[p].x() && points[p].x() >= mx && hx != points[p].x() && + point_in_triangle( + hy < my ? hx : qx, hy, mx, my, hy < my ? qx : hx, hy, points[p].x(), points[p].y())) { + double current_tan = std::abs(hy - points[p].y()) / (hx - points[p].x()); + + if ( + locally_inside(p, hole_index, points) && + (current_tan < min_tan || + (current_tan == min_tan && (points[p].x() > points[bridge_index.value()].x() || + sector_contains_sector(bridge_index.value(), p, points))))) { + bridge_index = p; + min_tan = current_tan; } - b = b->next; } - a = a->next; - } while (a != start); -} -/// @brief check whether sector in vertex m contains sector in vertex p in the same coordinates -bool EarClipping::sector_contains_sector(const EarClipping::Point * m, const EarClipping::Point * p) -{ - return area(m->prev, m, p->prev) < 0 && area(p->next, m, m->next) < 0; + p = next_index; + next_index = points[p].next_index.value(); + } + + return bridge_index.value(); } -/// @brief find the leftmost Point of a polygon ring -EarClipping::Point * EarClipping::get_leftmost(EarClipping::Point * start) +std::size_t split_polygon( + std::size_t a_index, std::size_t b_index, std::vector & points) { - EarClipping::Point * p = start; - EarClipping::Point * leftmost = start; - do { - if (p->x() < leftmost->x() || (p->x() == leftmost->x() && p->y() < leftmost->y())) leftmost = p; - p = p->next; - } while (p != start); + std::size_t an_idx = points[a_index].next_index.value(); + std::size_t bp_idx = points[b_index].prev_index.value(); - return leftmost; -} + std::size_t a2_idx = points.size(); + std::size_t b2_idx = points.size() + 1; + points.push_back(points[a_index]); + points.push_back(points[b_index]); -/// @brief check if a point lies within a convex triangle -bool EarClipping::point_in_triangle( - double ax, double ay, double bx, double by, double cx, double cy, double px, double py) -{ - return (cx - px) * (ay - py) >= (ax - px) * (cy - py) && - (ax - px) * (by - py) >= (bx - px) * (ay - py) && - (bx - px) * (cy - py) >= (cx - px) * (by - py); -} + points[a_index].next_index = b_index; + points[a2_idx].prev_index = b2_idx; + points[a2_idx].next_index = an_idx; -/// @brief check if a diagonal between two polygon Points is valid -bool EarClipping::is_valid_diagonal(EarClipping::Point * a, EarClipping::Point * b) -{ - return a->next->i != b->i && a->prev->i != b->i && !intersects_polygon(a, b) && - ((locally_inside(a, b) && locally_inside(b, a) && middle_inside(a, b) && - (area(a->prev, a, b->prev) != 0.0 || area(a, b->prev, b) != 0.0)) || - (equals(a, b) && area(a->prev, a, a->next) > 0 && area(b->prev, b, b->next) > 0)); -} + points[b_index].prev_index = a_index; + points[an_idx].prev_index = b2_idx; + points[b2_idx].next_index = a2_idx; + points[b2_idx].prev_index = bp_idx; -/// @brief signed area of a triangle -double EarClipping::area( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) -{ - return (q->y() - p->y()) * (r->x() - q->x()) - (q->x() - p->x()) * (r->y() - q->y()); -} + if (bp_idx != b_index) { + points[bp_idx].next_index = b2_idx; + } -/// @brief check if two points are equal -bool EarClipping::equals(const EarClipping::Point * p1, const EarClipping::Point * p2) -{ - return p1->x() == p2->x() && p1->y() == p2->y(); + return b2_idx; } -/// @brief check if two segments intersect -bool EarClipping::intersects( - const EarClipping::Point * p1, const EarClipping::Point * q1, const EarClipping::Point * p2, - const EarClipping::Point * q2) +std::size_t filter_points( + const std::size_t start_index, const std::size_t end_index, std::vector & points) { - int o1 = sign(area(p1, q1, p2)); - int o2 = sign(area(p1, q1, q2)); - int o3 = sign(area(p2, q2, p1)); - int o4 = sign(area(p2, q2, q1)); + auto p = start_index; + bool again = true; - if (o1 != o2 && o3 != o4) return true; + while (again && p != end_index) { + again = false; - if (o1 == 0 && on_segment(p1, p2, q1)) return true; - if (o2 == 0 && on_segment(p1, q2, q1)) return true; - if (o3 == 0 && on_segment(p2, p1, q2)) return true; - if (o4 == 0 && on_segment(p2, q1, q2)) return true; + if ( + !points[p].steiner && + (equals(p, points[p].next_index.value(), points) || + area(points, points[p].prev_index.value(), p, points[p].next_index.value()) == 0)) { + remove_point(p, points); + p = points[p].prev_index.value(); + + if (p == points[p].next_index.value()) { + break; + } + again = true; + } else { + p = points[p].next_index.value(); + } + } - return false; + return end_index; } -/// @brief for collinear points p, q, r, check if point q lies on segment pr -bool EarClipping::on_segment( - const EarClipping::Point * p, const EarClipping::Point * q, const EarClipping::Point * r) +std::size_t eliminate_hole( + const std::size_t hole_index, const std::size_t outer_index, std::vector & points) { - return q->x() <= std::max(p->x(), r->x()) && q->x() >= std::min(p->x(), r->x()) && - q->y() <= std::max(p->y(), r->y()) && q->y() >= std::min(p->y(), r->y()); + auto bridge = find_hole_bridge(hole_index, outer_index, points); + auto bridge_reverse = split_polygon(bridge, hole_index, points); + + auto next_index_bridge_reverse = points[bridge_reverse].next_index.value(); + filter_points(bridge_reverse, next_index_bridge_reverse, points); + + auto next_index_bridge = points[bridge].next_index.value(); + return filter_points(bridge, next_index_bridge, points); } -/// @brief Sign function for area calculation -int EarClipping::sign(double val) +std::size_t eliminate_holes( + const std::vector & inners, std::size_t outer_index, std::size_t & vertices, + std::vector & points) { - return (0.0 < val) - (val < 0.0); + std::vector queue; + + for (const auto & ring : inners) { + auto inner_index = linked_list(ring, false, vertices, points); + + if (points[inner_index].next_index.value() == inner_index) { + points[inner_index].steiner = true; + } + + queue.push_back(get_leftmost(inner_index, points)); + } + + std::sort(queue.begin(), queue.end(), [&](std::size_t a, std::size_t b) { + return points[a].x() < points[b].x(); + }); + + for (const auto & q : queue) { + outer_index = eliminate_hole(q, outer_index, points); + } + + return outer_index; } -/// @brief Check if a polygon diagonal intersects any polygon segments -bool EarClipping::intersects_polygon(const EarClipping::Point * a, const EarClipping::Point * b) +bool is_ear(const std::size_t ear_index, const std::vector & points) { - const EarClipping::Point * p = a; - do { + const auto a_index = points[ear_index].prev_index.value(); + const auto b_index = ear_index; + const auto c_index = points[ear_index].next_index.value(); + + const auto a = points[a_index]; + const auto b = points[b_index]; + const auto c = points[c_index]; + + if (area(points, a_index, b_index, c_index) >= 0) return false; + auto p_index = points[c_index].next_index.value(); + while (p_index != a_index) { + const auto p = points[p_index]; if ( - p->i != a->i && p->next->i != a->i && p->i != b->i && p->next->i != b->i && - intersects(p, p->next, a, b)) - return true; - p = p->next; - } while (p != a); + point_in_triangle(a.x(), a.y(), b.x(), b.y(), c.x(), c.y(), p.x(), p.y()) && + area(points, p.prev_index.value(), p_index, p.next_index.value()) >= 0) { + return false; + } + p_index = points[p_index].next_index.value(); + } - return false; + return true; } -/// @brief Check if a polygon diagonal is locally inside the polygon -bool EarClipping::locally_inside(const EarClipping::Point * a, const EarClipping::Point * b) +std::size_t cure_local_intersections( + std::size_t start_index, std::vector & indices, std::vector & points) { - return area(a->prev, a, a->next) < 0 ? area(a, b, a->next) >= 0 && area(a, a->prev, b) >= 0 - : area(a, b, a->prev) < 0 || area(a, a->next, b) < 0; + auto p = start_index; + bool updated = false; + + while (p != start_index || updated) { + updated = false; + auto a_idx = points[p].prev_index.value(); + auto b_idx = points[points[p].next_index.value()].next_index.value(); + + if ( + !equals(a_idx, b_idx, points) && + intersects( + a_idx, p, points[points[p].next_index.value()].next_index.value(), b_idx, points) && + locally_inside(a_idx, b_idx, points) && locally_inside(b_idx, a_idx, points)) { + indices.push_back(a_idx); + indices.push_back(p); + indices.push_back(b_idx); + + remove_point(p, points); + remove_point(points[p].next_index.value(), points); + + p = start_index = b_idx; + updated = true; + } else { + p = points[p].next_index.value(); + } + } + + return filter_points(p, p, points); } -/// @brief Check if the middle vertex of a polygon diagonal is inside the polygon -bool EarClipping::middle_inside(const EarClipping::Point * a, const EarClipping::Point * b) +void split_ear_clipping( + std::vector & points, const std::size_t start_idx, + std::vector & indices) { - const EarClipping::Point * p = a; - bool inside = false; - double px = (a->x() + b->x()) / 2; - double py = (a->y() + b->y()) / 2; + std::size_t a_idx = start_idx; do { - if ( - ((p->y() > py) != (p->next->y() > py)) && p->next->y() != p->y() && - (px < (p->next->x() - p->x()) * (py - p->y()) / (p->next->y() - p->y()) + p->x())) - inside = !inside; - p = p->next; - } while (p != a); + std::size_t b_idx = points[points[a_idx].next_index.value()].next_index.value(); + while (b_idx != points[a_idx].prev_index.value()) { + if (a_idx != b_idx && is_valid_diagonal(a_idx, b_idx, points)) { + std::size_t c_idx = split_polygon(a_idx, b_idx, points); - return inside; + a_idx = filter_points(start_idx, points[a_idx].next_index.value(), points); + c_idx = filter_points(start_idx, points[c_idx].next_index.value(), points); + + ear_clipping_linked(a_idx, indices, points); + ear_clipping_linked(c_idx, indices, points); + return; + } + b_idx = points[b_idx].next_index.value(); + } + a_idx = points[a_idx].next_index.value(); + } while (a_idx != start_idx); } -/// @brief Link two polygon vertices with a bridge -EarClipping::Point * EarClipping::split_polygon(EarClipping::Point * a, EarClipping::Point * b) +void ear_clipping_linked( + std::size_t ear_index, std::vector & indices, std::vector & points, + const int pass) { - Point2d a_point(a->x(), a->y()); - Point2d b_point(b->x(), b->y()); + auto stop = ear_index; + std::optional next = std::nullopt; - // Use construct_point to create new Point objects - EarClipping::Point * a2 = construct_point(a->i, a_point); - EarClipping::Point * b2 = construct_point(b->i, b_point); + while (points[ear_index].prev_index.value() != points[ear_index].next_index.value()) { + next = points[ear_index].next_index; - EarClipping::Point * an = a->next; - EarClipping::Point * bp = b->prev; + if (is_ear(ear_index, points)) { + indices.push_back(points[ear_index].prev_index.value()); + indices.push_back(ear_index); + indices.push_back(next.value()); - // Update the linked list connections - a->next = b; - b->prev = a; + remove_point(ear_index, points); - a2->next = an; - if (an) { - an->prev = a2; - } + ear_index = points[next.value()].next_index.value(); + stop = points[next.value()].next_index.value(); + continue; + } - b2->next = a2; - a2->prev = b2; + ear_index = next.value(); - if (bp) { - bp->next = b2; + if (ear_index == stop) { + if (pass == 0) { + ear_clipping_linked(filter_points(ear_index, ear_index, points), indices, points, 1); + } else if (pass == 1) { + ear_index = + cure_local_intersections(filter_points(ear_index, ear_index, points), indices, points); + ear_clipping_linked(ear_index, indices, points, 2); + } else if (pass == 2) { + split_ear_clipping(points, ear_index, indices); + } + break; + } } - b2->prev = bp; - - return b2; } -/// @brief create a Point and optionally link it with the previous one (in a circular doubly linked -/// list) -EarClipping::Point * EarClipping::insert_point( - std::size_t i, const Point2d & pt, EarClipping::Point * last) +std::vector perform_triangulation( + const Polygon2d & polygon, std::vector & indices) { - EarClipping::Point * p = construct_point(static_cast(i), pt); + indices.clear(); + std::vector points; + std::size_t vertices = 0; + const auto & outer_ring = polygon.outer(); + std::size_t len = outer_ring.size(); + points.reserve(len * 3 / 2); - if (!last) { - p->prev = p; - p->next = p; - } else { - assert(last); - p->next = last->next; - p->prev = last; - last->next->prev = p; - last->next = p; + if (polygon.outer().empty()) return points; + + indices.reserve(len + outer_ring.size()); + auto outer_point_index = linked_list(outer_ring, true, vertices, points); + if ( + !points[outer_point_index].prev_index.has_value() || + outer_point_index == points[outer_point_index].prev_index.value()) { + return points; } - return p; -} -/// @brief remove a Point from the linked list -void EarClipping::remove_point(EarClipping::Point * p) -{ - p->next->prev = p->prev; - p->prev->next = p->next; + if (!polygon.inners().empty()) { + outer_point_index = eliminate_holes(polygon.inners(), outer_point_index, vertices, points); + } + + ear_clipping_linked(outer_point_index, indices, points); + return points; } -/// @brief main triangulate function std::vector triangulate(const Polygon2d & poly) { - autoware::universe_utils::EarClipping triangulate; - triangulate(poly); + std::vector indices; + auto points = perform_triangulation(poly, indices); std::vector triangles; - - const auto & indices = triangulate.indices; const std::size_t num_indices = indices.size(); if (num_indices % 3 != 0) { throw std::runtime_error("Indices size should be a multiple of 3"); } - // Gather all vertices from outer and inner rings - std::vector all_vertices; - const auto & outer_ring = poly.outer(); - all_vertices.insert(all_vertices.end(), outer_ring.begin(), outer_ring.end()); - - for (const auto & inner_ring : poly.inners()) { - all_vertices.insert(all_vertices.end(), inner_ring.begin(), inner_ring.end()); - } - - const std::size_t total_vertices = all_vertices.size(); - for (std::size_t i = 0; i < num_indices; i += 3) { - if ( - indices[i] >= total_vertices || indices[i + 1] >= total_vertices || - indices[i + 2] >= total_vertices) { - throw std::runtime_error("Index out of range"); - } - Polygon2d triangle; - triangle.outer().push_back(all_vertices[indices[i]]); - triangle.outer().push_back(all_vertices[indices[i + 1]]); - triangle.outer().push_back(all_vertices[indices[i + 2]]); - triangle.outer().push_back(all_vertices[indices[i]]); // Close the triangle + triangle.outer().push_back(points[indices[i]].pt); + triangle.outer().push_back(points[indices[i + 1]].pt); + triangle.outer().push_back(points[indices[i + 2]].pt); + triangle.outer().push_back(points[indices[i]].pt); triangles.push_back(triangle); } - + points.clear(); return triangles; } } // namespace autoware::universe_utils - -#endif // EAR_CUT_IMPL_HPP