c1.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <complex>
 
#include <Eigen/Core>
 
#include "detail/c1.hpp"
#include "detail/macro.hpp"
#include "lie_group_base.hpp"
#include "smooth/detail/macro.hpp"
#include "so2.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
// \cond
template<typename Scalar>
class SO2;
// \endcond
 
template<typename _Derived>
class C1Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
 
protected:
  C1Base() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  Scalar angle() const
  {
    using std::atan2;
 
    return atan2(static_cast<const _Derived &>(*this).coeffs().x(), static_cast<const _Derived &>(*this).coeffs().y());
  }
 
  Scalar scaling() const
  {
    using std::sqrt;
 
    return sqrt(
      static_cast<const _Derived &>(*this).coeffs().x() * static_cast<const _Derived &>(*this).coeffs().x()
      + static_cast<const _Derived &>(*this).coeffs().y() * static_cast<const _Derived &>(*this).coeffs().y());
  }
 
  SO2<Scalar> so2() const
  {
    return SO2<Scalar>(c1());  // it's normalized inside SO2
  }
 
  std::complex<Scalar> c1() const
  {
    return std::complex<Scalar>(
      static_cast<const _Derived &>(*this).coeffs().y(), static_cast<const _Derived &>(*this).coeffs().x());
  }
 
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 2, 1> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return Base::matrix() * v;
  }
};
 
// \cond
template<typename _Scalar>
class C1;
// \endcond
 
// \cond
template<typename _Scalar>
struct liebase_info<C1<_Scalar>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = C1Impl<_Scalar>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = C1<NewScalar>;
};
// \endcond
 
template<typename _Scalar>
class C1 : public C1Base<C1<_Scalar>>
{
  using Base = C1Base<C1<_Scalar>>;
  SMOOTH_GROUP_API(C1);
 
public:
  C1(const Scalar & scaling, const Scalar & angle)
  {
    using std::cos, std::sin;
 
    m_coeffs.x() = scaling * sin(angle);
    m_coeffs.y() = scaling * cos(angle);
  }
 
  explicit C1(const std::complex<Scalar> & c)
  {
    m_coeffs.x() = c.imag();
    m_coeffs.y() = c.real();
  }
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<C1<_Scalar>>> : public liebase_info<C1<_Scalar>>
{};
// \endcond
 
template<typename _Scalar>
class Map<C1<_Scalar>> : public C1Base<Map<C1<_Scalar>>>
{
  using Base = C1Base<Map<C1<_Scalar>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<const C1<_Scalar>>> : public liebase_info<C1<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar>
class Map<const C1<_Scalar>> : public C1Base<Map<const C1<_Scalar>>>
{
  using Base = C1Base<Map<const C1<_Scalar>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
using C1f = C1<float>;   
using C1d = C1<double>;  
 
SMOOTH_END_NAMESPACE