Feat/line2 d type

vortex_utils/cpp_test/test_types.cpp

@@ -143,3 +143,74 @@ TEST_F(TypesTests, test_twist) {
     EXPECT_NEAR(diff.q, 0.3, 1e-12);
     EXPECT_NEAR(diff.r, 1.4, 1e-12);
 }
+
+TEST_F(TypesTests, test_line_segment_2d) {
+    vortex::utils::types::LineSegment2D line_segment;
+    line_segment.p0 = {1.0, 0.2};
+    line_segment.p1 = {0.4, 1.2};
+
+    vortex::utils::types::Line2D line2d = line_segment.polar_parametrization();
+
+    double expected_rho = 0.96039207679805;     // sqrt(13)
+    double expected_theta = 0.540419500270584;  // atan2(4, 3)
+
+    EXPECT_NEAR(line2d.rho, expected_rho, 1e-12);
+    EXPECT_NEAR(line2d.theta, expected_theta, 1e-12);
+}
+
+TEST_F(TypesTests, test_line_segment_vertical) {
+    vortex::utils::types::LineSegment2D line_segment;
+    line_segment.p0 = {1.0, 0.0};
+    line_segment.p1 = {1.0, 1.2};
+
+    vortex::utils::types::Line2D line2d = line_segment.polar_parametrization();
+
+    double expected_rho = 1.0;
+    double expected_theta = 0.0;
+
+    EXPECT_NEAR(line2d.rho, expected_rho, 1e-12);
+    EXPECT_NEAR(line2d.theta, expected_theta, 1e-12);
+}
+
+TEST_F(TypesTests, test_line_segment_horizontal) {
+    vortex::utils::types::LineSegment2D line_segment;
+    line_segment.p0 = {1.2, 1.2};
+    line_segment.p1 = {0, 1.2};
+
+    vortex::utils::types::Line2D line2d = line_segment.polar_parametrization();
+
+    double expected_rho = 1.2;
+    double expected_theta = M_PI / 2.0;
+
+    EXPECT_NEAR(line2d.rho, expected_rho, 1e-12);
+    EXPECT_NEAR(line2d.theta, expected_theta, 1e-12);
+}
+
+TEST_F(TypesTests, test_line_segment_zero_length) {
+    vortex::utils::types::LineSegment2D line_segment;
+    line_segment.p0 = {0.0, 0.0};
+    line_segment.p1 = {0.0, 0.0};
+
+    ASSERT_THROW(line_segment.polar_parametrization(), std::runtime_error);
+}
+
+TEST_F(TypesTests, test_line_segment_wrap_theta) {
+    vortex::utils::types::LineSegment2D line_segment1;
+    double epsilon = 0.01;
+    line_segment1.p0 = {0.4, -0.2};
+    line_segment1.p1 = {0.4 - epsilon, 0.4};
+
+    vortex::utils::types::Line2D line2d1 =
+        line_segment1.polar_parametrization();
+
+    vortex::utils::types::LineSegment2D line_segment2;
+    line_segment2.p0 = {0.4, -0.2};
+    line_segment2.p1 = {0.4 + epsilon, 0.4};
+
+    vortex::utils::types::Line2D line2d2 =
+        line_segment2.polar_parametrization();
+
+    EXPECT_NEAR(line2d1.rho, line2d2.rho, 0.1);
+
+    EXPECT_NEAR(line2d1.theta + 2 * M_PI, line2d2.theta, 0.1);
+}

vortex_utils/include/vortex/utils/types.hpp

@@ -1,8 +1,10 @@
 #ifndef VORTEX_UTILS_TYPES_HPP
 #define VORTEX_UTILS_TYPES_HPP
 
+#include <cmath>
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Dense>
+#include <stdexcept>
 #include "math.hpp"
 
 namespace vortex::utils::types {
@@ -25,6 +27,20 @@ struct Pose;
  */
 struct Twist;
 
+/**
+ * @brief Polar (Hesse normal) representation of a 2D line.
+ *
+ * @details
+ * - rho is the signed distance from the origin to the line along the normal.
+ * - theta is the angle (in radians) from the +x axis to the line's unit normal.
+ */
+struct Line2D;
+
+/**
+ * @brief Struct to represent a 2D line segment.
+ */
+struct LineSegment2D;
+
 struct PoseEuler {
     double x{};
     double y{};
@@ -328,6 +344,57 @@ inline PoseEuler Pose::as_pose_euler() const {
     return eta;
 }
 
+struct Point2D {
+    double x{};
+    double y{};
+};
+
+struct Line2D {
+    double rho{};    ///< Distance from origin to the line in canonical form.
+    double theta{};  ///< Angle (rad) from +x axis to the unit normal.
+};
+
+struct LineSegment2D {
+    Point2D p0{};
+    Point2D p1{};
+
+    /**
+     * @brief Get the polar (Hesse normal) parametrization of the line segment.
+     * Enforces positive rho and theta in [0, 2pi).
+     * @return Line2D with rho and theta
+     */
+    Line2D polar_parametrization() const {
+        const double dx = p1.x - p0.x;
+        const double dy = p1.y - p0.y;
+
+        const double len = std::hypot(dx, dy);
+
+        if (len <= std::numeric_limits<double>::epsilon()) {
+            throw std::runtime_error("Invalid length for line segment");
+        }
+
+        const double nx = -dy / len;
+        const double ny = dx / len;
+
+        double rho = nx * p0.x + ny * p0.y;
+        double theta = std::atan2(ny, nx);  // (-pi, pi]
+
+        // Enforce positive rho
+        if (rho < 0) {
+            rho = -rho;
+            theta += M_PI;
+        }
+
+        theta = std::fmod(theta, 2.0 * M_PI);
+
+        if (theta < 0) {
+            theta += 2.0 * M_PI;
+        }
+
+        return Line2D{.rho = rho, .theta = theta};
+    }
+};
+
 }  // namespace vortex::utils::types
 
 #endif  // VORTEX_UTILS_TYPES_HPP