diff --git a/trajoptlib/include/trajopt/geometry/Rotation2.hpp b/trajoptlib/include/trajopt/geometry/Rotation2.hpp index 9f04bf649..91650ab55 100644 --- a/trajoptlib/include/trajopt/geometry/Rotation2.hpp +++ b/trajoptlib/include/trajopt/geometry/Rotation2.hpp @@ -41,6 +41,26 @@ class Rotation2 { constexpr Rotation2(T cos, T sin) : m_cos{std::move(cos)}, m_sin{std::move(sin)} {} + /** + * Constructs a rotation with the given x and y components. The x and y don't + * have to be normalized. + * + * @param x The x component of the rotation. + * @param y The y component of the rotation. + */ + constexpr Rotation2(double x, double y) + requires std::same_as + { + double magnitude = std::hypot(x, y); + if (magnitude > 1e-6) { + m_cos = x / magnitude; + m_sin = y / magnitude; + } else { + m_cos = 1.0; + m_sin = 0.0; + } + } + /** * Coerces one rotation type into another. * diff --git a/trajoptlib/test/src/geometry/Translation2dTest.cpp b/trajoptlib/test/src/geometry/Translation2dTest.cpp index 8f42d1a63..a00ca07f7 100644 --- a/trajoptlib/test/src/geometry/Translation2dTest.cpp +++ b/trajoptlib/test/src/geometry/Translation2dTest.cpp @@ -75,8 +75,17 @@ TEST_CASE("Translation2d - Angle", "[Translation2d]") { const trajopt::Translation2d one{1.0, 3.0}; const trajopt::Translation2d two{2.0, 5.0}; - CHECK(one.Angle().Radians() == std::atan2(3.0, 1.0)); - CHECK(two.Angle().Radians() == std::atan2(5.0, 2.0)); + const auto oneAngle = one.Angle(); + CHECK(oneAngle.Cos() == Catch::Approx(1.0 / std::sqrt(10.0)).margin(1e-15)); + CHECK(oneAngle.Sin() == Catch::Approx(3.0 / std::sqrt(10.0)).margin(1e-15)); + CHECK(oneAngle.Radians() == + Catch::Approx(std::atan2(3.0, 1.0)).margin(1e-15)); + + const auto twoAngle = two.Angle(); + CHECK(twoAngle.Cos() == Catch::Approx(2.0 / std::sqrt(29.0)).margin(1e-15)); + CHECK(twoAngle.Sin() == Catch::Approx(5.0 / std::sqrt(29.0)).margin(1e-15)); + CHECK(twoAngle.Radians() == + Catch::Approx(std::atan2(5.0, 2.0)).margin(1e-15)); } TEST_CASE("Translation2d - Dot", "[Translation2d]") {