Make Rotation2 normalize inputs if required (#215)

include/trajopt/geometry/Rotation2.hpp

@@ -2,7 +2,6 @@
 
 #pragma once
 
-#include <cassert>
 #include <cmath>
 #include <concepts>
 #include <numbers>
@@ -36,13 +35,22 @@ class Rotation2 {
 
   /**
    * Constructs a rotation with the given x and y (cosine and sine) components.
-   * x and y must be normalized.
+   * The x and y don't have to be normalized.
    *
    * @param x The x component or cosine of the rotation.
    * @param y The y component or sine of the rotation.
    */
   constexpr Rotation2(T x, T y) : m_cos{std::move(x)}, m_sin{std::move(y)} {
-    assert(abs(x * x + y * y - 1.0) < 1e-9);  // NOLINT
+    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;
+    }
   }
 
   /**

test/src/geometry/Translation2dTest.cpp

@@ -76,7 +76,8 @@ 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() == std::atan2(5.0, 2.0));
+  CHECK(two.Angle().Radians() ==
+        Catch::Approx(std::atan2(5.0, 2.0)).margin(1e-9));
 }
 
 TEST_CASE("Translation2d - Dot", "[Translation2d]") {