Added useful functions to so2 (#45)

* Added angle_cw and angle_ccw to so2 * Added so2::unit_complex --------- Co-authored-by: Thomas Gurriet <tgurriet@3lawsrobotics.com>
pettni · Oct 7, 2023 · f43a0b2 · f43a0b2
1 parent ea1da4b
commit f43a0b2
Show file tree

Hide file tree

Showing 2 changed files with 61 additions and 3 deletions.
diff --git a/include/smooth/so2.hpp b/include/smooth/so2.hpp
@@ -70,10 +70,51 @@ class SO2Base : public LieGroupBase<_Derived>
   SMOOTH_INHERIT_TYPEDEFS;
 
   /**
-   * @brief Angle represetation.
+   * @brief Angle representation, in \f$[-\pi,\pi]\f$ range
    */
   Scalar angle() const { return Base::log().x(); }
 
+  /**
+   * @brief Angle representation in clockwise direction, in \f$[-2\pi,0]\f$ range
+   */
+  Scalar angle_cw() const
+  {
+    const auto & x = static_cast<const _Derived &>(*this).coeffs().y();
+    const auto & y = static_cast<const _Derived &>(*this).coeffs().x();
+
+    using std::atan2;
+    if (y <= 0.) {
+      return atan2(y, x);
+    } else {
+      return atan2(-y, -x) - Scalar(M_PI);
+    }
+  }
+
+  /**
+   * @brief Angle representation in clockwise direction, in \f$[0,2\pi]\f$ range
+   */
+  Scalar angle_ccw() const
+  {
+    const auto & x = static_cast<const _Derived &>(*this).coeffs().y();
+    const auto & y = static_cast<const _Derived &>(*this).coeffs().x();
+
+    using std::atan2;
+    if (y >= 0.) {
+      return atan2(y, x);
+    } else {
+      return Scalar(M_PI) + atan2(-y, -x);
+    }
+  }
+
+  /**
+   * @brief Unit complex number (U(1)) representation.
+   */
+  Eigen::Vector2<Scalar> unit_complex() const
+  {
+    return Eigen::Vector2<Scalar>(
+      static_cast<const _Derived &>(*this).coeffs().y(), static_cast<const _Derived &>(*this).coeffs().x());
+  }
+
   /**
    * @brief Unit complex number (U(1)) representation.
    */

diff --git a/tests/test_so2.cpp b/tests/test_so2.cpp
@@ -10,10 +10,27 @@ TEST(SO2, Angle)
   for (auto i = 0; i != 5; ++i) {
     const auto g1 = smooth::SO2d::Random();
     const auto g2 = smooth::SO2d::Random();
+    {
+      smooth::SO2d g_prod(g1.angle() + g2.angle());
 
-    smooth::SO2d g_prod(g1.angle() + g2.angle());
+      ASSERT_TRUE(g_prod.isApprox(g1 * g2));
+      ASSERT_LE(g1.angle(), M_PI);
+      ASSERT_GE(g1.angle(), -M_PI);
+    }
+    {
+      smooth::SO2d g_prod(g1.angle_cw() + g2.angle_cw());
 
-    ASSERT_TRUE(g_prod.isApprox(g1 * g2));
+      ASSERT_TRUE(g_prod.isApprox(g1 * g2));
+      ASSERT_LE(g1.angle_cw(), 0);
+      ASSERT_GE(g1.angle_cw(), -2. * M_PI);
+    }
+    {
+      smooth::SO2d g_prod(g1.angle_ccw() + g2.angle_ccw());
+
+      ASSERT_TRUE(g_prod.isApprox(g1 * g2));
+      ASSERT_LE(g1.angle_ccw(), 2. * M_PI);
+      ASSERT_GE(g1.angle_ccw(), 0);
+    }
   }
 }