Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/motion-ref.hpp	Lines:	28	28	100.0 %
Date:	2024-04-26 13:14:21	Branches:	4	8	50.0 %


//
// Copyright (c) 2017-2019 CNRS INRIA
//

#ifndef __pinocchio_motion_ref_hpp__
#define __pinocchio_motion_ref_hpp__

namespace pinocchio
{

  template<typename Vector6ArgType>
  struct traits< MotionRef<Vector6ArgType> >
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    enum {
      LINEAR = 0,
      ANGULAR = 3,
      Options = Vector6::Options
    };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef MotionRef<Vector6ArgType> MotionRefType;

  }; // traits MotionRef

  template<typename Vector6ArgType>
  struct SE3GroupAction< MotionRef<Vector6ArgType> >
  {
    typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
  };

  template<typename Vector6ArgType, typename MotionDerived>
  struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
  {
    typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
  };

  namespace internal
  {
    template<typename Vector6ArgType, typename Scalar>
    struct RHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
    {
      typedef typename pinocchio::traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
    };

    template<typename Vector6ArgType, typename Scalar>
    struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
    {
      typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
    };
  }

  template<typename Vector6ArgType>
  class MotionRef : public MotionDense< MotionRef<Vector6ArgType> >
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef MotionDense<MotionRef> Base;
    typedef typename traits<MotionRef>::DataRefType DataRefType;
    MOTION_TYPEDEF_TPL(MotionRef);

    using Base::operator=;
    using Base::linear;
    using Base::angular;

    using Base::__plus__;
    using Base::__opposite__;
    using Base::__minus__;
    using Base::__pequ__;
    using Base::__mequ__;
    using Base::__mult__;

    /// \brief Default constructor from a 6 dimensional vector.
    MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
    : m_ref(v_like)
    {
      EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
                          YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(v_like.size() == 6);
    }

    /// \brief Copy constructor from another MotionRef.
    MotionRef(const MotionRef & other)
    : m_ref(other.m_ref)
    {}

    ToVectorConstReturnType toVector_impl() const { return m_ref; }
    ToVectorReturnType toVector_impl() { return m_ref; }

    // Getters
    ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
    ConstLinearType linear_impl()  const { return ConstLinearType(m_ref.derived(),LINEAR); }
    AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
    LinearType linear_impl()  { return m_ref.template segment<3> (LINEAR); }

    template<typename V3>
    void angular_impl(const Eigen::MatrixBase<V3> & w)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
      angular_impl()=w;
    }

    template<typename V3>
    void linear_impl(const Eigen::MatrixBase<V3> & v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
      linear_impl()=v;
    }

    // Specific operators for MotionTpl and MotionRef
    template<typename S1, int O1>
    MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
    { return MotionPlain(m_ref+v.toVector()); }

    template<typename Vector6Like>
    MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
    { return MotionPlain(m_ref+v.toVector()); }

    template<typename S1, int O1>
    MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
    { return MotionPlain(m_ref-v.toVector()); }

    template<typename Vector6Like>
    MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
    { return MotionPlain(m_ref-v.toVector()); }

    template<typename S1, int O1>
    MotionRef & __pequ__(const MotionTpl<S1,O1> & v)
    { m_ref += v.toVector(); return *this; }

    template<typename Vector6Like>
    MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
    { m_ref += v.toVector(); return *this; }

    template<typename S1, int O1>
    MotionRef & __mequ__(const MotionTpl<S1,O1> & v)
    { m_ref -= v.toVector(); return *this; }

    template<typename Vector6Like>
    MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
    { m_ref -= v.toVector(); return *this; }

    template<typename OtherScalar>
    MotionPlain __mult__(const OtherScalar & alpha) const
    { return MotionPlain(alpha*m_ref); }

    MotionRef & ref() { return *this; }

    inline PlainReturnType plain() const { return PlainReturnType(m_ref); }

  protected:
    DataRefType m_ref;

  }; // class MotionRef<Vector6Like>

  template<typename Vector6ArgType>
  struct traits< MotionRef<const Vector6ArgType> >
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    enum {
      LINEAR = 0,
      ANGULAR = 3,
      Options = Vector6::Options
    };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ConstLinearType LinearType;
    typedef ConstAngularType AngularType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef ConstDataRefType DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef MotionRef<const Vector6ArgType> MotionRefType;

  }; // traits MotionRef<const Vector6ArgType>

  template<typename Vector6ArgType>
  class MotionRef<const Vector6ArgType>
  : public MotionDense< MotionRef<const Vector6ArgType> >
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef MotionDense<MotionRef> Base;
    typedef typename traits<MotionRef>::DataRefType DataRefType;
    MOTION_TYPEDEF_TPL(MotionRef);

    using Base::operator=;
    using Base::linear;
    using Base::angular;

    using Base::__plus__;
    using Base::__opposite__;
    using Base::__minus__;
    using Base::__mult__;

    MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
    : m_ref(v_like)
    {
      EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
                          YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(v_like.size() == 6);
    }

    ToVectorConstReturnType toVector_impl() const { return m_ref; }

    // Getters
    ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
    ConstLinearType linear_impl()  const { return ConstLinearType(m_ref.derived(),LINEAR); }

    // Specific operators for MotionTpl and MotionRef
    template<typename S1, int O1>
    MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
    { return MotionPlain(m_ref+v.toVector()); }

    template<typename Vector6Like>
    MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
    { return MotionPlain(m_ref+v.toVector()); }

    template<typename S1, int O1>
    MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
    { return MotionPlain(m_ref-v.toVector()); }

    template<typename Vector6Like>
    MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
    { return MotionPlain(m_ref-v.toVector()); }

    template<typename OtherScalar>
    MotionPlain __mult__(const OtherScalar & alpha) const
    { return MotionPlain(alpha*m_ref); }

    const MotionRef & ref() const { return *this; }

    inline PlainReturnType plain() const { return PlainReturnType(m_ref); }

  protected:
    DataRefType m_ref;

  }; // class MotionRef<const Vector6Like>

} // namespace pinocchio

