se_k_3.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "detail/macro.hpp"
#include "detail/se_k_3.hpp"
#include "detail/utils.hpp"
#include "lie_group_base.hpp"
#include "so3.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<typename _Derived>
class SE_K_3Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
  using Impl = typename liebase_info<_Derived>::Impl;
 
protected:
  SE_K_3Base() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  static constexpr auto K = Impl::K;
 
  Map<SO3<Scalar>> so3()
    requires is_mutable
  {
    return Map<SO3<Scalar>>(static_cast<_Derived &>(*this).data() + 3 * K);
  }
 
  Map<const SO3<Scalar>> so3() const
  {
    return Map<const SO3<Scalar>>(static_cast<const _Derived &>(*this).data() + 3 * K);
  }
 
  template<int Ksel>
  Eigen::Map<Eigen::Vector3<Scalar>> r3()
    requires(is_mutable && Ksel < K)
  {
    return Eigen::Map<Eigen::Vector3<Scalar>>(static_cast<_Derived &>(*this).data() + 3 * Ksel);
  }
 
  Eigen::Map<Eigen::Vector3<Scalar>> r3(int k)
    requires is_mutable
  {
    assert(k < K);
    return Eigen::Map<Eigen::Vector3<Scalar>>(static_cast<_Derived &>(*this).data() + 3 * k);
  }
 
  template<int Ksel>
  Eigen::Map<const Eigen::Vector3<Scalar>> r3() const
    requires(Ksel < K)
  {
    return Eigen::Map<const Eigen::Vector3<Scalar>>(static_cast<const _Derived &>(*this).data() + 3 * Ksel);
  }
 
  Eigen::Map<const Eigen::Vector3<Scalar>> r3(int k) const
  {
    assert(k < K);
    return Eigen::Map<const Eigen::Vector3<Scalar>>(static_cast<const _Derived &>(*this).data() + 3 * k);
  }
};
 
// \cond
template<typename _Scalar, int K>
class SE_K_3;
// \endcond
 
// \cond
template<typename _Scalar, int K>
struct liebase_info<SE_K_3<_Scalar, K>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = SE_K_3Impl<_Scalar, K>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = SE_K_3<NewScalar, K>;
};
// \endcond
 
template<typename _Scalar, int K>
class SE_K_3 : public SE_K_3Base<SE_K_3<_Scalar, K>>
{
  using Base = SE_K_3Base<SE_K_3<_Scalar, K>>;
 
  SMOOTH_GROUP_API(SE_K_3);
 
public:
  template<typename SO3Derived, typename... RnDerived>
  SE_K_3(const SO3Base<SO3Derived> & so3, const Eigen::MatrixBase<RnDerived> &... r3s)
    requires(sizeof...(r3s) == K)
  {
    const auto tpl = std::forward_as_tuple(r3s...);
    Base::so3()    = static_cast<const SO3Derived &>(so3);
#ifdef __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-lambda-capture"
#endif
    utils::static_for<K>([this, &tpl](auto i) { Base::template r3<i>() = std::get<i>(tpl); });
#ifdef __clang__
#pragma GCC diagnostic pop
#endif
  }
};
 
// \cond
template<typename _Scalar, int _K>
struct liebase_info<Map<SE_K_3<_Scalar, _K>>> : public liebase_info<SE_K_3<_Scalar, _K>>
{};
// \endcond
 
template<typename _Scalar, int _K>
class Map<SE_K_3<_Scalar, _K>> : public SE_K_3Base<Map<SE_K_3<_Scalar, _K>>>
{
  using Base = SE_K_3Base<Map<SE_K_3<_Scalar, _K>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar, int _K>
struct liebase_info<Map<const SE_K_3<_Scalar, _K>>> : public liebase_info<SE_K_3<_Scalar, _K>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar, int _K>
class Map<const SE_K_3<_Scalar, _K>> : public SE_K_3Base<Map<const SE_K_3<_Scalar, _K>>>
{
  using Base = SE_K_3Base<Map<const SE_K_3<_Scalar, _K>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
SMOOTH_END_NAMESPACE