18 #ifndef __se3_force_ref_hpp__ 19 #define __se3_force_ref_hpp__ 24 template<
typename Vector6ArgType>
27 typedef typename Vector6ArgType::Scalar Scalar;
32 Options = Vector6::Options
34 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
35 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
36 typedef Matrix6 ActionMatrixType;
37 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
38 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
39 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
40 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
42 typedef typename EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
43 typedef DataRefType ToVectorReturnType;
44 typedef typename EIGEN_REF_CONSTTYPE(Vector6ArgType) ConstDataRefType;
45 typedef ConstDataRefType ToVectorConstReturnType;
52 template<
typename Vector6ArgType>
53 struct SE3GroupAction< ForceRef<Vector6ArgType> >
58 template<
typename Vector6ArgType,
typename MotionDerived>
59 struct MotionAlgebraAction< ForceRef<Vector6ArgType>, MotionDerived >
65 template<
typename Vector6ArgType>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 using Base::operator=;
76 ForceRef(
const Eigen::MatrixBase<Vector6ArgType> & f_like)
77 : m_ref(const_cast<Vector6ArgType &>(f_like.derived()))
79 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
80 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
81 assert(f_like.size() == 6);
84 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
85 ToVectorReturnType toVector_impl() {
return m_ref; }
88 ConstAngularType angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
89 ConstLinearType linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
90 AngularType angular_impl() {
return m_ref.template segment<3> (ANGULAR); }
91 LinearType linear_impl() {
return m_ref.template segment<3> (LINEAR); }
94 void angular_impl(
const Eigen::MatrixBase<V3> & w)
96 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
100 template<
typename V3>
101 void linear_impl(
const Eigen::MatrixBase<V3> & v)
103 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
116 #endif // ifndef __se3_force_ref_hpp__ EIGEN_PLAIN_TYPE(Matrix3x) cross(const Eigen
Applies the cross product onto the columns of M.