diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index 72486b5de8818..311dc59ae12e4 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -28,6 +28,7 @@ ament_auto_add_library(autoware_universe_utils SHARED src/system/backtrace.cpp src/system/time_keeper.cpp src/geometry/ear_clipping.cpp + src/geometry/polygon_clip.cpp ) target_link_libraries(autoware_universe_utils diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/polygon_clip.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/polygon_clip.hpp new file mode 100644 index 0000000000000..e323b9e31264b --- /dev/null +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/polygon_clip.hpp @@ -0,0 +1,242 @@ +// 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__POLYGON_CLIP_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POLYGON_CLIP_HPP_ + +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ +namespace polygon_clip +{ +struct LinkedVertex +{ + double x; + double y; + std::optional next; + std::optional prev; + std::optional corresponding; + double distance; + bool is_entry; + bool is_intersection; + bool visited; + + LinkedVertex() + : x(0.0), + y(0.0), + next(std::nullopt), + prev(std::nullopt), + corresponding(std::nullopt), + distance(0.0), + is_entry(false), + is_intersection(false), + visited(false) + { + } + + LinkedVertex( + double x_coord, double y_coord, std::optional next_index = std::nullopt, + std::optional prev_index = std::nullopt, + std::optional corresponding_index = std::nullopt, double dist = 0.0, + bool entry = false, bool intersection = false, bool visited_state = false) + : x(x_coord), + y(y_coord), + next(next_index), + prev(prev_index), + corresponding(corresponding_index), + distance(dist), + is_entry(entry), + is_intersection(intersection), + visited(visited_state) + { + } +}; + +struct Intersection +{ + double x; + double y; + double distance_to_source; + double distance_to_clip; +}; + +struct ExtendedPolygon +{ + std::size_t first; + std::vector vertices; + std::optional last_unprocessed; + std::optional first_intersect; + + ExtendedPolygon() + : first(0), vertices(0), last_unprocessed(std::nullopt), first_intersect(std::nullopt) + { + } +}; + +/** + * @brief creates an intersection between two polygon edges defined by vertex indices. + * @param source_vertices a vector of LinkedVertex objects representing the source polygon's + * vertices. + * @param s1_index index of the first vertex of the source polygon edge. + * @param s2_index index of the second vertex of the source polygon edge. + * @param clip_vertices a vector of LinkedVertex objects representing the clipping polygon's + * vertices. + * @param c1_index index of the first vertex of the clipping polygon edge. + * @param c2_index index of the second vertex of the clipping polygon edge. + * @return intersection object with point and distance to each vertex. + */ +Intersection intersection( + const std::vector & source_vertices, std::size_t s1_index, std::size_t s2_index, + const std::vector & clip_vertices, std::size_t c1_index, std::size_t c2_index); + +/** + * @brief creates an ExtendedPolygon from a Polygon2d object. + * @param poly2d the input Polygon2d object. + * @return the created ExtendedPolygon object. + */ +ExtendedPolygon create_extended_polygon(const autoware::universe_utils::Polygon2d & poly2d); + +/** + * @brief adds a new vertex to the given ExtendedPolygon, updating the linked vertex structure. + * @details inserts the new vertex at the end of the polygon's vertices list and updates + * the doubly linked list's `next` and `prev` pointers. It also ensures that the new vertex links + * properly with the previous and next vertices in the polygon. + * + * @param polygon The polygon to which the vertex will be added. + * @param new_vertex The vertex to add. + * @param last_index The index of the last vertex before adding the new one (used to link the new + * vertex). Defaults to 0. + * + * @return The index of the newly added vertex. + */ +std::size_t add_vertex( + ExtendedPolygon & polygon, const LinkedVertex & new_vertex, const std::size_t last_index = 0); + +/** + * @brief inserts a vertex into a vector of vertices between a given start and end index. + * @details traverses the vector of vertices from `start_index` to `end_index` and + * compares the distances of the vertices to find the correct insertion point for the new vertex. + * once the appropriate position is found, it updates the `next` and `prev` pointers of the + * surrounding vertices to maintain the doubly linked list structure. + * + * @param vertices A vector of LinkedVertex objects representing the vertices of the polygon. + * @param vertex The vertex to be inserted. + * @param start_index The starting index of the range in which to insert the vertex. + * @param end_index The ending index of the range in which to insert the vertex. + */ +void insert_vertex( + std::vector & vertices, const std::size_t & vertex_index, const std::size_t start, + const std::size_t end); + +/** + * @brief gets the index of the first intersection vertex in the polygon. + * @param polygon the ExtendedPolygon to check. + * @return the index of the first intersection vertex. + */ +std::size_t get_first_intersect(ExtendedPolygon & polygon); + +// cSpell:ignore Greiner, Hormann +/** + * @brief Clips one polygon using another and returns the resulting polygons. + * @param source The source polygon to be clipped. + * @param clip The clipping polygon. + * @param source_forwards Specifies the direction of traversal for the source polygon. + * @param clip_forwards Specifies the direction of traversal for the clipping polygon. + * @details Based on Greiner-Hormann algorithm + * https://www.inf.usi.ch/faculty/hormann/papers/Greiner.1998.ECO.pdf Greiner-Hormann algorithm + * cannot handle degenerate cases, e.g. when intersections are falling on the edges of the polygon, + * or vertices coincide. source_forwards and clip_forwards for each function: + * - false, true : difference + * - false, false : union + * - true, true : intersection + * The Clipping is splitted into three main parts: + * 1. Marking Intersections + * 2. Identify Entry and Exit Points + * 3. Construct Clipped Polygons + * @return A vector of Polygon2d objects representing the clipped result. + * + * @note This implementation may encounter precision errors due to the + * difference in the methods used to calculate intersection points with the + * Boost libraries. Results have been validated to be correct when compared + * to the Shapely library in Python, and any discrepancies are + * primarily due to the difference in the way Boost.Geometry handles + * intersection points. The difference could reach 1.0 error. + * In a cases where the found clipped polygon is small, Boost geometry will ignore/simplify the + * multi polygon thus ignoring the small/negligible polygon. But that is not the case is shapely and + * our custom polygon clip, as shown in 'test_geometry.cpp' line. + */ +std::vector clip( + ExtendedPolygon & source, ExtendedPolygon & clip, bool source_forwards, bool clip_forwards); + +} // namespace polygon_clip + +/** + * @brief computes the difference between two polygons. + * @param polygon_a The source polygon. + * @param polygon_b The clip polygon. + * @return a vector of Polygon2d objects representing the difference. + */ +std::vector difference( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b); + +/** + * @brief computes the union of two polygons. + * @param polygon_a The source polygon. + * @param polygon_b The clip polygon. + * @return a vector of Polygon2d objects representing the union. + */ +std::vector union_( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b); + +/** + * @brief computes the intersection of two polygons. + * @param polygon_a The source polygon. + * @param polygon_b The clip polygon. + * @return a vector of Polygon2d objects representing the intersection. + */ +std::vector intersection( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b); + +/** + * @brief Calculates the intersection point between two line segments. + * @details + * This function computes the intersection point of two line segments defined by their endpoints: + * (s1, s2) for the source segment and (c1, c2) for the clipping segment. + * @param s1 The first endpoint of the source segment. + * @param s2 The second endpoint of the source segment. + * @param c1 The first endpoint of the clipping segment. + * @param c2 The second endpoint of the clipping segment. + * @return A vector of intersection points (typically one or none). + */ + +std::vector intersection( + const autoware::universe_utils::Point2d & s1, const autoware::universe_utils::Point2d & s2, + const autoware::universe_utils::Point2d & c1, const autoware::universe_utils::Point2d & c2); + +std::vector intersection( + const autoware::universe_utils::Segment2d & source_segment, + const autoware::universe_utils::Segment2d & clip_segment); + +} // namespace autoware::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__POLYGON_CLIP_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/polygon_clip.cpp b/common/autoware_universe_utils/src/geometry/polygon_clip.cpp new file mode 100644 index 0000000000000..b24aa5012086e --- /dev/null +++ b/common/autoware_universe_utils/src/geometry/polygon_clip.cpp @@ -0,0 +1,542 @@ +// 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. + +#include "autoware/universe_utils/geometry/polygon_clip.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::universe_utils +{ + +namespace polygon_clip +{ + +void visit( + std::vector & vertices, std::vector & vertices_2, std::size_t index) +{ + LinkedVertex & vertex = vertices[index]; + vertex.visited = true; + + if (vertex.corresponding.has_value()) { + std::size_t corresponding_index = vertex.corresponding.value(); + if (corresponding_index < vertices_2.size()) { + if (!vertices_2[corresponding_index].visited) { + visit(vertices_2, vertices, corresponding_index); + } + } + } +} + +bool is_inside(const LinkedVertex & v, const ExtendedPolygon & poly) +{ + bool contains = false; + constexpr double tolerance = 1e-9; + + std::size_t vertexIndex = poly.first; + std::size_t next_index = poly.vertices[vertexIndex].next.value_or(poly.first); + + do { + const LinkedVertex & vertex = poly.vertices[vertexIndex]; + const LinkedVertex & next = poly.vertices[next_index]; + + bool y_intersects = ((next.y < v.y) != (vertex.y < v.y)) && + (v.x < (vertex.x - next.x) * (v.y - next.y) / (vertex.y - next.y) + next.x); + + if (y_intersects) { + contains = !contains; + + if (std::abs(vertex.x - next.x) < tolerance && std::abs(vertex.y - next.y) < tolerance) { + vertexIndex = next_index; + next_index = poly.vertices[vertexIndex].next.value_or(poly.first); + continue; + } + } + + vertexIndex = next_index; + next_index = poly.vertices[vertexIndex].next.value_or(poly.first); + } while (vertexIndex != poly.first); + + return contains; +} + +Intersection intersection( + const std::vector & source_vertices, std::size_t s1_index, std::size_t s2_index, + const std::vector & clip_vertices, std::size_t c1_index, std::size_t c2_index) +{ + Intersection intersection_; + + const LinkedVertex & s1 = source_vertices[s1_index]; + const LinkedVertex & s2 = source_vertices[s2_index]; + const LinkedVertex & c1 = clip_vertices[c1_index]; + const LinkedVertex & c2 = clip_vertices[c2_index]; + + double d = (c2.y - c1.y) * (s2.x - s1.x) - (c2.x - c1.x) * (s2.y - s1.y); + + if (std::abs(d) > 1e-9) { + double t1 = ((c2.x - c1.x) * (s1.y - c1.y) - (c2.y - c1.y) * (s1.x - c1.x)) / d; + double t2 = ((s2.x - s1.x) * (s1.y - c1.y) - (s2.y - s1.y) * (s1.x - c1.x)) / d; + + if (t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1) { + intersection_.x = t1 * s2.x + (1.0 - t1) * s1.x; + intersection_.y = t1 * s2.y + (1.0 - t1) * s1.y; + intersection_.distance_to_source = t1; + intersection_.distance_to_clip = t2; + return intersection_; + } + } + + intersection_.x = std::numeric_limits::quiet_NaN(); + intersection_.y = std::numeric_limits::quiet_NaN(); + intersection_.distance_to_source = 0; + intersection_.distance_to_clip = 0; + + return intersection_; +} + +bool valid(const Intersection & intersection_) +{ + return (intersection_.distance_to_source > 0 && intersection_.distance_to_source < 1) && + (intersection_.distance_to_clip > 0 && intersection_.distance_to_clip < 1); +} + +std::size_t add_vertex( + ExtendedPolygon & polygon, const LinkedVertex & new_vertex, const std::size_t last_index) +{ + std::size_t p_idx = polygon.vertices.size(); + polygon.vertices.push_back(new_vertex); + if (!polygon.vertices.empty()) { + std::size_t last = last_index; + std::size_t next = polygon.vertices[last].next.value_or(0); + polygon.vertices[p_idx].prev = last; + polygon.vertices[p_idx].next = next; + polygon.vertices[last].next = p_idx; + if (next != p_idx) { + polygon.vertices[next].prev = p_idx; + } + } + return p_idx; +} + +ExtendedPolygon create_extended_polygon(const autoware::universe_utils::Polygon2d & poly2d) +{ + ExtendedPolygon polygon; + + const auto & outer = poly2d.outer(); + polygon.vertices.resize(outer.size()); + + for (std::size_t i = 0; i < outer.size(); ++i) { + const auto & point = outer[i]; + LinkedVertex vertex{point.x(), point.y()}; + + vertex.prev = (i == 0) ? outer.size() - 1 : i - 1; + vertex.next = (i + 1) % outer.size(); + + polygon.vertices[i] = vertex; + } + return polygon; +} + +ExtendedPolygon create_extended_polygon(const LinkedVertex & vertex) +{ + ExtendedPolygon polygon; + + polygon.vertices.push_back(vertex); + + polygon.vertices.back().prev = std::nullopt; + polygon.vertices.back().next = std::nullopt; + return polygon; +} + +void insert_vertex( + std::vector & vertices, const std::size_t & vertex_index, + const std::size_t start_index, const std::size_t end_index) +{ + std::size_t current_index = start_index; + + while (current_index != end_index && + vertices[current_index].distance < vertices[vertex_index].distance) { + current_index = vertices[current_index].next.value(); + } + + vertices[vertex_index].next = current_index; + vertices[vertex_index].prev = vertices[current_index].prev.value(); + std::size_t prev_index = vertices[current_index].prev.value(); + + if (prev_index != current_index) { + vertices[prev_index].next = vertex_index; + } + vertices[current_index].prev = vertex_index; + + if (current_index == start_index) { + vertices[vertex_index].prev = start_index; + vertices[vertex_index].next = start_index; + } +} + +std::size_t get_next(std::size_t index, const std::vector & vertices) +{ + std::size_t current_index = index; + while (vertices[current_index].is_intersection) { + current_index = vertices[current_index].next.value(); + } + return current_index; +} + +std::size_t get_first_intersect(ExtendedPolygon & polygon) +{ + std::size_t v = + polygon.first_intersect.has_value() ? polygon.first_intersect.value() : polygon.first; + + do { + if (polygon.vertices[v].is_intersection && !polygon.vertices[v].visited) break; + v = polygon.vertices[v].next.value(); + } while (v != polygon.first); + + polygon.first_intersect = v; + return v; +} + +bool has_unprocessed(ExtendedPolygon & polygon) +{ + std::size_t v = + polygon.last_unprocessed.has_value() ? polygon.last_unprocessed.value() : polygon.first; + + do { + if (polygon.vertices[v].is_intersection && !polygon.vertices[v].visited) { + polygon.last_unprocessed = v; + return true; + } + v = polygon.vertices[v].next.value(); + } while (v != polygon.first); + polygon.last_unprocessed = std::nullopt; + return false; +} + +autoware::universe_utils::Polygon2d get_points(const ExtendedPolygon & polygon) +{ + autoware::universe_utils::Polygon2d poly; + std::size_t v_index = polygon.first; + std::size_t start_index = v_index; + autoware::universe_utils::LinearRing2d outer_ringA; + std::set> unique_points; + + do { + const auto & vertex = polygon.vertices[v_index]; + autoware::universe_utils::Point2d point(vertex.x, vertex.y); + if (unique_points.insert(std::make_pair(vertex.x, vertex.y)).second) { + outer_ringA.push_back(point); + } + + v_index = vertex.next.value(); + } while (v_index != start_index); + + boost::geometry::append(poly.outer(), outer_ringA); + boost::geometry::correct(poly); + return poly; +} + +void mark_intersections(ExtendedPolygon & source, ExtendedPolygon & clip, bool & intersection_exist) +{ + std::size_t source_vertex_index = source.first; + std::size_t clip_vertex_index; + + do { + if (source.vertices[source_vertex_index].is_intersection) { + source_vertex_index = source.vertices[source_vertex_index].next.value(); + continue; + } + + clip_vertex_index = clip.first; + + do { + if (clip.vertices[clip_vertex_index].is_intersection) { + clip_vertex_index = clip.vertices[clip_vertex_index].next.value(); + continue; + } + + Intersection i = intersection( + source.vertices, source_vertex_index, + get_next(source.vertices[source_vertex_index].next.value(), source.vertices), clip.vertices, + clip_vertex_index, get_next(clip.vertices[clip_vertex_index].next.value(), clip.vertices)); + + if (!valid(i)) { + clip_vertex_index = clip.vertices[clip_vertex_index].next.value(); + continue; + } + + intersection_exist = true; + + LinkedVertex intersection_vertex_1{i.x, i.y, std::nullopt, + std::nullopt, std::nullopt, i.distance_to_source, + false, true, false}; + LinkedVertex intersection_vertex_2{ + i.x, i.y, std::nullopt, std::nullopt, std::nullopt, i.distance_to_clip, false, true, false}; + + source.vertices.push_back(intersection_vertex_1); + clip.vertices.push_back(intersection_vertex_2); + + std::size_t index1 = source.vertices.size() - 1; + std::size_t index2 = clip.vertices.size() - 1; + + source.vertices[index1].corresponding = index2; + clip.vertices[index2].corresponding = index1; + + insert_vertex( + source.vertices, index1, source_vertex_index, + get_next(source.vertices[source_vertex_index].next.value(), source.vertices)); + insert_vertex( + clip.vertices, index2, clip_vertex_index, + get_next(clip.vertices[clip_vertex_index].next.value(), clip.vertices)); + + clip_vertex_index = clip.vertices[clip_vertex_index].next.value(); + } while (clip_vertex_index != clip.first); + + source_vertex_index = source.vertices[source_vertex_index].next.value(); + } while (source_vertex_index != source.first); +} + +void identify_entry_exit( + ExtendedPolygon & source, ExtendedPolygon & clip, bool source_forwards, bool clip_forwards) +{ + std::size_t source_vertex_index = source.first; + std::size_t clip_vertex_index = clip.first; + + bool source_in_clip = is_inside(source.vertices[source_vertex_index], clip); + bool clip_in_source = is_inside(clip.vertices[clip_vertex_index], source); + source_forwards ^= source_in_clip; + clip_forwards ^= clip_in_source; + + do { + if (source.vertices[source_vertex_index].is_intersection) { + source.vertices[source_vertex_index].is_entry = source_forwards; + source_forwards = !source_forwards; + } + source_vertex_index = source.vertices[source_vertex_index].next.value(); + } while (source_vertex_index != source.first); + + do { + if (clip.vertices[clip_vertex_index].is_intersection) { + clip.vertices[clip_vertex_index].is_entry = clip_forwards; + clip_forwards = !clip_forwards; + } + clip_vertex_index = clip.vertices[clip_vertex_index].next.value(); + } while (clip_vertex_index != clip.first); +} + +std::vector construct_clipped_polygons( + ExtendedPolygon & source, ExtendedPolygon & clip, const bool is_union) +{ + std::vector polygon_vector; + + while (has_unprocessed(source)) { + std::size_t currentIndex = get_first_intersect(source); + ExtendedPolygon clipped = create_extended_polygon(source.vertices[currentIndex]); + std::size_t last_idx = 0; + bool usingSource = true; + do { + visit( + usingSource ? source.vertices : clip.vertices, + usingSource ? clip.vertices : source.vertices, currentIndex); + + if (usingSource) { + if (source.vertices[currentIndex].is_entry) { + do { + currentIndex = source.vertices[currentIndex].next.value(); + last_idx = add_vertex(clipped, source.vertices[currentIndex], last_idx); + } while (!source.vertices[currentIndex].is_intersection); + } else { + do { + currentIndex = source.vertices[currentIndex].prev.value(); + last_idx = add_vertex(clipped, source.vertices[currentIndex], last_idx); + } while (!source.vertices[currentIndex].is_intersection); + } + } else { + if (clip.vertices[currentIndex].is_entry) { + do { + currentIndex = clip.vertices[currentIndex].next.value(); + last_idx = add_vertex(clipped, clip.vertices[currentIndex], last_idx); + } while (!clip.vertices[currentIndex].is_intersection); + } else { + do { + currentIndex = clip.vertices[currentIndex].prev.value(); + last_idx = add_vertex(clipped, clip.vertices[currentIndex], last_idx); + } while (!clip.vertices[currentIndex].is_intersection); + } + } + + currentIndex = (usingSource ? source.vertices[currentIndex] : clip.vertices[currentIndex]) + .corresponding.value(); + usingSource = !usingSource; + } while ( + !((usingSource ? source.vertices[currentIndex] : clip.vertices[currentIndex]).visited)); + auto points = get_points(clipped); + + if (is_union && !polygon_vector.empty()) { + const auto & existing_polygon = polygon_vector[0]; + + if (boost::geometry::within(points, existing_polygon)) { + continue; + } else if (boost::geometry::within(existing_polygon, points)) { + polygon_vector[0] = points; + continue; + } + + } else { + polygon_vector.push_back(points); + } + } + return polygon_vector; +} + +std::vector clip( + ExtendedPolygon & source, ExtendedPolygon & clip, bool source_forwards, bool clip_forwards) +{ + bool intersection_exist = false; + mark_intersections(source, clip, intersection_exist); + identify_entry_exit(source, clip, source_forwards, clip_forwards); + + bool is_union = !source_forwards && !clip_forwards; + bool is_intersection = source_forwards && clip_forwards; + + auto polygon_vector = construct_clipped_polygons(source, clip, is_union); + + if (!intersection_exist) { + polygon_vector.clear(); + bool source_in_clip = is_inside(source.vertices[source.first], clip); + bool clip_in_source = is_inside(clip.vertices[clip.first], source); + + if (is_union) { + if (source_in_clip) { + polygon_vector.push_back(get_points(clip)); + } else if (clip_in_source) { + polygon_vector.push_back(get_points(source)); + } else { + polygon_vector.push_back(get_points(source)); + polygon_vector.push_back(get_points(clip)); + } + } else if (is_intersection) { + if (source_in_clip) { + polygon_vector.push_back(get_points(source)); + } else if (clip_in_source) { + polygon_vector.push_back(get_points(clip)); + } + } else { + if (!source_in_clip) { + polygon_vector.push_back(get_points(source)); + } + } + } + + return polygon_vector; +} + +} // namespace polygon_clip + +std::vector apply_operation( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b, bool source_forwards, bool clip_forwards) +{ + if (polygon_a.outer().size() < 3 || polygon_b.outer().size() < 3) { + return std::vector{polygon_a, polygon_b}; + } + + polygon_clip::ExtendedPolygon poly_a = polygon_clip::create_extended_polygon(polygon_a); + polygon_clip::ExtendedPolygon poly_b = polygon_clip::create_extended_polygon(polygon_b); + + return polygon_clip::clip(poly_a, poly_b, source_forwards, clip_forwards); +} + +// Difference function +std::vector difference( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b) +{ + return apply_operation(polygon_a, polygon_b, false, true); +} + +// Union function +std::vector union_( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b) +{ + return apply_operation(polygon_a, polygon_b, false, false); +} + +// Intersection function +std::vector intersection( + const autoware::universe_utils::Polygon2d & polygon_a, + const autoware::universe_utils::Polygon2d & polygon_b) +{ + return apply_operation(polygon_a, polygon_b, true, true); +} + +std::vector intersection( + const autoware::universe_utils::Point2d & s1, const autoware::universe_utils::Point2d & s2, + const autoware::universe_utils::Point2d & c1, const autoware::universe_utils::Point2d & c2) +{ + std::vector intersection_points; + + double d = (c2.y() - c1.y()) * (s2.x() - s1.x()) - (c2.x() - c1.x()) * (s2.y() - s1.y()); + + if (std::abs(d) > 1e-9) { + double t1 = ((c2.x() - c1.x()) * (s1.y() - c1.y()) - (c2.y() - c1.y()) * (s1.x() - c1.x())) / d; + double t2 = ((s2.x() - s1.x()) * (s1.y() - c1.y()) - (s2.y() - s1.y()) * (s1.x() - c1.x())) / d; + + if (t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1) { + autoware::universe_utils::Point2d intersection_; + intersection_.x() = t1 * s2.x() + (1.0 - t1) * s1.x(); + intersection_.y() = t1 * s2.y() + (1.0 - t1) * s1.y(); + intersection_points.push_back(intersection_); + } + } + + return intersection_points; +} + +std::vector intersection( + const autoware::universe_utils::Segment2d & source_segment, + const autoware::universe_utils::Segment2d & clip_segment) +{ + std::vector intersection_points; + + const auto & s1 = source_segment.first; + const auto & s2 = source_segment.second; + const auto & c1 = clip_segment.first; + const auto & c2 = clip_segment.second; + + double d = (c2.y() - c1.y()) * (s2.x() - s1.x()) - (c2.x() - c1.x()) * (s2.y() - s1.y()); + + if (std::abs(d) > 1e-9) { + double t1 = ((c2.x() - c1.x()) * (s1.y() - c1.y()) - (c2.y() - c1.y()) * (s1.x() - c1.x())) / d; + double t2 = ((s2.x() - s1.x()) * (s1.y() - c1.y()) - (s2.y() - s1.y()) * (s1.x() - c1.x())) / d; + + if (t1 >= 0 && t1 <= 1 && t2 >= 0 && t2 <= 1) { + autoware::universe_utils::Point2d intersection_; + intersection_.x() = t1 * s2.x() + (1.0 - t1) * s1.x(); + intersection_.y() = t1 * s2.y() + (1.0 - t1) * s1.y(); + intersection_points.push_back(intersection_); + } + } + + return intersection_points; +} + +} // namespace autoware::universe_utils 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 3e4b08c4cc272..13d54feb8fc37 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -15,6 +15,7 @@ #include "autoware/universe_utils/geometry/boost_geometry.hpp" #include "autoware/universe_utils/geometry/ear_clipping.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/geometry/polygon_clip.hpp" #include "autoware/universe_utils/geometry/random_concave_polygon.hpp" #include "autoware/universe_utils/geometry/random_convex_polygon.hpp" #include "autoware/universe_utils/geometry/sat_2d.hpp" @@ -24,6 +25,9 @@ #include #include +#include +#include +#include #include #include @@ -31,6 +35,7 @@ #include #include #include +#include #include #include @@ -2423,3 +2428,433 @@ TEST(geometry, intersectConcavePolygonRand) std::printf("\tTotal:\n\t\tTriangulation = %2.2f ms\n", triangulation_ns / 1e6); } } + +bool polygon_equal( + const autoware::universe_utils::Polygon2d & A, const autoware::universe_utils::Polygon2d & B, + double max_difference_threshold) +{ + const auto & outer_A = A.outer(); + const auto & outer_B = B.outer(); + + int n = outer_A.size() - 1; + int m = outer_B.size() - 1; + + if (n != m) { + if (std::abs(boost::geometry::area(outer_A) - boost::geometry::area(outer_B)) < 1) { + return true; + } else { + return false; + } + } + + int start_index_B = -1; + double min_distance = std::numeric_limits::max(); + + for (int i = 0; i < m; ++i) { + double dist = boost::geometry::distance(outer_A[0], outer_B[i]) + + boost::geometry::distance(outer_A[1], outer_B[i + 1]); + if (dist < min_distance) { + min_distance = dist; + start_index_B = i; + } + } + + if (start_index_B == -1) { + std::cout << "No common starting point found\n"; + return false; + } + + std::vector rotated_B(outer_B.begin(), outer_B.end() - 1); + std::rotate(rotated_B.begin(), rotated_B.begin() + start_index_B, rotated_B.end()); + + for (int i = 0; i < n - 1; ++i) { + double x_diff = std::abs(outer_A[i].x() - rotated_B[i].x()); + double y_diff = std::abs(outer_A[i].y() - rotated_B[i].y()); + + if (x_diff >= max_difference_threshold || y_diff >= max_difference_threshold) { + std::cout << outer_A[i].x() << ", " << outer_A[i].y() << ": A\n"; + std::cout << rotated_B[i].x() << ", " << rotated_B[i].y() << ": B\n"; + std::cout << std::abs(rotated_B[i].x() - outer_A[i].x()) << ", " + << std::abs(rotated_B[i].y() - outer_A[i].y()) << ": Difference\n"; + + return false; + } + } + + return true; +} + +// function to compare two sets of polygons +bool polygon_equal_vector( + std::vector & customPolygons, + std::vector & boostPolygons, double max_difference_threshold) +{ + // sort custom polygons by area + std::sort( + customPolygons.begin(), customPolygons.end(), + []( + const autoware::universe_utils::Polygon2d & a, + const autoware::universe_utils::Polygon2d & b) { + return boost::geometry::area(a) < boost::geometry::area(b); + }); + + std::sort( + boostPolygons.begin(), boostPolygons.end(), + []( + const autoware::universe_utils::Polygon2d & a, + const autoware::universe_utils::Polygon2d & b) { + return boost::geometry::area(a) < boost::geometry::area(b); + }); + + if (customPolygons.size() != boostPolygons.size()) { + double customArea = 0.0; + double boostArea = 0.0; + + for (const auto & polygon : customPolygons) { + customArea += boost::geometry::area(polygon); + } + + for (const auto & polygon : boostPolygons) { + boostArea += boost::geometry::area(polygon); + } + + if (std::abs(customArea - boostArea) < 1) { + return true; + } else { + return false; + } + } + + for (size_t i = 0; i < customPolygons.size(); ++i) { + if (!polygon_equal(customPolygons[i], boostPolygons[i], max_difference_threshold)) { + return false; + } + } + return true; +} + +TEST(geometry, UnionDifferenceIntersectPolygon) +{ + using autoware::universe_utils::Polygon2d; + double epsilon = 1e-2; + + { // same size polygons + Polygon2d polyA; + Polygon2d polyB; + + polyA.outer().emplace_back(0.0, 0.0); + polyA.outer().emplace_back(4.0, 0.0); + polyA.outer().emplace_back(4.0, 4.0); + polyA.outer().emplace_back(0.0, 4.0); + boost::geometry::correct(polyA); + + polyB.outer().emplace_back(1.0, 1.0); + polyB.outer().emplace_back(5.0, 1.0); + polyB.outer().emplace_back(5.0, 5.0); + polyB.outer().emplace_back(1.0, 5.0); + boost::geometry::correct(polyB); + + auto custom_union_poly = autoware::universe_utils::union_(polyA, polyB); + auto custom_intersection_poly = autoware::universe_utils::intersection(polyA, polyB); + auto custom_difference_poly = autoware::universe_utils::difference(polyA, polyB); + + boost::geometry::model::multi_polygon boost_union_resultMP; + boost::geometry::union_(polyA, polyB, boost_union_resultMP); + std::vector boost_union_result( + boost_union_resultMP.begin(), boost_union_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_intersection_resultMP; + boost::geometry::intersection(polyA, polyB, boost_intersection_resultMP); + std::vector boost_intersection_result( + boost_intersection_resultMP.begin(), boost_intersection_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_difference_resultMP; + boost::geometry::difference(polyA, polyB, boost_difference_resultMP); + std::vector boost_difference_result( + boost_difference_resultMP.begin(), boost_difference_resultMP.end()); + + EXPECT_TRUE(polygon_equal_vector(custom_union_poly, boost_union_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_intersection_poly, boost_intersection_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_difference_poly, boost_difference_result, epsilon)); + } + + { // different size polygons + Polygon2d polyA; + Polygon2d polyB; + + polyA.outer().emplace_back(0.0, 0.0); + polyA.outer().emplace_back(6.0, 0.0); + polyA.outer().emplace_back(6.0, 6.0); + polyA.outer().emplace_back(0.0, 6.0); + boost::geometry::correct(polyA); + + polyB.outer().emplace_back(2.0, 2.0); + polyB.outer().emplace_back(3.0, 2.0); + polyB.outer().emplace_back(2.0, 3.0); + boost::geometry::correct(polyB); + + auto custom_union_poly = autoware::universe_utils::union_(polyA, polyB); + auto custom_intersection_poly = autoware::universe_utils::intersection(polyA, polyB); + auto custom_difference_poly = autoware::universe_utils::difference(polyA, polyB); + + boost::geometry::model::multi_polygon boost_union_resultMP; + boost::geometry::union_(polyA, polyB, boost_union_resultMP); + std::vector boost_union_result( + boost_union_resultMP.begin(), boost_union_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_intersection_resultMP; + boost::geometry::intersection(polyA, polyB, boost_intersection_resultMP); + std::vector boost_intersection_result( + boost_intersection_resultMP.begin(), boost_intersection_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_difference_resultMP; + boost::geometry::difference(polyA, polyB, boost_difference_resultMP); + std::vector boost_difference_result( + boost_difference_resultMP.begin(), boost_difference_resultMP.end()); + + EXPECT_TRUE(polygon_equal_vector(custom_union_poly, boost_union_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_intersection_poly, boost_intersection_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_difference_poly, boost_difference_result, epsilon)); + } + + { // degenerate polygon (failed case) + Polygon2d polyA; + Polygon2d polyB; + + polyA.outer().emplace_back(0.0, 0.0); + polyA.outer().emplace_back(2.0, 1.0); // degenerate case (same point) + polyA.outer().emplace_back(2.0, 0.0); + boost::geometry::correct(polyA); + + polyB.outer().emplace_back(1.0, 1.0); + polyB.outer().emplace_back(2.0, 1.0); + polyB.outer().emplace_back(2.0, 2.0); + polyB.outer().emplace_back(1.0, 2.0); + boost::geometry::correct(polyB); + + auto custom_union_poly = autoware::universe_utils::union_(polyA, polyB); + auto custom_intersection_poly = autoware::universe_utils::intersection(polyA, polyB); + auto custom_difference_poly = autoware::universe_utils::difference(polyA, polyB); + + boost::geometry::model::multi_polygon boost_union_resultMP; + boost::geometry::union_(polyA, polyB, boost_union_resultMP); + std::vector boost_union_result( + boost_union_resultMP.begin(), boost_union_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_intersection_resultMP; + boost::geometry::intersection(polyA, polyB, boost_intersection_resultMP); + std::vector boost_intersection_result( + boost_intersection_resultMP.begin(), boost_intersection_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_difference_resultMP; + boost::geometry::difference(polyA, polyB, boost_difference_resultMP); + std::vector boost_difference_result( + boost_difference_resultMP.begin(), boost_difference_resultMP.end()); + + EXPECT_TRUE(polygon_equal_vector(custom_union_poly, boost_union_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_intersection_poly, boost_intersection_result, epsilon)); + EXPECT_TRUE(polygon_equal_vector(custom_difference_poly, boost_difference_result, epsilon)); + } + + { // case where the difference between the polygons is greater than 0.5 using Boost.Geometry, but + // it is confirmed to match the result from Shapely + Polygon2d polyA; + Polygon2d polyB; + + polyA.outer().emplace_back(474.1861139144, 357.4532187344); + polyA.outer().emplace_back(2840.6550926656, 2880.5752363202); + polyA.outer().emplace_back(2274.8557444340, 989.8630822076); + polyA.outer().emplace_back(2203.7919603015, 32.5559338946); + boost::geometry::correct(polyA); + + polyB.outer().emplace_back(16.5571756985, 2726.9026932982); + polyB.outer().emplace_back(2085.6873996180, 2075.6552665052); + polyB.outer().emplace_back(1367.0448775695, 1309.2684834777); + polyB.outer().emplace_back(530.7105471140, 301.6894918973); + boost::geometry::correct(polyB); + + auto custom_union_poly = autoware::universe_utils::union_(polyA, polyB); + auto custom_intersection_poly = autoware::universe_utils::intersection(polyA, polyB); + auto custom_difference_poly = autoware::universe_utils::difference(polyA, polyB); + + boost::geometry::model::multi_polygon boost_union_resultMP; + boost::geometry::union_(polyA, polyB, boost_union_resultMP); + std::vector boost_union_result( + boost_union_resultMP.begin(), boost_union_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_intersection_resultMP; + boost::geometry::intersection(polyA, polyB, boost_intersection_resultMP); + std::vector boost_intersection_result( + boost_intersection_resultMP.begin(), boost_intersection_resultMP.end()); + + boost::geometry::model::multi_polygon + boost_difference_resultMP; + boost::geometry::difference(polyA, polyB, boost_difference_resultMP); + std::vector boost_difference_result( + boost_difference_resultMP.begin(), boost_difference_resultMP.end()); + + EXPECT_FALSE(polygon_equal_vector(custom_union_poly, boost_union_result, epsilon)); + EXPECT_FALSE( + polygon_equal_vector(custom_intersection_poly, boost_intersection_result, epsilon)); + EXPECT_FALSE(polygon_equal_vector(custom_difference_poly, boost_difference_result, epsilon)); + } + + { // case where Boost.Geometry omits a very small polygon in the difference result, while Shapely + // and our custom function recognize it as a valid polygon. + + Polygon2d polyA; + Polygon2d polyB; + + polyA.outer().emplace_back(398.6627895225, 2020.9971337400); + polyA.outer().emplace_back(2053.4301277502, 1487.3008988972); + polyA.outer().emplace_back(1361.3689322077, 746.2935043375); + polyA.outer().emplace_back(624.1941572081, 18.0106444090); + polyA.outer().emplace_back(569.3918138266, 694.6232593731); + polyA.outer().emplace_back(992.9521794850, 1248.9022921509); + boost::geometry::correct(polyA); + + polyB.outer().emplace_back(353.0508543735, 2217.0897988768); + polyB.outer().emplace_back(1261.2989930437, 1316.8199700947); + polyB.outer().emplace_back(2515.7069164679, 1206.7858412531); + polyB.outer().emplace_back(947.4189338473, 89.9986425623); + polyB.outer().emplace_back(362.9869975068, 919.9162595880); + polyB.outer().emplace_back(559.5373047609, 1329.3731983727); + boost::geometry::correct(polyB); + + auto custom_difference_poly = autoware::universe_utils::difference(polyA, polyB); + + boost::geometry::model::multi_polygon + boost_difference_resultMP; + boost::geometry::difference(polyA, polyB, boost_difference_resultMP); + std::vector boost_difference_result( + boost_difference_resultMP.begin(), boost_difference_resultMP.end()); + + EXPECT_TRUE(polygon_equal_vector(custom_difference_poly, boost_difference_result, epsilon)); + } +} + +TEST(geometry, RandomUnionIntersectPolygon) +{ + std::vector polygons; + constexpr auto polygons_nb = 250; + constexpr auto max_vertices = 10; + constexpr auto max_values = 1000; + + autoware::universe_utils::StopWatch sw; + + std::cout << std::fixed << std::setprecision(10); + + for (auto vertices = 4UL; vertices < max_vertices; ++vertices) { + double custom_union_ns = 0.0; + double custom_intersection_ns = 0.0; + double boost_intersection_ns = 0.0; + double boost_union_ns = 0.0; + + int count_matching_polygon_union = 0.0; + int count_different_polygon_union = 0.0; + int count_matching_polygon_intersection = 0.0; + int count_different_polygon_intersection = 0.0; + polygons.clear(); + + for (auto i = 0; i < polygons_nb; ++i) { + auto polygon_opt = autoware::universe_utils::random_concave_polygon(vertices, max_values); + if (polygon_opt.has_value()) { + polygons.push_back(polygon_opt.value()); + } + } + + for (auto i = 0UL; i < polygons.size(); ++i) { + for (auto j = 0UL; j < polygons.size(); ++j) { + if (i == j) { + continue; + } + sw.tic(); + auto custom_union_result = autoware::universe_utils::union_(polygons[i], polygons[j]); + custom_union_ns += sw.toc(); + + sw.tic(); + auto custom_intersection_result = + autoware::universe_utils::intersection(polygons[i], polygons[j]); + custom_intersection_ns += sw.toc(); + + sw.tic(); + boost::geometry::model::multi_polygon + boost_union_resultMP; + boost::geometry::union_(polygons[i], polygons[j], boost_union_resultMP); + std::vector boost_union_result( + boost_union_resultMP.begin(), boost_union_resultMP.end()); + boost_union_ns += sw.toc(); + + sw.tic(); + boost::geometry::model::multi_polygon + boost_intersection_resultMP; + boost::geometry::intersection(polygons[i], polygons[j], boost_intersection_resultMP); + std::vector boost_intersection_result( + boost_intersection_resultMP.begin(), boost_intersection_resultMP.end()); + boost_intersection_ns += sw.toc(); + + bool union_match = polygon_equal_vector(custom_union_result, boost_union_result, 1); + if (union_match) { + ++count_matching_polygon_union; + } else { + for (size_t k = 0; k < polygons[i].outer().size() - 1; ++k) { + const auto & point = polygons[i].outer()[k]; + auto x = point.x(); + auto y = point.y(); + std::cout << "union polygon source point: (" << x << ", " << y << ")\n"; + } + for (size_t k = 0; k < polygons[j].outer().size() - 1; ++k) { + const auto & point = polygons[j].outer()[k]; + auto x = point.x(); + auto y = point.y(); + std::cout << "polygon clip point: (" << x << ", " << y << ")\n"; + } + + ++count_different_polygon_union; + } + + bool intersection_match = + polygon_equal_vector(custom_intersection_result, boost_intersection_result, 1); + if (intersection_match) { + ++count_matching_polygon_intersection; + } else { + for (size_t k = 0; k < polygons[i].outer().size() - 1; ++k) { + const auto & point = polygons[i].outer()[k]; + auto x = point.x(); + auto y = point.y(); + std::cout << "intersection polygon source point: (" << x << ", " << y << ")\n"; + } + for (size_t k = 0; k < polygons[j].outer().size() - 1; ++k) { + const auto & point = polygons[j].outer()[k]; + auto x = point.x(); + auto y = point.y(); + std::cout << "polygon clip point: (" << x << ", " << y << ")\n"; + } + + ++count_different_polygon_intersection; + } + } + } + + std::printf("vertices = %ld\n", vertices); + std::printf( + "\tUnion:\n\t\tCustom = %2.2f ms\n\t\tBoost::geometry = %2.2f ms\n", custom_union_ns / 1e6, + boost_union_ns / 1e6); + std::printf( + "\t\tMatching Polygon = %d\n\t\tDifferent Polygon = %d\n", count_matching_polygon_union, + count_different_polygon_union); + std::printf( + "\tIntersection:\n\t\tCustom = %2.2f ms\n\t\tBoost::geometry = %2.2f ms\n", + custom_intersection_ns / 1e6, boost_intersection_ns / 1e6); + std::printf( + "\t\tMatching Polygon = %d\n\t\tDifferent Polygon = %d\n", + count_matching_polygon_intersection, count_different_polygon_intersection); + } +}