se3-base.hpp

//
// Copyright (c) 2015-2021 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
 
#ifndef __pinocchio_spatial_se3_base_hpp__
#define __pinocchio_spatial_se3_base_hpp__
 
namespace pinocchio
{
  template<class Derived>
  struct SE3Base : NumericalBase<Derived>
  {
    PINOCCHIO_SE3_TYPEDEF_TPL(Derived);
 
    Derived & derived()
    {
      return *static_cast<Derived *>(this);
    }
    const Derived & derived() const
    {
      return *static_cast<const Derived *>(this);
    }
 
    Derived & const_cast_derived() const
    {
      return *const_cast<Derived *>(&derived());
    }
 
    ConstAngularRef rotation() const
    {
      return derived().rotation_impl();
    }
    ConstLinearRef translation() const
    {
      return derived().translation_impl();
    }
    AngularRef rotation()
    {
      return derived().rotation_impl();
    }
    LinearRef translation()
    {
      return derived().translation_impl();
    }
    void rotation(const AngularType & R)
    {
      derived().rotation_impl(R);
    }
    void translation(const LinearType & t)
    {
      derived().translation_impl(t);
    }
 
    HomogeneousMatrixType toHomogeneousMatrix() const
    {
      return derived().toHomogeneousMatrix_impl();
    }
    operator HomogeneousMatrixType() const
    {
      return toHomogeneousMatrix();
    }
 
    ActionMatrixType toActionMatrix() const
    {
      return derived().toActionMatrix_impl();
    }
    operator ActionMatrixType() const
    {
      return toActionMatrix();
    }
 
    template<typename Matrix6Like>
    void toActionMatrix(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const
    {
      derived().toActionMatrix_impl(action_matrix);
    }
 
    ActionMatrixType toActionMatrixInverse() const
    {
      return derived().toActionMatrixInverse_impl();
    }
 
    template<typename Matrix6Like>
    void toActionMatrixInverse(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const
    {
      derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived());
    }
 
    ActionMatrixType toDualActionMatrix() const
    {
      return derived().toDualActionMatrix_impl();
    }
 
    void disp(std::ostream & os) const
    {
      static_cast<const Derived *>(this)->disp_impl(os);
    }
 
    template<typename Matrix6Like>
    void toDualActionMatrix(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const
    {
      derived().toDualActionMatrix_impl(dual_action_matrix);
    }
 
    typename SE3GroupAction<Derived>::ReturnType operator*(const Derived & m2) const
    {
      return derived().__mult__(m2);
    }
 
    template<typename D>
    typename SE3GroupAction<D>::ReturnType act(const D & d) const
    {
      return derived().act_impl(d);
    }
 
    template<typename D>
    typename SE3GroupAction<D>::ReturnType actInv(const D & d) const
    {
      return derived().actInv_impl(d);
    }
 
    bool operator==(const Derived & other) const
    {
      return derived().isEqual(other);
    }
 
    bool operator!=(const Derived & other) const
    {
      return !(*this == other);
    }
 
    bool isApprox(
      const Derived & other,
      const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return derived().isApprox_impl(other, prec);
    }
 
    friend std::ostream & operator<<(std::ostream & os, const SE3Base<Derived> & X)
    {
      X.disp(os);
      return os;
    }
 
    bool isIdentity(
      const typename traits<Derived>::Scalar & prec =
        Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const
    {
      return derived().isIdentity(prec);
    }
 
    bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return derived().isNormalized(prec);
    }
 
    void normalize()
    {
      derived().normalize();
    }
 
    PlainType normalized() const
    {
      derived().normalized();
    }
 
  }; // struct SE3Base
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_spatial_se3_base_hpp__