[trajoptlib] Normalize Rotation2d constructor arguments

SleipnirGroup · Jan 4, 2025 · 274133f · 274133f
1 parent f8816ee
commit 274133f
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 2 deletions.
diff --git 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<T, double>
+  {
+    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
@@ -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]") {