Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/motion-tpl.hpp	Lines:	41	44	93.2 %
Date:	2024-04-26 13:14:21	Branches:	17	36	47.2 %


//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_motion_tpl_hpp__
#define __pinocchio_motion_tpl_hpp__

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  struct traits< MotionTpl<_Scalar, _Options> >
  {
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
    typedef Eigen::Matrix<Scalar,4,4,_Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef MotionTpl<Scalar,_Options> MotionPlain;
    typedef const MotionPlain & PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3,
      Options = _Options
    };

    typedef MotionRef<Vector6> MotionRefType;
  }; // traits MotionTpl

  template<typename _Scalar, int _Options>
  class MotionTpl : public MotionDense< MotionTpl< _Scalar, _Options > >
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef MotionDense<MotionTpl> Base;
    MOTION_TYPEDEF_TPL(MotionTpl);
    enum { Options = _Options };

    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__;

    // Constructors
    MotionTpl() : m_data() {}

    template<typename V1,typename V2>
    MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
    {
      assert(v.size() == 3);
      assert(w.size() == 3);
      linear() = v; angular() = w;
    }

    template<typename V6>
    explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
    : m_data(v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
      assert(v.size() == 6);
    }

    template<int O2>
    explicit MotionTpl(const MotionTpl<Scalar,O2> & clone)
    : m_data(clone.toVector())
    {}

    // Same explanation as converting constructor from MotionBase
    template <typename M2,
              typename std::enable_if<
                !std::is_convertible<MotionDense<M2>, MotionTpl>::value,
                bool>::type = true>
    explicit MotionTpl(const MotionDense<M2> & clone)
    { linear() = clone.linear(); angular() = clone.angular(); }

    // MotionBase implement a conversion function to PlainReturnType.
    // Usually, PlainReturnType is defined as MotionTpl.
    // In this case, this converting constructor is redundant and
    // create a warning with -Wconversion
    template <typename M2,
              typename std::enable_if<
                !std::is_convertible<MotionBase<M2>, MotionTpl>::value,
                bool>::type = true>
    explicit MotionTpl(const MotionBase<M2> & clone)
    { *this = clone; }

    // initializers
    static MotionTpl Zero()   { return MotionTpl(Vector6::Zero());   }
    static MotionTpl Random() { return MotionTpl(Vector6::Random()); }

    inline PlainReturnType plain() const { return *this; }

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

    // Getters
    ConstAngularType angular_impl() const { return m_data.template segment<3> (ANGULAR); }
    ConstLinearType linear_impl()  const { return m_data.template segment<3> (LINEAR); }
    AngularType angular_impl() { return m_data.template segment<3> (ANGULAR); }
    LinearType linear_impl()  { return m_data.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<int O2>
    MotionPlain __plus__(const MotionTpl<Scalar,O2> & v) const
    { return MotionPlain(m_data+v.toVector()); }

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

    template<int O2>
    MotionPlain __minus__(const MotionTpl<Scalar,O2> & v) const
    { return MotionPlain(m_data-v.toVector()); }

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

    template<int O2>
    MotionTpl & __pequ__(const MotionTpl<Scalar,O2> & v)
    { m_data += v.toVector(); return *this; }

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

    template<int O2>
    MotionTpl & __mequ__(const MotionTpl<Scalar,O2> & v)
    { m_data -= v.toVector(); return *this; }

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

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

    MotionRef<Vector6> ref() { return MotionRef<Vector6>(m_data); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    MotionTpl<NewScalar,Options> cast() const
    {
      typedef MotionTpl<NewScalar,Options> ReturnType;
      ReturnType res(linear().template cast<NewScalar>(),
                     angular().template cast<NewScalar>());
      return res;
    }

  protected:
    Vector6 m_data;

  }; // class MotionTpl

} // namespace pinocchio

