se3.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "detail/macro.hpp"
#include "detail/se3.hpp"
#include "lie_group_base.hpp"
#include "so3.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<typename Scalar>
class SE2;
 
template<typename _Derived>
class SE3Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
 
protected:
  SE3Base() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  Map<SO3<Scalar>> so3()
    requires is_mutable
  {
    return Map<SO3<Scalar>>(static_cast<_Derived &>(*this).data() + 3);
  }
 
  Map<const SO3<Scalar>> so3() const { return Map<const SO3<Scalar>>(static_cast<const _Derived &>(*this).data() + 3); }
 
  Eigen::Map<Eigen::Vector3<Scalar>> r3()
    requires is_mutable
  {
    return Eigen::Map<Eigen::Vector3<Scalar>>(static_cast<_Derived &>(*this).data());
  }
 
  Eigen::Map<const Eigen::Vector3<Scalar>> r3() const
  {
    return Eigen::Map<const Eigen::Vector3<Scalar>>(static_cast<const _Derived &>(*this).data());
  }
 
  Eigen::Transform<Scalar, 3, Eigen::Isometry> isometry() const
  {
    return Eigen::Translation<Scalar, 3>(r3()) * so3().quat();
  }
 
  template<typename EigenDerived>
  Eigen::Vector3<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return so3() * v + r3();
  }
 
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 3, 6> dr_action(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    Eigen::Matrix<Scalar, 3, 6> ret;
    ret.template leftCols<3>()  = so3().matrix();
    ret.template rightCols<3>() = so3().dr_action(v);
    return ret;
  }
 
  SE2<Scalar> project_se2() const { return SE2<Scalar>(so3().project_so2(), r3().template head<2>()); }
};
 
// \cond
template<typename _Scalar>
class SE3;
// \endcond
 
// \cond
template<typename _Scalar>
struct liebase_info<SE3<_Scalar>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = SE3Impl<_Scalar>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = SE3<NewScalar>;
};
// \endcond
 
template<typename _Scalar>
class SE3 : public SE3Base<SE3<_Scalar>>
{
  using Base = SE3Base<SE3<_Scalar>>;
 
  SMOOTH_GROUP_API(SE3);
 
public:
  template<typename SO3Derived, typename T3Derived>
  SE3(const SO3Base<SO3Derived> & so3, const Eigen::MatrixBase<T3Derived> & r3)
  {
    Base::so3() = static_cast<const SO3Derived &>(so3);
    Base::r3()  = static_cast<const T3Derived &>(r3);
  }
 
  explicit SE3(const Eigen::Transform<Scalar, 3, Eigen::Isometry> & t)
  {
    Base::so3() = smooth::SO3<Scalar>(Eigen::Quaternion<Scalar>(t.rotation()));
    Base::r3()  = t.translation();
  }
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<SE3<_Scalar>>> : public liebase_info<SE3<_Scalar>>
{};
// \endcond
 
template<typename _Scalar>
class Map<SE3<_Scalar>> : public SE3Base<Map<SE3<_Scalar>>>
{
  using Base = SE3Base<Map<SE3<_Scalar>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<const SE3<_Scalar>>> : public liebase_info<SE3<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar>
class Map<const SE3<_Scalar>> : public SE3Base<Map<const SE3<_Scalar>>>
{
  using Base = SE3Base<Map<const SE3<_Scalar>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
using SE3f = SE3<float>;   
using SE3d = SE3<double>;  
 
SMOOTH_END_NAMESPACE
 
// Std format
#if __has_include(<format>)
#include <format>
#include <string>
 
template<class Scalar>
struct std::formatter<smooth::SE3<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::SE3<Scalar> & obj, std::format_context & ctx) const
  {
    const auto fmtSting = std::format("r3: [{0}, {0}, {0}], so3: [{0}, {0}, {0}, {0}]", m_format);
    return std::vformat_to(
      ctx.out(),
      fmtSting,
      std::make_format_args(
        obj.r3().x(),
        obj.r3().y(),
        obj.r3().z(),
        obj.so3().quat().w(),
        obj.so3().quat().x(),
        obj.so3().quat().y(),
        obj.so3().quat().z()));
  }
};
 
#endif