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

…8592)

* add find_farthest()

Signed-off-by: mitukou1109 <[email protected]>

* add simplify()

Signed-off-by: mitukou1109 <[email protected]>

* add envelope()

Signed-off-by: mitukou1109 <[email protected]>

* (WIP) add buffer()

Signed-off-by: mitukou1109 <[email protected]>

* add Polygon2d class

Signed-off-by: mitukou1109 <[email protected]>

* change input type of envelope()

Signed-off-by: mitukou1109 <[email protected]>

* disable convexity check until correct() supports non-convex polygons

Signed-off-by: mitukou1109 <[email protected]>

* add is_clockwise()

Signed-off-by: mitukou1109 <[email protected]>

* make correct() support non-convex polygons

Signed-off-by: mitukou1109 <[email protected]>

* fix test case

Signed-off-by: mitukou1109 <[email protected]>

* Revert "(WIP) add buffer()"

This reverts commit 123b0ba.

Signed-off-by: mitukou1109 <[email protected]>

---------

Signed-off-by: mitukou1109 <[email protected]>

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

@@ -17,6 +17,8 @@
 
 #include "autoware/universe_utils/geometry/boost_geometry.hpp"
 
+#include <list>
+#include <optional>
 #include <utility>
 #include <vector>
 
@@ -33,6 +35,10 @@ class Vector2d
 
   Vector2d(const double x, const double y) : x_(x), y_(y) {}
 
+  explicit Vector2d(const autoware::universe_utils::Point2d & point) : x_(point.x()), y_(point.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(); }
@@ -60,38 +66,6 @@ class Vector2d
   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()};
@@ -112,20 +86,74 @@ inline Vector2d operator*(const double & s, const Vector2d & v)
   return {s * v.x(), s * v.y()};
 }
 
-Point2d from_boost(const autoware::universe_utils::Point2d & point);
+// 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>;
+using PointList2d = std::list<Point2d>;
+
+class Polygon2d
+{
+public:
+  static std::optional<Polygon2d> create(
+    const PointList2d & outer, const std::vector<PointList2d> & inners) noexcept;
 
-ConvexPolygon2d from_boost(const autoware::universe_utils::Polygon2d & polygon);
+  static std::optional<Polygon2d> create(
+    PointList2d && outer, std::vector<PointList2d> && inners) noexcept;
 
-autoware::universe_utils::Point2d to_boost(const Point2d & point);
+  static std::optional<Polygon2d> create(
+    const autoware::universe_utils::Polygon2d & polygon) noexcept;
 
-autoware::universe_utils::Polygon2d to_boost(const ConvexPolygon2d & polygon);
+  const PointList2d & outer() const noexcept { return outer_; }
+
+  PointList2d & outer() noexcept { return outer_; }
+
+  const std::vector<PointList2d> & inners() const noexcept { return inners_; }
+
+  std::vector<PointList2d> & inners() noexcept { return inners_; }
+
+protected:
+  Polygon2d(const PointList2d & outer, const std::vector<PointList2d> & inners)
+  : outer_(outer), inners_(inners)
+  {
+  }
+
+  Polygon2d(PointList2d && outer, std::vector<PointList2d> && inners)
+  : outer_(std::move(outer)), inners_(std::move(inners))
+  {
+  }
+
+  PointList2d outer_;
+
+  std::vector<PointList2d> inners_;
+};
+
+class ConvexPolygon2d : public Polygon2d
+{
+public:
+  static std::optional<ConvexPolygon2d> create(const PointList2d & vertices) noexcept;
+
+  static std::optional<ConvexPolygon2d> create(PointList2d && vertices) noexcept;
+
+  static std::optional<ConvexPolygon2d> create(
+    const autoware::universe_utils::Polygon2d & polygon) noexcept;
+
+  const PointList2d & vertices() const noexcept { return outer(); }
+
+  PointList2d & vertices() noexcept { return outer(); }
+
+private:
+  explicit ConvexPolygon2d(const PointList2d & vertices) : Polygon2d(vertices, {}) {}
+
+  explicit ConvexPolygon2d(PointList2d && vertices) : Polygon2d(std::move(vertices), {}) {}
+};
 }  // namespace alt
 
 double area(const alt::ConvexPolygon2d & poly);
 
-alt::ConvexPolygon2d convex_hull(const alt::Points2d & points);
+std::optional<alt::ConvexPolygon2d> convex_hull(const alt::Points2d & points);
 
-void correct(alt::ConvexPolygon2d & poly);
+void correct(alt::Polygon2d & poly);
 
 bool covered_by(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
 
@@ -136,9 +164,14 @@ double distance(
 
 double distance(const alt::Point2d & point, const alt::ConvexPolygon2d & poly);
 
+std::optional<alt::ConvexPolygon2d> envelope(const alt::Polygon2d & poly);
+
 bool equals(const alt::Point2d & point1, const alt::Point2d & point2);
 
-bool equals(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d & poly2);
+bool equals(const alt::Polygon2d & poly1, const alt::Polygon2d & poly2);
+
+alt::Points2d::const_iterator find_farthest(
+  const alt::Points2d & points, const alt::Point2d & seg_start, const alt::Point2d & seg_end);
 
 bool intersects(
   const alt::Point2d & seg1_start, const alt::Point2d & seg1_end, const alt::Point2d & seg2_start,
@@ -149,7 +182,11 @@ bool intersects(const alt::ConvexPolygon2d & poly1, const alt::ConvexPolygon2d &
 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 is_clockwise(const alt::PointList2d & vertices);
+
+bool is_convex(const alt::Polygon2d & poly);
+
+alt::PointList2d simplify(const alt::PointList2d & line, const double max_distance);
 
 bool touches(
   const alt::Point2d & point, const alt::Point2d & seg_start, const alt::Point2d & seg_end);