so2.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <complex>
 
#include <Eigen/Core>
 
#include "detail/macro.hpp"
#include "detail/so2.hpp"
#include "lie_group_base.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
// \cond
template<typename Scalar>
class SO3;
// \endcond
 
template<typename _Derived>
class SO2Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
 
protected:
  SO2Base() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  Scalar angle() const { return Base::log().x(); }
 
  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);
    }
  }
 
  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);
    }
  }
 
  Eigen::Vector2<Scalar> unit_complex() const
  {
    return Eigen::Vector2<Scalar>(
      static_cast<const _Derived &>(*this).coeffs().y(), static_cast<const _Derived &>(*this).coeffs().x());
  }
 
  std::complex<Scalar> u1() const
  {
    return std::complex<Scalar>(
      static_cast<const _Derived &>(*this).coeffs().y(), static_cast<const _Derived &>(*this).coeffs().x());
  }
 
  template<typename EigenDerived>
  Eigen::Vector2<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return Base::matrix() * v;
  }
 
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> dr_action(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return Base::matrix() * Base::hat(Eigen::Vector<Scalar, 1>::Ones()) * v;
  }
 
  SO3<Scalar> lift_so3() const
  {
    using std::cos, std::sin;
 
    const Scalar yaw = Base::log().x();
    return SO3<Scalar>(Eigen::Quaternion<Scalar>(cos(yaw / 2), 0, 0, sin(yaw / 2)));
  }
};
 
// \cond
template<typename _Scalar>
class SO2;
// \endcond
 
// \cond
template<typename _Scalar>
struct liebase_info<SO2<_Scalar>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = SO2Impl<_Scalar>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = SO2<NewScalar>;
};
// \endcond
 
template<typename _Scalar>
class SO2 : public SO2Base<SO2<_Scalar>>
{
  using Base = SO2Base<SO2<_Scalar>>;
  SMOOTH_GROUP_API(SO2);
 
public:
  SO2(const Scalar & qz, const Scalar & qw)
  {
    using std::sqrt;
 
    const Scalar n = sqrt(qw * qw + qz * qz);
    m_coeffs.x()   = qz / n;
    m_coeffs.y()   = qw / n;
  }
 
  explicit SO2(const Scalar & angle)
  {
    using std::cos, std::sin;
 
    m_coeffs.x() = sin(angle);
    m_coeffs.y() = cos(angle);
  }
 
  explicit SO2(const std::complex<Scalar> & c)
  {
    using std::sqrt;
 
    const Scalar n = sqrt(c.imag() * c.imag() + c.real() * c.real());
    m_coeffs.x()   = c.imag() / n;
    m_coeffs.y()   = c.real() / n;
  }
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<SO2<_Scalar>>> : public liebase_info<SO2<_Scalar>>
{};
// \endcond
 
template<typename _Scalar>
class Map<SO2<_Scalar>> : public SO2Base<Map<SO2<_Scalar>>>
{
  using Base = SO2Base<Map<SO2<_Scalar>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<const SO2<_Scalar>>> : public liebase_info<SO2<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar>
class Map<const SO2<_Scalar>> : public SO2Base<Map<const SO2<_Scalar>>>
{
  using Base = SO2Base<Map<const SO2<_Scalar>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
using SO2f = SO2<float>;   
using SO2d = SO2<double>;  
 
SMOOTH_END_NAMESPACE
 
// Std format
#if __has_include(<format>)
#include <format>
#include <string>
 
template<class Scalar>
struct std::formatter<smooth::SO2<Scalar>>
{
  std::string m_format;
 
  constexpr auto parse(std::format_parse_context & ctx)
  {
    m_format = "{:";
    for (auto it = ctx.begin(); it != ctx.end(); ++it) {
      char c = *it;
      m_format += c;
      if (c == '}') return it;
    }
    return ctx.end();
  }
 
  auto format(const smooth::SO2<Scalar> & obj, std::format_context & ctx) const
  {
    return std::vformat_to(ctx.out(), m_format, std::make_format_args(obj.angle()));
  }
};
 
#endif