GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/motion-tpl.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 51 64 79.7%
Branches: 9 37 24.3%
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 198492 MotionTpl()
64 198492 {
65 198492 }
66
67 template<typename V1, typename V2>
68 6739 MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
69 6739 {
70 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1);
71 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2);
72
1/2
✓ Branch 2 taken 3522 times.
✗ Branch 3 not taken.
 6739 linear() = v;
73
1/2
✓ Branch 2 taken 3522 times.
✗ Branch 3 not taken.
 6739 angular() = w;
74 6739 }
75
76 template<typename V6>
77 127554 explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
78 127554 : m_data(v)
79 {
80 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
81 127554 }
82
83 127191 MotionTpl(const MotionTpl & other)
84 127191 {
85 127191 *this = other;
86 127191 }
87
88 template<typename S2, int O2>
89 ✗ explicit MotionTpl(const MotionTpl<S2, O2> & other)
90 {
91 ✗ *this = other.template cast<Scalar>();
92 }
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 258458 MotionTpl & operator=(const MotionTpl & clone) // Copy assignment operator
130 {
131 258458 m_data = clone.toVector();
132 258458 return *this;
133 }
134
135 // initializers
136 7047 static MotionTpl Zero()
137 {
138
1/2
✓ Branch 2 taken 7047 times.
✗ Branch 3 not taken.
 7047 return MotionTpl(Vector6::Zero());
139 }
140 ✗ static MotionTpl Random()
141 {
142 ✗ return MotionTpl(Vector6::Random());
143 }
144
145 30368 inline PlainReturnType plain() const
146 {
147 30368 return *this;
148 }
149
150 489276 ToVectorConstReturnType toVector_impl() const
151 {
152 489276 return m_data;
153 }
154 15429 ToVectorReturnType toVector_impl()
155 {
156 15429 return m_data;
157 }
158
159 // Getters
160 650092 ConstAngularType angular_impl() const
161 {
162 650092 return m_data.template segment<3>(ANGULAR);
163 }
164 372403 ConstLinearType linear_impl() const
165 {
166 372403 return m_data.template segment<3>(LINEAR);
167 }
168 375402 AngularType angular_impl()
169 {
170 375402 return m_data.template segment<3>(ANGULAR);
171 }
172 296549 LinearType linear_impl()
173 {
174 296549 return m_data.template segment<3>(LINEAR);
175 }
176
177 template<typename V3>
178 ✗ void angular_impl(const Eigen::MatrixBase<V3> & w)
179 {
180 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
181 ✗ angular_impl() = w;
182 }
183 template<typename V3>
184 ✗ void linear_impl(const Eigen::MatrixBase<V3> & v)
185 {
186 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
187 ✗ linear_impl() = v;
188 }
189
190 // Specific operators for MotionTpl and MotionRef
191 template<int O2>
192 559 MotionPlain __plus__(const MotionTpl<Scalar, O2> & v) const
193 {
194
1/2
✓ Branch 3 taken 559 times.
✗ Branch 4 not taken.
 559 return MotionPlain(m_data + v.toVector());
195 }
196
197 template<typename Vector6ArgType>
198 51 MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
199 {
200
2/5
✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
 51 return MotionPlain(m_data + v.toVector());
201 }
202
203 template<int O2>
204 46322 MotionPlain __minus__(const MotionTpl<Scalar, O2> & v) const
205 {
206
1/2
✓ Branch 3 taken 46322 times.
✗ Branch 4 not taken.
 46322 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 215557 MotionTpl & __pequ__(const MotionTpl<Scalar, O2> & v)
217 {
218 215557 m_data += v.toVector();
219 215557 return *this;
220 }
221
222 template<typename Vector6ArgType>
223 367 MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v)
224 {
225
1/2
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
 367 m_data += v.toVector();
226 367 return *this;
227 }
228
229 template<int O2>
230 323 MotionTpl & __mequ__(const MotionTpl<Scalar, O2> & v)
231 {
232 323 m_data -= v.toVector();
233 323 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 10597 MotionPlain __mult__(const OtherScalar & alpha) const
245 {
246
1/2
✓ Branch 2 taken 10597 times.
✗ Branch 3 not taken.
 10597 return MotionPlain(alpha * m_data);
247 }
248
249 ✗ MotionRef<Vector6> ref()
250 {
251 ✗ 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 ✗ MotionTpl<NewScalar, Options> cast() const
257 {
258 typedef MotionTpl<NewScalar, Options> ReturnType;
259 ✗ ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>());
260 ✗ 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