so3.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "detail/macro.hpp"
#include "detail/so3.hpp"
#include "lie_group_base.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
// \cond
template<typename Scalar>
class SO2;
// \endcond
 
template<typename _Derived>
class SO3Base : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
 
protected:
  SO3Base() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  Eigen::Map<Eigen::Quaternion<Scalar>> quat()
    requires is_mutable
  {
    return Eigen::Map<Eigen::Quaternion<Scalar>>(static_cast<_Derived &>(*this).data());
  }
 
  Eigen::Map<const Eigen::Quaternion<Scalar>> quat() const
  {
    return Eigen::Map<const Eigen::Quaternion<Scalar>>(static_cast<const _Derived &>(*this).data());
  }
 
  Eigen::Vector3<Scalar> eulerAngles(Eigen::Index i1 = 2, Eigen::Index i2 = 1, Eigen::Index i3 = 0) const
  {
    return quat().toRotationMatrix().eulerAngles(i1, i2, i3);
  }
 
  template<typename EigenDerived>
  Eigen::Vector3<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return quat() * v;
  }
 
  template<typename EigenDerived>
  Eigen::Matrix3<Scalar> dr_action(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    return -Base::matrix() * Base::hat(v);
  }
 
  SO2<Scalar> project_so2() const
  {
    using std::atan2;
 
    const auto q = quat();
    Scalar yaw =
      atan2(Scalar(2) * (q.w() * q.z() + q.x() * q.y()), Scalar(1) - Scalar(2) * (q.y() * q.y() + q.z() * q.z()));
    return SO2<Scalar>(yaw);
  }
};
 
// STORAGE TYPE TRAITS
 
// \cond
template<typename _Scalar>
class SO3;
// \endcond
 
// \cond
template<typename _Scalar>
struct liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = SO3Impl<_Scalar>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = SO3<NewScalar>;
};
// \endcond
 
template<typename _Scalar>
class SO3 : public SO3Base<SO3<_Scalar>>
{
  using Base = SO3Base<SO3<_Scalar>>;
 
  SMOOTH_GROUP_API(SO3);
 
public:
  template<typename Derived>
  explicit SO3(const Eigen::QuaternionBase<Derived> & quat) : m_coeffs(quat.normalized().coeffs())
  {
    if (m_coeffs(3) < 0) { m_coeffs *= Scalar(-1); }
  }
 
  static SO3 rot_x(const Scalar & angle)
  {
    using std::cos, std::sin;
 
    SO3 ret;
    ret.coeffs() << sin(angle / 2), Scalar(0), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }
 
  static SO3 rot_y(const Scalar & angle)
  {
    using std::cos, std::sin;
 
    SO3 ret;
    ret.coeffs() << Scalar(0), sin(angle / 2), Scalar(0), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }
 
  static SO3 rot_z(const Scalar & angle)
  {
    using std::cos, std::sin;
 
    SO3 ret;
    ret.coeffs() << Scalar(0), Scalar(0), sin(angle / 2), cos(angle / 2);
    if (ret.coeffs()(3) < 0) { ret.coeffs() *= Scalar(-1); }
    return ret;
  }
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{};
// \endcond
 
template<typename _Scalar>
class Map<SO3<_Scalar>> : public SO3Base<Map<SO3<_Scalar>>>
{
  using Base = SO3Base<Map<SO3<_Scalar>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<const SO3<_Scalar>>> : public liebase_info<SO3<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar>
class Map<const SO3<_Scalar>> : public SO3Base<Map<const SO3<_Scalar>>>
{
  using Base = SO3Base<Map<const SO3<_Scalar>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
using SO3f = SO3<float>;   
using SO3d = SO3<double>;  
 
SMOOTH_END_NAMESPACE
 
// Std format
#if __has_include(<format>)
#include <format>
#include <string>
 
template<class Scalar>
struct std::formatter<smooth::SO3<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::SO3<Scalar> & obj, std::format_context & ctx) const
  {
    const auto fmtSting = std::format("[{0}, {0}, {0}, {0}]", m_format);
    return std::vformat_to(
      ctx.out(), fmtSting, std::make_format_args(obj.quat().w(), obj.quat().x(), obj.quat().y(), obj.quat().z()));
  }
};
 
#endif