feat(autoware_universe_utils): reduce dependence on Boost.Geometry (a…

…utowarefoundation#7778) * add within function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * return nullopt as is Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add disjoint function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add polygon-and-polygon version of intersect function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * use intersect for disjoint Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add test case for disjoint Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * checking intersection of edges is unnecessary Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * return nullopt when no intersection point found Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add distance function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add coveredBy function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add point-polygon variant of distance function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add isAbove function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add divideBySegment function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add convexHull function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add correct function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add area function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change point type to tf2::Vector3 Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * simplify correct function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * push geometry types to namespace Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * match the behavior of Boost.Geometry Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add test cases for benchmarking Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add headers for convex_hull() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * remove polygon-polygon intersect & disjoint function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add intersects function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add touches function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add disjoint function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * minor fix Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change name Polygon to CvxPolygon Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change name CvxPolygon to ConvexPolygon Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * rename intersect function and restore the original Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change function names to snake_case Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * early return Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change point type from tf2::Vector3 to custom struct Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * style(pre-commit): autofix * use alt::Vector2d to represent point Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * convert from boost before time measurement Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add header for std::move Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * avoid using long Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * convert from boost before time measurement Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add point-segment variant of touches function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of point-polygon touches() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of area() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add note for class naming Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of covered_by() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * simplify within() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of covered_by() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of within() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * use operator[] instead of at() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * print point when covered_by() test failed Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * avoid using hypot() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performace of convex_hull() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * remove divide_by_segment() function Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * fix test cases Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of touches() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add test case for touches() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * improve performance of touches() Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * change type alias PointList to Points2d Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * add & fix vector size assertions Signed-off-by: mitukou1109 <mitukou1109@gmail.com> * define epsilon respectively Signed-off-by: mitukou1109 <mitukou1109@gmail.com> --------- Signed-off-by: mitukou1109 <mitukou1109@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
esteve · Aug 13, 2024 · 8c64788 · 8c64788
1 parent bcbb599
commit 8c64788
Show file tree

Hide file tree

Showing 3 changed files with 1,630 additions and 0 deletions.
diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/geometry/geometry.hpp
@@ -22,6 +22,7 @@
 
 #include <exception>
 #include <string>
+#include <utility>
 #include <vector>
 
 #define EIGEN_MPL2_ONLY
@@ -583,6 +584,148 @@ std::optional<geometry_msgs::msg::Point> intersect(
  */
 bool intersects_convex(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);
 
+// Alternatives for Boost.Geometry ----------------------------------------------------------------
+
+// TODO(mitukou1109): remove namespace when migrating to alternatives
+namespace alt
+{
+class Vector2d
+{
+public:
+  Vector2d() : x_(0.0), y_(0.0) {}
+
+  Vector2d(const double x, const double y) : x_(x), y_(y) {}
+
+  double cross(const Vector2d & other) const { return x_ * other.y() - y_ * other.x(); }
+
+  double dot(const Vector2d & other) const { return x_ * other.x() + y_ * other.y(); }
+
+  double norm2() const { return x_ * x_ + y_ * y_; }
+
+  double norm() const { return std::sqrt(norm2()); }
+
+  Vector2d vector_triple(const Vector2d & v1, const Vector2d & v2) const
+  {
+    const auto tmp = this->cross(v1);
+    return {-v2.y() * tmp, v2.x() * tmp};
+  }
+
+  const double & x() const { return x_; }
+
+  double & x() { return x_; }
+
+  const double & y() const { return y_; }
+
+  double & y() { return y_; }
+
+private:
+  double x_;
+  double y_;
+};
+
+// We use Vector2d to represent points, but we do not name the class Point2d directly
+// as it has some vector operation functions.
+using Point2d = Vector2d;
+using Points2d = std::vector<Point2d>;
+
+class ConvexPolygon2d
+{
+public:
+  explicit ConvexPolygon2d(const Points2d & vertices)
+  {
+    if (vertices.size() < 3) {
+      throw std::invalid_argument("At least 3 points are required for vertices.");
+    }
+    vertices_ = vertices;
+  }
+
+  explicit ConvexPolygon2d(Points2d && vertices)
+  {
+    if (vertices.size() < 3) {
+      throw std::invalid_argument("At least 3 points are required for vertices.");
+    }
+    vertices_ = std::move(vertices);
+  }
+
+  const Points2d & vertices() const { return vertices_; }
+
+  Points2d & vertices() { return vertices_; }
+
+private:
+  Points2d vertices_;
+};
+
+inline Vector2d operator+(const Vector2d & v1, const Vector2d & v2)
+{
+  return {v1.x() + v2.x(), v1.y() + v2.y()};
+}
+
+inline Vector2d operator-(const Vector2d & v1, const Vector2d & v2)
+{
+  return {v1.x() - v2.x(), v1.y() - v2.y()};
+}
+
+inline Vector2d operator-(const Vector2d & v)
+{
+  return {-v.x(), -v.y()};
+}
+
+inline Vector2d operator*(const double & s, const Vector2d & v)
+{
+  return {s * v.x(), s * v.y()};
+}
+
+Point2d from_geom(const geometry_msgs::msg::Point & point);
+
+Point2d from_boost(const autoware::universe_utils::Point2d & point);
+
+ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon);
+
+autoware::universe_utils::Point2d to_boost(const Point2d & point);
+
+autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon);
+}  // namespace alt
+
+double area(const alt::ConvexPolygon2d & poly);
+
+alt::ConvexPolygon2d convex_hull(const alt::Points2d & points);
+
+void correct(alt::ConvexPolygon2d & poly);
+
+bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
+
+bool disjoint(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
+
+double distance(
+  const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
+
+double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
+
+bool equals(const alt::Point2d & point1, const alt::Point2d & point2);
+
+bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
+
+bool intersects(
+  const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start,
+  const alt::Point2d & seg2_end);
+
+bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
+
+bool is_above(
+  const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
+
+bool is_clockwise(const alt::ConvexPolygon2d & poly);
+
+bool touches(
+  const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
+
+bool touches(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
+
+bool within(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
+
+bool within(
+  const alt::ConvexPolygon2d & poly_contained, const alt::ConvexPolygon2d & poly_containing);
+
 }  // namespace autoware::universe_utils
 
 #endif  // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GEOMETRY_HPP_