GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/motion-tpl.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 68 70 97.1%
Branches: 23 47 48.9%
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_spatial_motion_tpl_hpp__
7 #define __pinocchio_spatial_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 {
31 LINEAR = 0,
32 ANGULAR = 3,
33 Options = _Options
34 };
35
36 typedef MotionRef<Vector6> MotionRefType;
37 }; // traits MotionTpl
38
39 template<typename _Scalar, int _Options>
40 class MotionTpl : public MotionDense<MotionTpl<_Scalar, _Options>>
41 {
42 public:
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 typedef MotionDense<MotionTpl> Base;
45 MOTION_TYPEDEF_TPL(MotionTpl);
46 enum
47 {
48 Options = _Options
49 };
50
51 using Base::operator=;
52 using Base::angular;
53 using Base::linear;
54
55 using Base::__mequ__;
56 using Base::__minus__;
57 using Base::__mult__;
58 using Base::__opposite__;
59 using Base::__pequ__;
60 using Base::__plus__;
61
62 // Constructors
63 404165 MotionTpl()
64 404165 {
65 404165 }
66
67 template<typename V1, typename V2>
68 22281 MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
69 22281 {
70 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1);
71 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2);
72
3/5
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 11137 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
 22281 linear() = v;
73
3/5
✓ Branch 1 taken 158 times.
✓ Branch 2 taken 11137 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 158 times.
✗ Branch 5 not taken.
 22281 angular() = w;
74 22281 }
75
76 template<typename V6>
77 767887 explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
78 767887 : m_data(v)
79 {
80 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
81 767887 }
82
83 579104 MotionTpl(const MotionTpl & other)
84 579104 {
85
1/2
✓ Branch 1 taken 4543 times.
✗ Branch 2 not taken.
 579104 *this = other;
86 579104 }
87
88 template<typename S2, int O2>
89 1 explicit MotionTpl(const MotionTpl<S2, O2> & other)
90 1 {
91
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 *this = other.template cast<Scalar>();
92 1 }
93
94 template<int O2>
95 explicit MotionTpl(const MotionTpl<Scalar, O2> & clone)
96 : m_data(clone.toVector())
97 {
98 }
99
100 // Same explanation as converting constructor from MotionBase
101 template<
102 typename M2,
103 typename std::enable_if<!std::is_convertible<MotionDense<M2>, MotionTpl>::value, bool>::type =
104 true>
105 explicit MotionTpl(const MotionDense<M2> & clone)
106 {
107 linear() = clone.linear();
108 angular() = clone.angular();
109 }
110
111 // MotionBase implement a conversion function to PlainReturnType.
112 // Usually, PlainReturnType is defined as MotionTpl.
113 // In this case, this converting constructor is redundant and
114 // create a warning with -Wconversion
115 template<
116 typename M2,
117 typename std::enable_if<!std::is_convertible<MotionBase<M2>, MotionTpl>::value, bool>::type =
118 true>
119 ✗ explicit MotionTpl(const MotionBase<M2> & clone)
120 {
121 ✗ *this = clone;
122 }
123
124 ///
125 /// \brief Copy assignment operator.
126 ///
127 /// \param[in] other MotionTpl to copy
128 ///
129 790040 MotionTpl & operator=(const MotionTpl & clone) // Copy assignment operator
130 {
131 790040 m_data = clone.toVector();
132 790040 return *this;
133 }
134
135 // initializers
136 214594 static MotionTpl Zero()
137 {
138
1/2
✓ Branch 2 taken 214588 times.
✗ Branch 3 not taken.
 214594 return MotionTpl(Vector6::Zero());
139 }
140 114 static MotionTpl Random()
141 {
142
1/2
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
 114 return MotionTpl(Vector6::Random());
143 }
144
145 30888 inline PlainReturnType plain() const
146 {
147 30888 return *this;
148 }
149
150 1245512 ToVectorConstReturnType toVector_impl() const
151 {
152 1245512 return m_data;
153 }
154 200252 ToVectorReturnType toVector_impl()
155 {
156 200814 return m_data;
157 }
158
159 // Getters
160 1265209 ConstAngularType angular_impl() const
161 {
162 1265209 return m_data.template segment<3>(ANGULAR);
163 }
164 775130 ConstLinearType linear_impl() const
165 {
166 777745 return m_data.template segment<3>(LINEAR);
167 }
168 812827 AngularType angular_impl()
169 {
170 815258 return m_data.template segment<3>(ANGULAR);
171 }
172 571835 LinearType linear_impl()
173 {
174 573445 return m_data.template segment<3>(LINEAR);
175 }
176
177 template<typename V3>
178 1 void angular_impl(const Eigen::MatrixBase<V3> & w)
179 {
180 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
181
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 angular_impl() = w;
182 1 }
183 template<typename V3>
184 6 void linear_impl(const Eigen::MatrixBase<V3> & v)
185 {
186 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
187
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 linear_impl() = v;
188 6 }
189
190 // Specific operators for MotionTpl and MotionRef
191 template<int O2>
192 102003 MotionPlain __plus__(const MotionTpl<Scalar, O2> & v) const
193 {
194
1/2
✓ Branch 3 taken 101949 times.
✗ Branch 4 not taken.
 102003 return MotionPlain(m_data + v.toVector());
195 }
196
197 template<typename Vector6ArgType>
198 52 MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
199 {
200
3/5
✓ Branch 2 taken 51 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 52 return MotionPlain(m_data + v.toVector());
201 }
202
203 template<int O2>
204 47745 MotionPlain __minus__(const MotionTpl<Scalar, O2> & v) const
205 {
206
1/2
✓ Branch 3 taken 47745 times.
✗ Branch 4 not taken.
 47745 return MotionPlain(m_data - v.toVector());
207 }
208
209 template<typename Vector6ArgType>
210 MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
211 {
212 return MotionPlain(m_data - v.toVector());
213 }
214
215 template<int O2>
216 243677 MotionTpl & __pequ__(const MotionTpl<Scalar, O2> & v)
217 {
218 243677 m_data += v.toVector();
219 243677 return *this;
220 }
221
222 template<typename Vector6ArgType>
223 782 MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v)
224 {
225
1/2
✓ Branch 2 taken 775 times.
✗ Branch 3 not taken.
 782 m_data += v.toVector();
226 782 return *this;
227 }
228
229 template<int O2>
230 373 MotionTpl & __mequ__(const MotionTpl<Scalar, O2> & v)
231 {
232 373 m_data -= v.toVector();
233 373 return *this;
234 }
235
236 template<typename Vector6ArgType>
237 MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v)
238 {
239 m_data -= v.toVector();
240 return *this;
241 }
242
243 template<typename OtherScalar>
244 30294 MotionPlain __mult__(const OtherScalar & alpha) const
245 {
246
1/2
✓ Branch 2 taken 30294 times.
✗ Branch 3 not taken.
 30294 return MotionPlain(alpha * m_data);
247 }
248
249 2 MotionRef<Vector6> ref()
250 {
251 2 return MotionRef<Vector6>(m_data);
252 }
253
254 /// \returns An expression of *this with the Scalar type casted to NewScalar.
255 template<typename NewScalar>
256 29 MotionTpl<NewScalar, Options> cast() const
257 {
258 typedef MotionTpl<NewScalar, Options> ReturnType;
259
4/10
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
 29 ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>());
260 29 return res;
261 }
262
263 protected:
264 Vector6 m_data;
265
266 }; // class MotionTpl
267
268 } // namespace pinocchio
269
270 #endif // ifndef __pinocchio_spatial_motion_tpl_hpp__
271