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__