#endif // ifndef __pinocchio_motion_ref_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_motion_ref_hpp__
6			#define __pinocchio_motion_ref_hpp__
7
8			namespace pinocchio
9			{
10
11			template<typename Vector6ArgType>
12			struct traits< MotionRef<Vector6ArgType> >
13			{
14			typedef typename Vector6ArgType::Scalar Scalar;
15			typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
16			enum {
17			LINEAR = 0,
18			ANGULAR = 3,
19			Options = Vector6::Options
20			};
21			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
22			typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
23			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
24			typedef Matrix6 ActionMatrixType;
25			typedef Matrix4 HomogeneousMatrixType;
26			typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
27			typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
28			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
29			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
30			typedef MotionTpl<Scalar,Options> MotionPlain;
31			typedef MotionPlain PlainReturnType;
32			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
33			typedef DataRefType ToVectorReturnType;
34			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
35			typedef ConstDataRefType ToVectorConstReturnType;
36			typedef MotionRef<Vector6ArgType> MotionRefType;
37
38			}; // traits MotionRef
39
40			template<typename Vector6ArgType>
41			struct SE3GroupAction< MotionRef<Vector6ArgType> >
42			{
43			typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
44			};
45
46			template<typename Vector6ArgType, typename MotionDerived>
47			struct MotionAlgebraAction< MotionRef<Vector6ArgType>, MotionDerived >
48			{
49			typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
50			};
51
52			namespace internal
53			{
54			template<typename Vector6ArgType, typename Scalar>
55			struct RHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
56			{
57			typedef typename pinocchio::traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
58			};
59
60			template<typename Vector6ArgType, typename Scalar>
61			struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
62			{
63			typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
64			};
65			}
66
67			template<typename Vector6ArgType>
68			class MotionRef : public MotionDense< MotionRef<Vector6ArgType> >
69			{
70			public:
71			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72			typedef MotionDense<MotionRef> Base;
73			typedef typename traits<MotionRef>::DataRefType DataRefType;
74			MOTION_TYPEDEF_TPL(MotionRef);
75
76			using Base::operator=;
77			using Base::linear;
78			using Base::angular;
79
80			using Base::__plus__;
81			using Base::__opposite__;
82			using Base::__minus__;
83			using Base::__pequ__;
84			using Base::__mequ__;
85			using Base::__mult__;
86
87			/// \brief Default constructor from a 6 dimensional vector.
88		185904	MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
89		185904	: m_ref(v_like)
90			{
91			EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
92			YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
93	✗✓	185904	assert(v_like.size() == 6);
94		185904	}
95
96			/// \brief Copy constructor from another MotionRef.
97			MotionRef(const MotionRef & other)
98			: m_ref(other.m_ref)
99			{}
100
101		132041	ToVectorConstReturnType toVector_impl() const { return m_ref; }
102		837	ToVectorReturnType toVector_impl() { return m_ref; }
103
104			// Getters
105		4394	ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
106		1324	ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
107		51775	AngularType angular_impl() { return m_ref.template segment<3> (ANGULAR); }
108		40872	LinearType linear_impl() { return m_ref.template segment<3> (LINEAR); }
109
110			template<typename V3>
111			void angular_impl(const Eigen::MatrixBase<V3> & w)
112			{
113			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
114			angular_impl()=w;
115			}
116
117			template<typename V3>
118			void linear_impl(const Eigen::MatrixBase<V3> & v)
119			{
120			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
121			linear_impl()=v;
122			}
123
124			// Specific operators for MotionTpl and MotionRef
125			template<typename S1, int O1>
126			MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
127			{ return MotionPlain(m_ref+v.toVector()); }
128
129			template<typename Vector6Like>
130			MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
131			{ return MotionPlain(m_ref+v.toVector()); }
132
133			template<typename S1, int O1>
134			MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
135			{ return MotionPlain(m_ref-v.toVector()); }
136
137			template<typename Vector6Like>
138			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
139			{ return MotionPlain(m_ref-v.toVector()); }
140
141			template<typename S1, int O1>
142		11378	MotionRef & __pequ__(const MotionTpl<S1,O1> & v)
143		11378	{ m_ref += v.toVector(); return *this; }
144
145			template<typename Vector6Like>
146			MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
147			{ m_ref += v.toVector(); return *this; }
148
149			template<typename S1, int O1>
150		60	MotionRef & __mequ__(const MotionTpl<S1,O1> & v)
151		60	{ m_ref -= v.toVector(); return *this; }
152
153			template<typename Vector6Like>
154			MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
155			{ m_ref -= v.toVector(); return *this; }
156
157			template<typename OtherScalar>
158		1	MotionPlain __mult__(const OtherScalar & alpha) const
159	✓✗	1	{ return MotionPlain(alpha*m_ref); }
160
161		2	MotionRef & ref() { return *this; }
162
163		385	inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
164
165			protected:
166			DataRefType m_ref;
167
168			}; // class MotionRef<Vector6Like>
169
170			template<typename Vector6ArgType>
171			struct traits< MotionRef<const Vector6ArgType> >
172			{
173			typedef typename Vector6ArgType::Scalar Scalar;
174			typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
175			enum {
176			LINEAR = 0,
177			ANGULAR = 3,
178			Options = Vector6::Options
179			};
180			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
181			typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
182			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
183			typedef Matrix6 ActionMatrixType;
184			typedef Matrix4 HomogeneousMatrixType;
185			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
186			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
187			typedef ConstLinearType LinearType;
188			typedef ConstAngularType AngularType;
189			typedef MotionTpl<Scalar,Options> MotionPlain;
190			typedef MotionPlain PlainReturnType;
191			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
192			typedef ConstDataRefType ToVectorConstReturnType;
193			typedef ConstDataRefType DataRefType;
194			typedef DataRefType ToVectorReturnType;
195			typedef MotionRef<const Vector6ArgType> MotionRefType;
196
197			}; // traits MotionRef<const Vector6ArgType>
198
199			template<typename Vector6ArgType>
200			class MotionRef<const Vector6ArgType>
201			: public MotionDense< MotionRef<const Vector6ArgType> >
202			{
203			public:
204			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205			typedef MotionDense<MotionRef> Base;
206			typedef typename traits<MotionRef>::DataRefType DataRefType;
207			MOTION_TYPEDEF_TPL(MotionRef);
208
209			using Base::operator=;
210			using Base::linear;
211			using Base::angular;
212
213			using Base::__plus__;
214			using Base::__opposite__;
215			using Base::__minus__;
216			using Base::__mult__;
217
218		186983	MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
219		186983	: m_ref(v_like)
220			{
221			EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
222			YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
223	✗✓	186983	assert(v_like.size() == 6);
224		186983	}
225
226		1176	ToVectorConstReturnType toVector_impl() const { return m_ref; }
227
228			// Getters
229		353174	ConstAngularType angular_impl() const { return ConstAngularType(m_ref.derived(),ANGULAR); }
230		92854	ConstLinearType linear_impl() const { return ConstLinearType(m_ref.derived(),LINEAR); }
231
232			// Specific operators for MotionTpl and MotionRef
233			template<typename S1, int O1>
234		887	MotionPlain __plus__(const MotionTpl<S1,O1> & v) const
235	✓✗	887	{ return MotionPlain(m_ref+v.toVector()); }
236
237			template<typename Vector6Like>
238			MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
239			{ return MotionPlain(m_ref+v.toVector()); }
240
241			template<typename S1, int O1>
242			MotionPlain __minus__(const MotionTpl<S1,O1> & v) const
243			{ return MotionPlain(m_ref-v.toVector()); }
244
245			template<typename Vector6Like>
246			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
247			{ return MotionPlain(m_ref-v.toVector()); }
248
249			template<typename OtherScalar>
250			MotionPlain __mult__(const OtherScalar & alpha) const
251			{ return MotionPlain(alpha*m_ref); }
252
253			const MotionRef & ref() const { return *this; }
254
255		2	inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
256
257			protected:
258			DataRefType m_ref;
259
260			}; // class MotionRef<const Vector6Like>
261
262			} // namespace pinocchio
263
264			#endif // ifndef __pinocchio_motion_ref_hpp__