6 #ifndef __pinocchio_se3_base_hpp__
7 #define __pinocchio_se3_base_hpp__
29 template<
class Derived>
32 PINOCCHIO_SE3_TYPEDEF_TPL(Derived);
34 Derived & derived() {
return *
static_cast<Derived*
>(
this); }
35 const Derived& derived()
const {
return *
static_cast<const Derived*
>(
this); }
37 ConstAngularRef rotation()
const {
return derived().rotation_impl(); }
38 ConstLinearRef translation()
const {
return derived().translation_impl(); }
39 AngularRef rotation() {
return derived().rotation_impl(); }
40 LinearRef translation() {
return derived().translation_impl(); }
41 void rotation(
const AngularType & R) { derived().rotation_impl(R); }
42 void translation(
const LinearType & t) { derived().translation_impl(t); }
44 HomogeneousMatrixType toHomogeneousMatrix()
const
46 return derived().toHomogeneousMatrix_impl();
48 operator HomogeneousMatrixType()
const {
return toHomogeneousMatrix(); }
62 return derived().toActionMatrix_impl();
72 return derived().toActionMatrixInverse_impl();
75 ActionMatrixType toDualActionMatrix()
const
76 {
return derived().toDualActionMatrix_impl(); }
78 void disp(std::ostream & os)
const
80 static_cast<const Derived*
>(
this)->disp_impl(os);
83 typename SE3GroupAction<Derived>::ReturnType
84 operator*(
const Derived & m2)
const
85 {
return derived().__mult__(m2); }
89 typename SE3GroupAction<D>::ReturnType
90 act(
const D & d)
const
92 return derived().act_impl(d);
96 template<
typename D>
typename SE3GroupAction<D>::ReturnType
99 return derived().actInv_impl(d);
102 bool operator==(
const Derived & other)
const
103 {
return derived().isEqual(other); }
105 bool operator!=(
const Derived & other)
const
106 {
return !(*
this == other); }
108 bool isApprox(
const Derived & other,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
110 return derived().isApprox_impl(other, prec);
113 friend std::ostream & operator <<(std::ostream & os,
const SE3Base<Derived> & X)
124 return derived().isIdentity(prec);
130 bool isNormalized(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
132 return derived().isNormalized(prec);
140 derived().normalize();
148 derived().normalized();
155 #endif // ifndef __pinocchio_se3_base_hpp__