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

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