From 528924e471ad1956a1dce784e9f72e3a3d79c50a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 15 Nov 2024 07:32:23 +0000 Subject: [PATCH] style(pre-commit): autofix --- .../universe_utils/geometry/buffer.hpp | 85 +++--- .../src/geometry/buffer.cpp | 274 +++++++++--------- 2 files changed, 179 insertions(+), 180 deletions(-) diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/buffer.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/buffer.hpp index 3958d08bb8de..c46c14558df5 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/buffer.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/buffer.hpp @@ -12,8 +12,8 @@ // 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 +#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" @@ -23,8 +23,8 @@ #include #include -#include #include +#include #include namespace autoware::universe_utils @@ -32,43 +32,37 @@ namespace autoware::universe_utils namespace { - /** - * @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 - * @param skip a flag to skip adding the start vertex - * @return the updated Polygon2d with the arc - */ - Polygon2d create_arc(autoware::universe_utils::Polygon2d& vertices, - const autoware::universe_utils::Point2d& center, - double radius, - const autoware::universe_utils::Point2d& start_vertex, - const autoware::universe_utils::Point2d& end_vertex, - const autoware::universe_utils::Point2d& start_vertex_next, - double segments, - bool skip); - - /** - * @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 - * @param skip a flag to skip adding the first segment - * @return the offset polygon segment - */ - autoware::universe_utils::Polygon2d _offset_segment(const autoware::universe_utils::Point2d& v1, - const autoware::universe_utils::Point2d& v2, - const autoware::universe_utils::Point2d& next_vertex, - double dist, - double segments, - bool skip); +/** + * @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 + * @param skip a flag to skip adding the start vertex + * @return the updated Polygon2d with the arc + */ +Polygon2d create_arc( + autoware::universe_utils::Polygon2d & vertices, const autoware::universe_utils::Point2d & center, + double radius, const autoware::universe_utils::Point2d & start_vertex, + const autoware::universe_utils::Point2d & end_vertex, + const autoware::universe_utils::Point2d & start_vertex_next, double segments, bool skip); + +/** + * @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 + * @param skip a flag to skip adding the first segment + * @return the offset polygon segment + */ +autoware::universe_utils::Polygon2d _offset_segment( + const autoware::universe_utils::Point2d & v1, const autoware::universe_utils::Point2d & v2, + const autoware::universe_utils::Point2d & next_vertex, double dist, double segments, bool skip); } // namespace /** @@ -78,7 +72,8 @@ namespace * @param segments the number of segments to divide the arcs into * @return the buffered polygon */ -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Polygon2d& input_polygon, double dist, double segments); +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::Polygon2d & input_polygon, double dist, double segments); /** * @brief buffer a point by a specified distance and number of segments @@ -87,7 +82,8 @@ autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Polyg * @param segments The number of segments to divide the arc into * @return the buffered polygon representing a circle (point buffer) */ -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Point2d& point, double distance, double segments); +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::Point2d & point, double distance, double segments); /** * @brief buffer (offset) multiple points and return the union of their buffers @@ -96,8 +92,9 @@ autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Point * @param segments the number of segments to divide the arcs into * @return The union of all buffered polygons */ -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::MultiPoint2d& multi_point, double distance, double segments); +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::MultiPoint2d & multi_point, double distance, double segments); } // namespace autoware::universe_utils -#endif // AUTOWARE_UNIVERSE_UTILS_GEOMETRY_BUFFER_HPP +#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__BUFFER_HPP_ diff --git a/common/autoware_universe_utils/src/geometry/buffer.cpp b/common/autoware_universe_utils/src/geometry/buffer.cpp index 81aaaa30227f..5e56dcb967b0 100644 --- a/common/autoware_universe_utils/src/geometry/buffer.cpp +++ b/common/autoware_universe_utils/src/geometry/buffer.cpp @@ -17,169 +17,171 @@ namespace autoware::universe_utils { -namespace +namespace { -autoware::universe_utils::Polygon2d create_arc(autoware::universe_utils::Polygon2d& vertices, - const autoware::universe_utils::Point2d& center, - double radius, - const autoware::universe_utils::Point2d& start_vertex, - const autoware::universe_utils::Point2d& end_vertex, - const autoware::universe_utils::Point2d& start_vertex_next, - double segments, - bool skip) { - using namespace autoware::universe_utils; - - const double PI2 = M_PI * 2; - double startAngle = atan2(start_vertex_next.y() - center.y(), start_vertex_next.x() - center.x()); - double endAngle = atan2(end_vertex.y() - center.y(), end_vertex.x() - center.x()); - - if (skip) { - vertices.outer().push_back(start_vertex); - skip = false; - } - - if (startAngle < 0) startAngle += PI2; - if (endAngle < 0) endAngle += PI2; - - double angleDiff = ((startAngle > endAngle) ? (startAngle - endAngle) : (startAngle + PI2 - endAngle)); - if (angleDiff < 0) angleDiff += PI2; - - double segmentAngle = angleDiff / segments; - - for (int i = 0; i <= segments; ++i) { - double angle = endAngle + i * segmentAngle; - double x = center.x() + radius * cos(angle); - double y = center.y() + radius * sin(angle); - - vertices.outer().push_back(Point2d(x, y)); - } - return vertices; -} - -autoware::universe_utils::Polygon2d _offset_segment(const autoware::universe_utils::Point2d& v1, - const autoware::universe_utils::Point2d& v2, - const autoware::universe_utils::Point2d& next_vertex, - double dist, - double segments, - bool skip) { - using namespace autoware::universe_utils; - - Polygon2d vertices; - - double dx = v2.x() - v1.x(); - double dy = v2.y() - v1.y(); - - double length = std::sqrt(dx * dx + dy * dy); - double normal_x = -dy / length; - double normal_y = dx / length; - - Point2d offset_v1(v1.x() - normal_x * dist, v1.y() - normal_y * dist); - Point2d offset_v2(v2.x() - normal_x * dist, v2.y() - normal_y * dist); - - double next_dx = next_vertex.x() - v2.x(); - double next_dy = next_vertex.y() - v2.y(); - double length_next = std::sqrt(next_dx * next_dx + next_dy * next_dy); - double normal_x_next = -next_dy / length_next; - double normal_y_next = next_dx / length_next; - Point2d offset_v1next(v2.x() - normal_x_next * dist, v2.y() - normal_y_next * dist); - Point2d offset_v2next(next_vertex.x() - normal_x_next * dist, next_vertex.y() - normal_y_next * dist); - - +autoware::universe_utils::Polygon2d create_arc( + autoware::universe_utils::Polygon2d & vertices, const autoware::universe_utils::Point2d & center, + double radius, const autoware::universe_utils::Point2d & start_vertex, + const autoware::universe_utils::Point2d & end_vertex, + const autoware::universe_utils::Point2d & start_vertex_next, double segments, bool skip) +{ + using namespace autoware::universe_utils; + const double PI2 = M_PI * 2; + double startAngle = atan2(start_vertex_next.y() - center.y(), start_vertex_next.x() - center.x()); + double endAngle = atan2(end_vertex.y() - center.y(), end_vertex.x() - center.x()); - double currAngle = atan2(dy, dx); - double nextAngle = atan2(next_dy, next_dx); + if (skip) { + vertices.outer().push_back(start_vertex); + skip = false; + } - double angle_current_next = nextAngle - currAngle; - if (angle_current_next < 0) angle_current_next += M_PI * 2; - std::cout << "angleBetween" << angle_current_next << "\n"; + if (startAngle < 0) startAngle += PI2; + if (endAngle < 0) endAngle += PI2; - if (2* M_PI - angle_current_next > M_PI) { - create_arc(vertices, v2, dist, offset_v1, offset_v2, offset_v1next, segments, skip); - } + double angleDiff = + ((startAngle > endAngle) ? (startAngle - endAngle) : (startAngle + PI2 - endAngle)); + if (angleDiff < 0) angleDiff += PI2; - else if (2* M_PI - angle_current_next < M_PI) { - auto intersection_point = autoware::universe_utils::intersection(offset_v1, offset_v2, offset_v1next, offset_v2next); - vertices.outer().push_back(offset_v1); - vertices.outer().push_back(intersection_point[0]); - vertices.outer().push_back(offset_v2next); - skip = true; - } + double segmentAngle = angleDiff / segments; - else { - vertices.outer().push_back(offset_v1); - vertices.outer().push_back(offset_v2); - } + for (int i = 0; i <= segments; ++i) { + double angle = endAngle + i * segmentAngle; + double x = center.x() + radius * cos(angle); + double y = center.y() + radius * sin(angle); - return vertices; + vertices.outer().push_back(Point2d(x, y)); + } + return vertices; } +autoware::universe_utils::Polygon2d _offset_segment( + const autoware::universe_utils::Point2d & v1, const autoware::universe_utils::Point2d & v2, + const autoware::universe_utils::Point2d & next_vertex, double dist, double segments, bool skip) +{ + using namespace autoware::universe_utils; + + Polygon2d vertices; + + double dx = v2.x() - v1.x(); + double dy = v2.y() - v1.y(); + + double length = std::sqrt(dx * dx + dy * dy); + double normal_x = -dy / length; + double normal_y = dx / length; + + Point2d offset_v1(v1.x() - normal_x * dist, v1.y() - normal_y * dist); + Point2d offset_v2(v2.x() - normal_x * dist, v2.y() - normal_y * dist); + + double next_dx = next_vertex.x() - v2.x(); + double next_dy = next_vertex.y() - v2.y(); + double length_next = std::sqrt(next_dx * next_dx + next_dy * next_dy); + double normal_x_next = -next_dy / length_next; + double normal_y_next = next_dx / length_next; + Point2d offset_v1next(v2.x() - normal_x_next * dist, v2.y() - normal_y_next * dist); + Point2d offset_v2next( + next_vertex.x() - normal_x_next * dist, next_vertex.y() - normal_y_next * dist); + + double currAngle = atan2(dy, dx); + double nextAngle = atan2(next_dy, next_dx); + + double angle_current_next = nextAngle - currAngle; + if (angle_current_next < 0) angle_current_next += M_PI * 2; + std::cout << "angleBetween" << angle_current_next << "\n"; + + if (2 * M_PI - angle_current_next > M_PI) { + create_arc(vertices, v2, dist, offset_v1, offset_v2, offset_v1next, segments, skip); + } + + else if (2 * M_PI - angle_current_next < M_PI) { + auto intersection_point = + autoware::universe_utils::intersection(offset_v1, offset_v2, offset_v1next, offset_v2next); + vertices.outer().push_back(offset_v1); + vertices.outer().push_back(intersection_point[0]); + vertices.outer().push_back(offset_v2next); + skip = true; + } + + else { + vertices.outer().push_back(offset_v1); + vertices.outer().push_back(offset_v2); + } + + return vertices; } -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Polygon2d& input_polygon, double dist, double segments) { - using namespace autoware::universe_utils; +} // namespace - Polygon2d offset_polygon; - size_t verticesCount = input_polygon.outer().size(); +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::Polygon2d & input_polygon, double dist, double segments) +{ + using namespace autoware::universe_utils; - if (verticesCount < 2) { - std::cerr << "Polygon needs at least 2 vertices!" << std::endl; - return offset_polygon; - } + Polygon2d offset_polygon; + size_t verticesCount = input_polygon.outer().size(); - bool skip = false; + if (verticesCount < 2) { + std::cerr << "Polygon needs at least 2 vertices!" << std::endl; + return offset_polygon; + } - for (size_t i = 0; i < verticesCount; ++i) { - const auto& v1 = input_polygon.outer()[i]; - const auto& v2 = input_polygon.outer()[(i + 1) % verticesCount]; + bool skip = false; - const auto& next_vertex = input_polygon.outer()[(i + 2) % verticesCount]; + for (size_t i = 0; i < verticesCount; ++i) { + const auto & v1 = input_polygon.outer()[i]; + const auto & v2 = input_polygon.outer()[(i + 1) % verticesCount]; - Polygon2d offset_segment = _offset_segment(v1, v2, next_vertex, dist, segments, skip); - - for (const auto& point : offset_segment.outer()) { - offset_polygon.outer().push_back(point); - } - } + const auto & next_vertex = input_polygon.outer()[(i + 2) % verticesCount]; + + Polygon2d offset_segment = _offset_segment(v1, v2, next_vertex, dist, segments, skip); - if (!offset_polygon.outer().empty()) { - offset_polygon.outer().push_back(offset_polygon.outer().front()); + for (const auto & point : offset_segment.outer()) { + offset_polygon.outer().push_back(point); } - boost::geometry::correct(offset_polygon); + } - return offset_polygon; + if (!offset_polygon.outer().empty()) { + offset_polygon.outer().push_back(offset_polygon.outer().front()); + } + boost::geometry::correct(offset_polygon); -} // namespace + return offset_polygon; -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::Point2d& point, double distance, double segments) { - using namespace autoware::universe_utils; +} // namespace - Polygon2d offsetPolygon; +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::Point2d & point, double distance, double segments) +{ + using namespace autoware::universe_utils; - 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)); - offsetPolygon.outer().push_back(buffer_point); - } - boost::geometry::correct(offsetPolygon); - return offsetPolygon; + Polygon2d offsetPolygon; + + 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)); + offsetPolygon.outer().push_back(buffer_point); + } + boost::geometry::correct(offsetPolygon); + return offsetPolygon; } -autoware::universe_utils::Polygon2d buffer(const autoware::universe_utils::MultiPoint2d& multi_point, double distance, double segments) { - using namespace autoware::universe_utils; - - autoware::universe_utils::Polygon2d buffer_union; - bool first = true; - for (const auto& point : multi_point) { - autoware::universe_utils::Polygon2d buffer_point = buffer(point, distance, segments); - if (!first) { - buffer_union = autoware::universe_utils::union_(buffer_point, buffer_union)[0]; - } else { - buffer_union = buffer_point; - first = false; - } +autoware::universe_utils::Polygon2d buffer( + const autoware::universe_utils::MultiPoint2d & multi_point, double distance, double segments) +{ + using namespace autoware::universe_utils; + + autoware::universe_utils::Polygon2d buffer_union; + bool first = true; + for (const auto & point : multi_point) { + autoware::universe_utils::Polygon2d buffer_point = buffer(point, distance, segments); + if (!first) { + buffer_union = autoware::universe_utils::union_(buffer_point, buffer_union)[0]; + } else { + buffer_union = buffer_point; + first = false; } - return buffer_union; + } + return buffer_union; } -} // namespace autoware::universe_utils +} // namespace autoware::universe_utils