diff --git a/vortex_utils/cpp_test/test_types.cpp b/vortex_utils/cpp_test/test_types.cpp index 4bcb555..a5396e1 100644 --- a/vortex_utils/cpp_test/test_types.cpp +++ b/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); +} diff --git a/vortex_utils/include/vortex/utils/types.hpp b/vortex_utils/include/vortex/utils/types.hpp index bcc5eeb..7cbbd99 100644 --- a/vortex_utils/include/vortex/utils/types.hpp +++ b/vortex_utils/include/vortex/utils/types.hpp @@ -1,8 +1,10 @@ #ifndef VORTEX_UTILS_TYPES_HPP #define VORTEX_UTILS_TYPES_HPP +#include #include #include +#include #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::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