Program Listing for File se3-base.hpp
↰ Return to documentation for file (include/pinocchio/spatial/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__