From c6e46da11724dc52999014a771750c80e952e853 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 25 Jun 2024 21:02:47 -0700
Subject: [PATCH] Remove Rotation2 normalization and assertions

For our use cases, users are only ever passing angles or cos/sin pairs
into Rotation2. This also avoids normalization bugs caused by
uninitialized autodiff variables, though that could have just been
special-cased.
---
 include/trajopt/geometry/Rotation2.hpp  | 21 +++++----------------
 test/src/geometry/Translation2dTest.cpp |  3 +--
 2 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/include/trajopt/geometry/Rotation2.hpp b/include/trajopt/geometry/Rotation2.hpp
index 54295f9f..148665bf 100644
--- a/include/trajopt/geometry/Rotation2.hpp
+++ b/include/trajopt/geometry/Rotation2.hpp
@@ -34,24 +34,13 @@ class Rotation2 {
       : m_cos{cos(angle)}, m_sin{sin(angle)} {}  // NOLINT
 
   /**
-   * Constructs a rotation with the given x and y (cosine and sine) components.
-   * The x and y don't have to be normalized.
+   * Constructs a rotation with the given cosine and sine components.
    *
-   * @param x The x component or cosine of the rotation.
-   * @param y The y component or sine of the rotation.
+   * @param cos The cosine component of the rotation.
+   * @param sin The sine component of the rotation.
    */
-  constexpr Rotation2(T x, T y) : m_cos{std::move(x)}, m_sin{std::move(y)} {
-    auto magnitude = hypot(m_cos, m_sin);  // NOLINT
-    if (magnitude > 1e-6) {
-      if (abs(m_cos * m_cos + m_sin * m_sin - 1.0) > 1e-9) {  // NOLINT
-        m_cos /= magnitude;
-        m_sin /= magnitude;
-      }
-    } else {
-      m_cos = 1.0;
-      m_sin = 0.0;
-    }
-  }
+  constexpr Rotation2(T cos, T sin)
+      : m_cos{std::move(cos)}, m_sin{std::move(sin)} {}
 
   /**
    * Coerces one rotation type into another.
diff --git a/test/src/geometry/Translation2dTest.cpp b/test/src/geometry/Translation2dTest.cpp
index 0d51c224..4e53e6c2 100644
--- a/test/src/geometry/Translation2dTest.cpp
+++ b/test/src/geometry/Translation2dTest.cpp
@@ -76,8 +76,7 @@ TEST_CASE("Translation2d - Angle", "[Translation2d]") {
   const trajopt::Translation2d two{2.0, 5.0};
 
   CHECK(one.Angle().Radians() == std::atan2(3.0, 1.0));
-  CHECK(two.Angle().Radians() ==
-        Catch::Approx(std::atan2(5.0, 2.0)).margin(1e-9));
+  CHECK(two.Angle().Radians() == std::atan2(5.0, 2.0));
 }
 
 TEST_CASE("Translation2d - Dot", "[Translation2d]") {