Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/motion-tpl.hpp	Lines:	44	47	93.6 %
Date:	2024-01-23 21:41:47	Branches:	21	44	47.7 %


//
// 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())
    {}

    template<typename M2>
    explicit MotionTpl(const MotionDense<M2> & clone)
    { linear() = clone.linear(); angular() = clone.angular(); }

    template<typename M2>
    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		204572	MotionTpl() : m_data() {}
60
61			template<typename V1,typename V2>
62		5093	MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
63		5093	{
64	✗✓	5093	assert(v.size() == 3);
65	✗✓	5093	assert(w.size() == 3);
66	✓✗✓✗	5093	linear() = v; angular() = w;
67		5093	}
68
69			template<typename V6>
70		104579	explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
71		104579	: m_data(v)
72			{
73			EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
74	✗✓	104579	assert(v.size() == 6);
75		104579	}
76
77			template<int O2>
78			explicit MotionTpl(const MotionTpl<Scalar,O2> & clone)
79			: m_data(clone.toVector())
80			{}
81
82			template<typename M2>
83		15991	explicit MotionTpl(const MotionDense<M2> & clone)
84	✓✗✓✗ ✓✗✓✗	15991	{ linear() = clone.linear(); angular() = clone.angular(); }
85
86			template<typename M2>
87		29331	explicit MotionTpl(const MotionBase<M2> & clone)
88		29331	{ *this = clone; }
89
90			// initializers
91	✓✗	6060	static MotionTpl Zero() { return MotionTpl(Vector6::Zero()); }
92	✓✗	90	static MotionTpl Random() { return MotionTpl(Vector6::Random()); }
93
94			inline PlainReturnType plain() const { return *this; }
95
96		139789	ToVectorConstReturnType toVector_impl() const { return m_data; }
97		19775	ToVectorReturnType toVector_impl() { return m_data; }
98
99			// Getters
100		651692	ConstAngularType angular_impl() const { return m_data.template segment<3> (ANGULAR); }
101		422175	ConstLinearType linear_impl() const { return m_data.template segment<3> (LINEAR); }
102		462625	AngularType angular_impl() { return m_data.template segment<3> (ANGULAR); }
103		292508	LinearType linear_impl() { return m_data.template segment<3> (LINEAR); }
104
105			template<typename V3>
106			void angular_impl(const Eigen::MatrixBase<V3> & w)
107			{
108			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
109			angular_impl()=w;
110			}
111			template<typename V3>
112		5	void linear_impl(const Eigen::MatrixBase<V3> & v)
113			{
114			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
115	✓✗	5	linear_impl()=v;
116		5	}
117
118			// Specific operators for MotionTpl and MotionRef
119			template<int O2>
120		902	MotionPlain __plus__(const MotionTpl<Scalar,O2> & v) const
121	✓✗	902	{ return MotionPlain(m_data+v.toVector()); }
122
123			template<typename Vector6ArgType>
124		1	MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
125	✓✗	1	{ return MotionPlain(m_data+v.toVector()); }
126
127			template<int O2>
128		5772	MotionPlain __minus__(const MotionTpl<Scalar,O2> & v) const
129	✓✗	5772	{ return MotionPlain(m_data-v.toVector()); }
130
131			template<typename Vector6ArgType>
132			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
133			{ return MotionPlain(m_data-v.toVector()); }
134
135			template<int O2>
136		86707	MotionTpl & __pequ__(const MotionTpl<Scalar,O2> & v)
137		86707	{ m_data += v.toVector(); return *this; }
138
139			template<typename Vector6ArgType>
140		1176	MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v)
141	✓✗	1176	{ m_data += v.toVector(); return *this; }
142
143			template<int O2>
144		87	MotionTpl & __mequ__(const MotionTpl<Scalar,O2> & v)
145		87	{ m_data -= v.toVector(); return *this; }
146
147			template<typename Vector6ArgType>
148			MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v)
149			{ m_data -= v.toVector(); return *this; }
150
151			template<typename OtherScalar>
152		25233	MotionPlain __mult__(const OtherScalar & alpha) const
153	✓✗	25233	{ return MotionPlain(alpha*m_data); }
154
155		2	MotionRef<Vector6> ref() { return MotionRef<Vector6>(m_data); }
156
157			/// \returns An expression of *this with the Scalar type casted to NewScalar.
158			template<typename NewScalar>
159		7	MotionTpl<NewScalar,Options> cast() const
160			{
161			typedef MotionTpl<NewScalar,Options> ReturnType;
162	✓✗✓✗ ✓✗✓✗	7	ReturnType res(linear().template cast<NewScalar>(),
163			angular().template cast<NewScalar>());
164		7	return res;
165			}
166
167			protected:
168			Vector6 m_data;
169
170			}; // class MotionTpl
171
172			} // namespace pinocchio
173
174			#endif // ifndef __pinocchio_motion_tpl_hpp__