feat(universe_utils): add Buffer implementation to replace boost.buffer()

common/autoware_universe_utils/CMakeLists.txt

@@ -29,6 +29,7 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/system/time_keeper.cpp
   src/geometry/ear_clipping.cpp
   src/geometry/polygon_clip.cpp
+  src/geometry/buffer.cpp
 )
 
 target_link_libraries(autoware_universe_utils

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

@@ -0,0 +1,102 @@
+// 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__BUFFER_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BUFFER_HPP_
+
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+#include "autoware/universe_utils/geometry/polygon_clip.hpp"
+
+#include <boost/geometry/algorithms/buffer.hpp>
+#include <boost/geometry/algorithms/correct.hpp>
+#include <boost/geometry/algorithms/simplify.hpp>
+#include <boost/geometry/algorithms/within.hpp>
+#include <boost/geometry/strategies/buffer.hpp>
+
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <vector>
+
+namespace autoware::universe_utils
+{
+
+namespace offset_buffer
+{
+/**
+ * @brief create an arc between two vertices with a given radius and center
+ * @param vertices the vertices to populate with the arc points
+ * @param center the center point of the arc
+ * @param radius the radius of the arc
+ * @param start_vertex the start vertex of the arc
+ * @param end_vertex the end vertex of the arc
+ * @param start_vertex_next the next vertex after the start vertex
+ * @param segments the number of segments to divide the arc into
+ */
+Polygon2d create_arc(
+  Polygon2d & vertices, const Point2d & center, double radius, const Point2d & offset_v1,
+  const Point2d & offset_v2next, const Point2d & end_vertex, const Point2d & start_vertex_next,
+  double segments);
+
+/**
+ * @brief offset a polygon segment between two vertices with a specified distance
+ * @param v1 the first vertex
+ * @param v2 the second vertex
+ * @param next_vertex the next vertex in the polygon
+ * @param dist the offset distance
+ * @param segments the number of segments to divide the arc into
+ * @return the offset polygon segment
+ */
+void offset_segment(
+  Polygon2d & vertices, const Point2d & v1, const Point2d & v2, const Point2d & next_vertex,
+  double dist, double segments);
+}  // namespace offset_buffer
+
+/**
+ * @brief buffer a polygon by a specified distance and number of segments
+ * @param input_polygon the input polygon to be buffered
+ * @param dist the offset distance
+ * @param segments the number of segments to divide the arcs into
+ * @return the buffered polygon
+ */
+Polygon2d buffer(const Polygon2d & input_polygon, double dist, double segments);
+
+/**
+ * @brief buffer a point by a specified distance and number of segments
+ * @param point the point to be buffer
+ * @param distance the offset distance
+ * @param segments The number of segments to divide the arc into
+ * @return the buffered polygon representing a circle (point buffer)
+ */
+Polygon2d buffer(const Point2d & point, double distance, double segments);
+
+/**
+ * @brief buffer (offset) multiple points and return the union of their buffers
+ * @param multi_point a collection of points for the buffer
+ * @param distance the offset distance
+ * @param segments the number of segments to divide the arcs into
+ * @return The union of all buffered polygons
+ */
+Polygon2d buffer(const MultiPoint2d & multi_point, double distance, double segments);
+
+/**
+ * @brief Dissolves the input polygon to eliminate self-intersections or redundant structures.
+ * @param polygon The input polygon of type Polygon2d.
+ * @return A simplified polygon with self-intersections dissolved and redundant structures removed.
+ */
+Polygon2d dissolve(const Polygon2d & polygon);
+
+}  // namespace autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BUFFER_HPP_

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

@@ -32,6 +32,8 @@ struct LinkedVertex
   double y;
   std::optional<std::size_t> next;
   std::optional<std::size_t> prev;
+  std::optional<std::size_t> next_2;
+  std::optional<std::size_t> prev_2;
   std::optional<std::size_t> corresponding;
   double distance;
   bool is_entry;
@@ -185,6 +187,30 @@ std::size_t get_first_intersect(ExtendedPolygon & polygon);
 std::vector<autoware::universe_utils::Polygon2d> clip(
   ExtendedPolygon & source, ExtendedPolygon & clip, bool source_forwards, bool clip_forwards);
 
+/**
+ * @brief Marks self-intersections in the given ExtendedPolygon.
+ * @details This function identifies self-intersecting segments in the polygon and creates new
+ * intersection vertices. The detected intersections are then marked and processed to update the
+ * polygon structure accordingly.
+ *
+ * @param source The ExtendedPolygon in which to detect and mark self-intersections.
+ * @param current_index The current index in the polygon being processed. This index is updated as
+ * intersections are marked.
+ */
+void mark_self_intersections(ExtendedPolygon & source, std::size_t & current_index);
+
+/**
+ * @brief Adjusts the `next` pointer of the intersection vertex in the polygon to point to the
+ * appropriate succeeding vertex.
+ * @details Ensures that the polygon to always go left, so no self-intersection occurs.
+ *
+ * @param polygon The ExtendedPolygon containing the vertices.
+ * @param current_index The index of the intersection vertex to adjust. This is updated within the
+ * function if necessary.
+ */
+void adjust_intersection_next(ExtendedPolygon & polygon, std::size_t & current_index);
+
+autoware::universe_utils::Polygon2d construct_self_intersecting_polygons(ExtendedPolygon & polygon);
 }  // namespace polygon_clip
 
 /**

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

@@ -18,6 +18,7 @@
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
 #include <optional>
+#include <random>
 #include <vector>
 
 namespace autoware::universe_utils

common/autoware_universe_utils/src/geometry/buffer.cpp

@@ -0,0 +1,172 @@
+// 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/buffer.hpp"
+
+#include "autoware/universe_utils/geometry/polygon_clip.hpp"
+#include "autoware/universe_utils/system/stop_watch.hpp"
+
+#include <iostream>
+#include <vector>
+
+namespace autoware::universe_utils
+{
+
+namespace offset_buffer
+{
+
+Polygon2d dissolve(const Polygon2d & polygon)
+{
+  auto extended_poly = polygon_clip::create_extended_polygon(polygon);
+
+  auto polygon_vector = polygon_clip::construct_self_intersecting_polygons(extended_poly);
+
+  return polygon_vector;
+}
+
+void create_arc(
+  Polygon2d & vertices, const Point2d & center, const double radius, const Point2d & end_vertex,
+  const Point2d & start_vertex_next, const double segments)
+{
+  const double PI2 = M_PI * 2;
+  double start_angle =
+    atan2(start_vertex_next.y() - center.y(), start_vertex_next.x() - center.x());
+  double end_angle = atan2(end_vertex.y() - center.y(), end_vertex.x() - center.x());
+
+  if (start_angle < 0) start_angle += PI2;
+  if (end_angle < 0) end_angle += PI2;
+
+  const double angle_diff =
+    ((start_angle > end_angle) ? (start_angle - end_angle) : (start_angle + PI2 - end_angle));
+
+  int dynamic_segments = static_cast<int>(segments * (angle_diff / PI2));
+  if (dynamic_segments < 1) dynamic_segments = 1;
+
+  const double segment_angle = angle_diff / dynamic_segments;
+
+  for (int i = 0; i <= dynamic_segments; ++i) {
+    const double angle = end_angle + i * segment_angle;
+    const double x = center.x() + radius * cos(angle);
+    const double y = center.y() + radius * sin(angle);
+
+    vertices.outer().push_back(Point2d(x, y));
+  }
+}
+
+void offset_segment(
+  Polygon2d & vertices, const Point2d & v1, const Point2d & v2, const Point2d & next_vertex,
+  const double dist, const double segments)
+{
+  // Calculate direction and normals
+  const double dx = v2.x() - v1.x();
+  const double dy = v2.y() - v1.y();
+
+  const double length = std::sqrt(dx * dx + dy * dy);
+  const double normal_x = -dy / length;
+  const double normal_y = dx / length;
+
+  const Point2d offset_v1(v1.x() - normal_x * dist, v1.y() - normal_y * dist);
+  const Point2d offset_v2(v2.x() - normal_x * dist, v2.y() - normal_y * dist);
+
+  const double next_dx = next_vertex.x() - v2.x();
+  const double next_dy = next_vertex.y() - v2.y();
+  const double length_next = std::sqrt(next_dx * next_dx + next_dy * next_dy);
+  const double normal_x_next = -next_dy / length_next;
+  const double normal_y_next = next_dx / length_next;
+  const Point2d offset_v1next(v2.x() - normal_x_next * dist, v2.y() - normal_y_next * dist);
+  const Point2d offset_v2next(
+    next_vertex.x() - normal_x_next * dist, next_vertex.y() - normal_y_next * dist);
+
+  double current_angle = atan2(dy, dx);
+  double next_angle = atan2(next_dy, next_dx);
+
+  if (current_angle < 0) current_angle += M_PI * 2;
+  if (next_angle < 0) next_angle += M_PI * 2;
+
+  double angle_current_next =
+    ((next_angle > current_angle) ? (next_angle - current_angle)
+                                  : (next_angle + M_PI * 2 - current_angle));
+
+  if (angle_current_next < 0) {
+    angle_current_next += M_PI * 2;
+  }
+
+  if (angle_current_next < M_PI) {
+    vertices.outer().push_back(offset_v1);
+    create_arc(vertices, v2, dist, offset_v2, offset_v1next, segments);
+  } else {
+    vertices.outer().push_back(offset_v1);
+    vertices.outer().push_back(offset_v2);
+  }
+}
+
+}  // namespace offset_buffer
+
+Polygon2d buffer(const Polygon2d & input_polygon, double dist, double segments)
+{
+  Polygon2d offset_polygon;
+  Polygon2d final_polygon;
+  size_t vertices_count = input_polygon.outer().size();
+
+  if (vertices_count < 2) {
+    std::cerr << "Polygon needs at least 2 vertices!" << std::endl;
+    return offset_polygon;
+  }
+
+  std::vector<Point2d> new_ring(input_polygon.outer().begin(), input_polygon.outer().end() - 1);
+  size_t modified_vertices_count = new_ring.size();
+
+  for (size_t i = modified_vertices_count; i > 0; --i) {
+    const auto & v1 = new_ring[(i - 1) % modified_vertices_count];
+    const auto & v2 = new_ring[(i - 2 + modified_vertices_count) % modified_vertices_count];
+    const auto & next_vertex =
+      new_ring[(i - 3 + modified_vertices_count) % modified_vertices_count];
+
+    offset_buffer::offset_segment(offset_polygon, v1, v2, next_vertex, dist, segments);
+  }
+  boost::geometry::correct(offset_polygon);
+  final_polygon = offset_buffer::dissolve(offset_polygon);
+  return final_polygon;
+}
+
+Polygon2d buffer(const Point2d & point, double distance, double segments)
+{
+  Polygon2d offset_polygon;
+
+  for (int i = 0; i < segments; ++i) {
+    double angle = 2 * M_PI * i / segments;
+    Point2d buffer_point(point.x() + distance * cos(angle), point.y() + distance * sin(angle));
+    offset_polygon.outer().push_back(buffer_point);
+  }
+  boost::geometry::correct(offset_polygon);
+  return offset_polygon;
+}
+
+Polygon2d buffer(const MultiPoint2d & multi_point, double distance, double segments)
+{
+  Polygon2d buffer_union;
+  bool first = true;
+  for (const auto & point : multi_point) {
+    Polygon2d buffer_point = buffer(point, distance, segments);
+    if (!first) {
+      buffer_union = union_(buffer_point, buffer_union)[0];
+    } else {
+      buffer_union = buffer_point;
+      first = false;
+    }
+  }
+  return buffer_union;
+}
+
+}  // namespace autoware::universe_utils