galilei.hpp

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "detail/galilei.hpp"
#include "detail/macro.hpp"
#include "lie_group_base.hpp"
#include "so3.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<typename _Derived>
class GalileiBase : public LieGroupBase<_Derived>
{
  using Base = LieGroupBase<_Derived>;
 
protected:
  GalileiBase() = default;
 
public:
  SMOOTH_INHERIT_TYPEDEFS;
 
  Map<SO3<Scalar>> so3()
    requires is_mutable
  {
    return Map<SO3<Scalar>>(static_cast<_Derived &>(*this).data() + 7);
  }
 
  Map<const SO3<Scalar>> so3() const { return Map<const SO3<Scalar>>(static_cast<const _Derived &>(*this).data() + 7); }
 
  Eigen::Map<Eigen::Vector3<Scalar>> r3_v()
    requires is_mutable
  {
    return Eigen::Map<Eigen::Vector3<Scalar>>(static_cast<_Derived &>(*this).data() + 0);
  }
 
  Eigen::Map<const Eigen::Vector3<Scalar>> r3_v() const
  {
    return Eigen::Map<const Eigen::Vector3<Scalar>>(static_cast<const _Derived &>(*this).data() + 0);
  }
 
  Eigen::Map<Eigen::Vector3<Scalar>> r3_p()
    requires is_mutable
  {
    return Eigen::Map<Eigen::Vector3<Scalar>>(static_cast<_Derived &>(*this).data() + 3);
  }
 
  Eigen::Map<const Eigen::Vector3<Scalar>> r3_p() const
  {
    return Eigen::Map<const Eigen::Vector3<Scalar>>(static_cast<const _Derived &>(*this).data() + 3);
  }
 
  Eigen::Map<Eigen::Vector<Scalar, 1>> r1_t()
    requires is_mutable
  {
    return Eigen::Map<Eigen::Vector<Scalar, 1>>(static_cast<_Derived &>(*this).data() + 6);
  }
 
  Eigen::Map<const Eigen::Vector<Scalar, 1>> r1_t() const
  {
    return Eigen::Map<const Eigen::Vector<Scalar, 1>>(static_cast<const _Derived &>(*this).data() + 6);
  }
 
  template<typename EigenDerived>
  Eigen::Vector4<Scalar> operator*(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    Eigen::Vector4<Scalar> ret;
    ret.template segment<3>(0) = so3() * v.template segment<3>(0) + r3_v() * v(3) + r3_p();
    ret(3)                     = v(3) + r1_t().x();
    return ret;
  }
 
  template<typename EigenDerived>
  Eigen::Matrix<Scalar, 4, 10> dr_action(const Eigen::MatrixBase<EigenDerived> & v) const
  {
    Eigen::Matrix<Scalar, 4, 10> ret = Eigen::Matrix<Scalar, 4, 10>::Zero();
 
    // v transformation
    ret.template block<3, 3>(0, 0) = v(3) * so3().matrix();
    ret.template block<3, 3>(0, 3) = so3().matrix();
    ret.template block<3, 1>(0, 6) = r3_v();
    ret.template block<3, 3>(0, 7) = so3().dr_action(v.template segment<3>(0));
 
    // t transformation
    ret(3, 6) = 1;
 
    return ret;
  }
};
 
// \cond
template<typename _Scalar>
class Galilei;
// \endcond
 
// \cond
template<typename _Scalar>
struct liebase_info<Galilei<_Scalar>>
{
  static constexpr bool is_mutable = true;
 
  using Impl   = GalileiImpl<_Scalar>;
  using Scalar = _Scalar;
 
  template<typename NewScalar>
  using PlainObject = Galilei<NewScalar>;
};
// \endcond
 
template<typename _Scalar>
class Galilei : public GalileiBase<Galilei<_Scalar>>
{
  using Base = GalileiBase<Galilei<_Scalar>>;
 
  SMOOTH_GROUP_API(Galilei);
 
public:
  template<typename SO3Derived, typename T1, typename T2>
  Galilei(
    const SO3Base<SO3Derived> & so3,
    const Eigen::MatrixBase<T1> & r3_v,
    const Eigen::MatrixBase<T2> & r3_p,
    double r1_t = 0)
  {
    Base::so3()      = static_cast<const SO3Derived &>(so3);
    Base::r3_v()     = static_cast<const T1 &>(r3_v);
    Base::r3_p()     = static_cast<const T2 &>(r3_p);
    Base::r1_t().x() = r1_t;
  }
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<Galilei<_Scalar>>> : public liebase_info<Galilei<_Scalar>>
{};
// \endcond
 
template<typename _Scalar>
class Map<Galilei<_Scalar>> : public GalileiBase<Map<Galilei<_Scalar>>>
{
  using Base = GalileiBase<Map<Galilei<_Scalar>>>;
 
  SMOOTH_MAP_API();
};
 
// \cond
template<typename _Scalar>
struct liebase_info<Map<const Galilei<_Scalar>>> : public liebase_info<Galilei<_Scalar>>
{
  static constexpr bool is_mutable = false;
};
// \endcond
 
template<typename _Scalar>
class Map<const Galilei<_Scalar>> : public GalileiBase<Map<const Galilei<_Scalar>>>
{
  using Base = GalileiBase<Map<const Galilei<_Scalar>>>;
 
  SMOOTH_CONST_MAP_API();
};
 
using Galileif = Galilei<float>;   
using Galileid = Galilei<double>;  
 
SMOOTH_END_NAMESPACE