GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_spatial_motion_ref_hpp__
6			#define __pinocchio_spatial_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			{
18			LINEAR = 0,
19			ANGULAR = 3,
20			Options = Vector6::Options
21			};
22			typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
23			typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
24			typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
25			typedef Matrix6 ActionMatrixType;
26			typedef Matrix4 HomogeneousMatrixType;
27			typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
28			typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
29			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
30			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
31			typedef MotionTpl<Scalar, Options> MotionPlain;
32			typedef MotionPlain PlainReturnType;
33			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
34			typedef DataRefType ToVectorReturnType;
35			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
36			typedef ConstDataRefType ToVectorConstReturnType;
37			typedef MotionRef<Vector6ArgType> MotionRefType;
38
39			}; // traits MotionRef
40
41			template<typename Vector6ArgType>
42			struct SE3GroupAction<MotionRef<Vector6ArgType>>
43			{
44			typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
45			};
46
47			template<typename Vector6ArgType, typename MotionDerived>
48			struct MotionAlgebraAction<MotionRef<Vector6ArgType>, MotionDerived>
49			{
50			typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
51			};
52
53			namespace internal
54			{
55			template<typename Vector6ArgType, typename Scalar>
56			struct RHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
57			{
58			typedef typename pinocchio::traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
59			};
60
61			template<typename Vector6ArgType, typename Scalar>
62			struct LHSScalarMultiplication<MotionRef<Vector6ArgType>, Scalar>
63			{
64			typedef typename traits<MotionRef<Vector6ArgType>>::MotionPlain ReturnType;
65			};
66			} // namespace internal
67
68			template<typename Vector6ArgType>
69			class MotionRef : public MotionDense<MotionRef<Vector6ArgType>>
70			{
71			public:
72			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73			typedef MotionDense<MotionRef> Base;
74			typedef typename traits<MotionRef>::DataRefType DataRefType;
75			MOTION_TYPEDEF_TPL(MotionRef);
76
77			using Base::operator=;
78			using Base::angular;
79			using Base::linear;
80
81			using Base::__mequ__;
82			using Base::__minus__;
83			using Base::__mult__;
84			using Base::__opposite__;
85			using Base::__pequ__;
86			using Base::__plus__;
87
88			/// \brief Default constructor from a 6 dimensional vector.
89		70928	MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
90		70928	: m_ref(v_like)
91			{
92			EIGEN_STATIC_ASSERT(
93			Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
94	1/2 ✗ Branch 1 not taken. ✓ Branch 2 taken 35464 times.	70928	assert(v_like.size() == 6);
95		70928	}
96
97			/// \brief Copy constructor from another MotionRef.
98			MotionRef(const MotionRef & other)
99			: m_ref(other.m_ref)
100			{
101			}
102
103		51	ToVectorConstReturnType toVector_impl() const
104			{
105		51	return m_ref;
106			}
107		27	ToVectorReturnType toVector_impl()
108			{
109		27	return m_ref;
110			}
111
112			// Getters
113		13084	ConstAngularType angular_impl() const
114			{
115		13084	return ConstAngularType(m_ref.derived(), ANGULAR);
116			}
117		4159	ConstLinearType linear_impl() const
118			{
119		8106	return ConstLinearType(m_ref.derived(), LINEAR);
120			}
121		44486	AngularType angular_impl()
122			{
123		50294	return m_ref.template segment<3>(ANGULAR);
124			}
125		26022	LinearType linear_impl()
126			{
127		51969	return m_ref.template segment<3>(LINEAR);
128			}
129
130			template<typename V3>
131			void angular_impl(const Eigen::MatrixBase<V3> & w)
132			{
133			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
134			angular_impl() = w;
135			}
136
137			template<typename V3>
138			void linear_impl(const Eigen::MatrixBase<V3> & v)
139			{
140			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
141			linear_impl() = v;
142			}
143
144			// Specific operators for MotionTpl and MotionRef
145			template<typename S1, int O1>
146			MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
147			{
148			return MotionPlain(m_ref + v.toVector());
149			}
150
151			template<typename Vector6Like>
152			MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
153			{
154			return MotionPlain(m_ref + v.toVector());
155			}
156
157			template<typename S1, int O1>
158			MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
159			{
160			return MotionPlain(m_ref - v.toVector());
161			}
162
163			template<typename Vector6Like>
164			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
165			{
166			return MotionPlain(m_ref - v.toVector());
167			}
168
169			template<typename S1, int O1>
170		8556	MotionRef & __pequ__(const MotionTpl<S1, O1> & v)
171			{
172		8556	m_ref += v.toVector();
173		8556	return *this;
174			}
175
176			template<typename Vector6Like>
177			MotionRef & __pequ__(const MotionRef<Vector6ArgType> & v)
178			{
179			m_ref += v.toVector();
180			return *this;
181			}
182
183			template<typename S1, int O1>
184		✗	MotionRef & __mequ__(const MotionTpl<S1, O1> & v)
185			{
186		✗	m_ref -= v.toVector();
187		✗	return *this;
188			}
189
190			template<typename Vector6Like>
191			MotionRef & __mequ__(const MotionRef<Vector6ArgType> & v)
192			{
193			m_ref -= v.toVector();
194			return *this;
195			}
196
197			template<typename OtherScalar>
198		✗	MotionPlain __mult__(const OtherScalar & alpha) const
199			{
200		✗	return MotionPlain(alpha * m_ref);
201			}
202
203		✗	MotionRef & ref()
204			{
205		✗	return *this;
206			}
207
208		✗	inline PlainReturnType plain() const
209			{
210		✗	return PlainReturnType(m_ref);
211			}
212
213			protected:
214			DataRefType m_ref;
215
216			}; // class MotionRef<Vector6Like>
217
218			template<typename Vector6ArgType>
219			struct traits<MotionRef<const Vector6ArgType>>
220			{
221			typedef typename Vector6ArgType::Scalar Scalar;
222			typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
223			enum
224			{
225			LINEAR = 0,
226			ANGULAR = 3,
227			Options = Vector6::Options
228			};
229			typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
230			typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
231			typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
232			typedef Matrix6 ActionMatrixType;
233			typedef Matrix4 HomogeneousMatrixType;
234			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
235			typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
236			typedef ConstLinearType LinearType;
237			typedef ConstAngularType AngularType;
238			typedef MotionTpl<Scalar, Options> MotionPlain;
239			typedef MotionPlain PlainReturnType;
240			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
241			typedef ConstDataRefType ToVectorConstReturnType;
242			typedef ConstDataRefType DataRefType;
243			typedef DataRefType ToVectorReturnType;
244			typedef MotionRef<const Vector6ArgType> MotionRefType;
245
246			}; // traits MotionRef<const Vector6ArgType>
247
248			template<typename Vector6ArgType>
249			class MotionRef<const Vector6ArgType> : public MotionDense<MotionRef<const Vector6ArgType>>
250			{
251			public:
252			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
253			typedef MotionDense<MotionRef> Base;
254			typedef typename traits<MotionRef>::DataRefType DataRefType;
255			MOTION_TYPEDEF_TPL(MotionRef);
256
257			using Base::operator=;
258			using Base::angular;
259			using Base::linear;
260
261			using Base::__minus__;
262			using Base::__mult__;
263			using Base::__opposite__;
264			using Base::__plus__;
265
266		284571	MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
267		284571	: m_ref(v_like)
268			{
269			EIGEN_STATIC_ASSERT(
270			Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
271	1/2 ✗ Branch 1 not taken. ✓ Branch 2 taken 143517 times.	284571	assert(v_like.size() == 6);
272		284571	}
273
274			/// \brief Copy constructor from another MotionRef.
275			MotionRef(const MotionRef & other)
276			: m_ref(other.m_ref)
277			{
278			}
279
280		367	ToVectorConstReturnType toVector_impl() const
281			{
282		367	return m_ref;
283			}
284
285			// Getters
286		537038	ConstAngularType angular_impl() const
287			{
288		537038	return ConstAngularType(m_ref.derived(), ANGULAR);
289			}
290		3144	ConstLinearType linear_impl() const
291			{
292		139738	return ConstLinearType(m_ref.derived(), LINEAR);
293			}
294
295			// Specific operators for MotionTpl and MotionRef
296			template<typename S1, int O1>
297		1167	MotionPlain __plus__(const MotionTpl<S1, O1> & v) const
298			{
299	1/2 ✓ Branch 3 taken 1167 times. ✗ Branch 4 not taken.	1167	return MotionPlain(m_ref + v.toVector());
300			}
301
302			template<typename Vector6Like>
303			MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
304			{
305			return MotionPlain(m_ref + v.toVector());
306			}
307
308			template<typename S1, int O1>
309			MotionPlain __minus__(const MotionTpl<S1, O1> & v) const
310			{
311			return MotionPlain(m_ref - v.toVector());
312			}
313
314			template<typename Vector6Like>
315			MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
316			{
317			return MotionPlain(m_ref - v.toVector());
318			}
319
320			template<typename OtherScalar>
321			MotionPlain __mult__(const OtherScalar & alpha) const
322			{
323			return MotionPlain(alpha * m_ref);
324			}
325
326			const MotionRef & ref() const
327			{
328			return *this;
329			}
330
331		4222	inline PlainReturnType plain() const
332			{
333		4222	return PlainReturnType(m_ref);
334			}
335
336			protected:
337			DataRefType m_ref;
338
339			}; // class MotionRef<const Vector6Like>
340
341			} // namespace pinocchio
342
343			#endif // ifndef __pinocchio_spatial_motion_ref_hpp__
344