#endif // ifndef __pinocchio_motion_tpl_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2018 CNRS
3			// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#ifndef __pinocchio_motion_tpl_hpp__
7			#define __pinocchio_motion_tpl_hpp__
8
9			namespace pinocchio
10			{
11			template<typename _Scalar, int _Options>
12			struct traits< MotionTpl<_Scalar, _Options> >
13			{
14			typedef _Scalar Scalar;
15			typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
16			typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
17			typedef Eigen::Matrix<Scalar,4,4,_Options> Matrix4;
18			typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
19			typedef Matrix6 ActionMatrixType;
20			typedef Matrix4 HomogeneousMatrixType;
21			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
22			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
23			typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
24			typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
25			typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
26			typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
27			typedef MotionTpl<Scalar,_Options> MotionPlain;
28			typedef const MotionPlain & PlainReturnType;
29			enum {
30			LINEAR = 0,
31			ANGULAR = 3,
32			Options = _Options
33			};
34
35			typedef MotionRef<Vector6> MotionRefType;
36			}; // traits MotionTpl
37
38			template<typename _Scalar, int _Options>
39			class MotionTpl : public MotionDense< MotionTpl< _Scalar, _Options > >
40			{
41			public:
42			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43			typedef MotionDense<MotionTpl> Base;
44			MOTION_TYPEDEF_TPL(MotionTpl);
45			enum { Options = _Options };
46
47			using Base::operator=;
48			using Base::linear;
49			using Base::angular;
50
51			using Base::__plus__;
52			using Base::__opposite__;
53			using Base::__minus__;
54			using Base::__pequ__;
55			using Base::__mequ__;
56			using Base::__mult__;
57
58			// Constructors
59		214437	MotionTpl() : m_data() {}
60
61			template<typename V1,typename V2>
62		13457	MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
63		13457	{
64	✗✓	13457	assert(v.size() == 3);
65	✗✓	13457	assert(w.size() == 3);
66	✓✗✓✗	13457	linear() = v; angular() = w;
67		13457	}
68
69			template<typename V6>
70		105673	explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
71		105673	: m_data(v)
72			{
73			EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
74	✗✓	105673	assert(v.size() == 6);
75		105673	}
76
77			template<int O2>
78			explicit MotionTpl(const MotionTpl<Scalar,O2> & clone)
79			: m_data(clone.toVector())
80			{}
81
82			// Same explanation as converting constructor from MotionBase
83			template <typename M2,
84			typename std::enable_if<
85			!std::is_convertible<MotionDense<M2>, MotionTpl>::value,
86			bool>::type = true>
87			explicit MotionTpl(const MotionDense<M2> & clone)
88			{ linear() = clone.linear(); angular() = clone.angular(); }
89
90			// MotionBase implement a conversion function to PlainReturnType.
91			// Usually, PlainReturnType is defined as MotionTpl.
92			// In this case, this converting constructor is redundant and
93			// create a warning with -Wconversion
94			template <typename M2,
95			typename std::enable_if<
96			!std::is_convertible<MotionBase<M2>, MotionTpl>::value,
97			bool>::type = true>
98			explicit MotionTpl(const MotionBase<M2> & clone)
99			{ *this = clone; }
100
101			// initializers
102	✓✗	6690	static MotionTpl Zero() { return MotionTpl(Vector6::Zero()); }
103	✓✗	90	static MotionTpl Random() { return MotionTpl(Vector6::Random()); }
104
105		15990	inline PlainReturnType plain() const { return *this; }
106
107		139789	ToVectorConstReturnType toVector_impl() const { return m_data; }
108		78965	ToVectorReturnType toVector_impl() { return m_data; }
109
110			// Getters
111		635702	ConstAngularType angular_impl() const { return m_data.template segment<3> (ANGULAR); }
112		406185	ConstLinearType linear_impl() const { return m_data.template segment<3> (LINEAR); }
113		422587	AngularType angular_impl() { return m_data.template segment<3> (ANGULAR); }
114		259844	LinearType linear_impl() { return m_data.template segment<3> (LINEAR); }
115
116			template<typename V3>
117			void angular_impl(const Eigen::MatrixBase<V3> & w)
118			{
119			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
120			angular_impl()=w;
121			}
122			template<typename V3>
123		5	void linear_impl(const Eigen::MatrixBase<V3> & v)
124			{
125			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
126	✓✗	5	linear_impl()=v;
127		5	}
128
129			// Specific operators for MotionTpl and MotionRef
130			template<int O2>
131		902	MotionPlain __plus__(const MotionTpl<Scalar,O2> & v) const
132	✓✗	902	{ return MotionPlain(m_data+v.toVector()); }
133
134			template<typename Vector6ArgType>
135		1	MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
136	✓✗	1	{ return MotionPlain(m_data+v.toVector()); }
137
138			template<int O2>
139		5772	MotionPlain __minus__(const MotionTpl<Scalar,O2> & v) const
140	✓✗	5772	{ return MotionPlain(m_data-v.toVector()); }
141
142			template<typename Vector6ArgType>
143			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
144			{ return MotionPlain(m_data-v.toVector()); }
145
146			template<int O2>
147		86707	MotionTpl & __pequ__(const MotionTpl<Scalar,O2> & v)
148		86707	{ m_data += v.toVector(); return *this; }
149
150			template<typename Vector6ArgType>
151		1176	MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v)
152	✓✗	1176	{ m_data += v.toVector(); return *this; }
153
154			template<int O2>
155		87	MotionTpl & __mequ__(const MotionTpl<Scalar,O2> & v)
156		87	{ m_data -= v.toVector(); return *this; }
157
158			template<typename Vector6ArgType>
159			MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v)
160			{ m_data -= v.toVector(); return *this; }
161
162			template<typename OtherScalar>
163		25233	MotionPlain __mult__(const OtherScalar & alpha) const
164	✓✗	25233	{ return MotionPlain(alpha*m_data); }
165
166		2	MotionRef<Vector6> ref() { return MotionRef<Vector6>(m_data); }
167
168			/// \returns An expression of *this with the Scalar type casted to NewScalar.
169			template<typename NewScalar>
170		7	MotionTpl<NewScalar,Options> cast() const
171			{
172			typedef MotionTpl<NewScalar,Options> ReturnType;
173	✓✗✓✗ ✓✗✓✗	7	ReturnType res(linear().template cast<NewScalar>(),
174			angular().template cast<NewScalar>());
175		7	return res;
176			}
177
178			protected:
179			Vector6 m_data;
180
181			}; // class MotionTpl
182
183			} // namespace pinocchio
184
185			#endif // ifndef __pinocchio_motion_tpl_hpp__