GCC Code Coverage Report

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_math_matrix_hpp__
6			#define __pinocchio_math_matrix_hpp__
7
8			#include "pinocchio/macros.hpp"
9			#include "pinocchio/math/fwd.hpp"
10			#include "pinocchio/utils/static-if.hpp"
11
12			#include <boost/type_traits.hpp>
13			#include <Eigen/Dense>
14
15			namespace pinocchio
16			{
17
18			template<typename Derived>
19		1367	inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
20			{
21	3/6 ✓ Branch 4 taken 685 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 685 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 685 times. ✗ Branch 11 not taken.	1367	return !((m.derived().array() == m.derived().array()).all());
22			}
23
24			namespace internal
25			{
26			template<
27			typename MatrixLike,
28			bool value = is_floating_point<typename MatrixLike::Scalar>::value>
29			struct isZeroAlgo
30			{
31			typedef typename MatrixLike::Scalar Scalar;
32			typedef typename MatrixLike::RealScalar RealScalar;
33
34		5107	static bool run(
35			const Eigen::MatrixBase<MatrixLike> & mat,
36			const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
37			{
38		5107	return mat.isZero(prec);
39			}
40			};
41
42			template<typename MatrixLike>
43			struct isZeroAlgo<MatrixLike, false>
44			{
45			typedef typename MatrixLike::Scalar Scalar;
46			typedef typename MatrixLike::RealScalar RealScalar;
47
48			static bool run(
49			const Eigen::MatrixBase<MatrixLike> & /vec/,
50			const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
51			{
52			PINOCCHIO_UNUSED_VARIABLE(prec);
53			return true;
54			}
55			};
56			} // namespace internal
57
58			template<typename MatrixLike>
59		5107	inline bool isZero(
60			const Eigen::MatrixBase<MatrixLike> & m,
61			const typename MatrixLike::RealScalar & prec =
62			Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
63			{
64		5107	return internal::isZeroAlgo<MatrixLike>::run(m, prec);
65			}
66
67			template<typename M1, typename M2>
68			struct MatrixMatrixProduct
69			{
70			#if EIGEN_VERSION_AT_LEAST(3, 2, 90)
71			typedef typename Eigen::Product<M1, M2> type;
72			#else
73			typedef typename Eigen::ProductReturnType<M1, M2>::Type type;
74			#endif
75			};
76
77			template<typename Scalar, typename Matrix>
78			struct ScalarMatrixProduct
79			{
80			#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
81			typedef Eigen::CwiseBinaryOp<
82			EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) < Scalar,
83			typename Eigen::internal::traits<Matrix>::Scalar>,
84			const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type,
85			const Matrix > type;
86			#elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
87			typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
88			#else
89			typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
90			type;
91			#endif
92			};
93
94			template<typename Matrix, typename Scalar>
95			struct MatrixScalarProduct
96			{
97			#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
98			typedef Eigen::CwiseBinaryOp<
99			EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) <
100			typename Eigen::internal::traits<Matrix>::Scalar,
101			Scalar>,
102			const Matrix,
103			const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type > type;
104			#elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
105			typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
106			#else
107			typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
108			type;
109			#endif
110			};
111
112			namespace internal
113			{
114			template<
115			typename MatrixLike,
116			bool value = is_floating_point<typename MatrixLike::Scalar>::value>
117			struct isUnitaryAlgo
118			{
119			typedef typename MatrixLike::Scalar Scalar;
120			typedef typename MatrixLike::RealScalar RealScalar;
121
122		124730	static bool run(
123			const Eigen::MatrixBase<MatrixLike> & mat,
124			const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
125			{
126		124730	return mat.isUnitary(prec);
127			}
128			};
129
130			template<typename MatrixLike>
131			struct isUnitaryAlgo<MatrixLike, false>
132			{
133			typedef typename MatrixLike::Scalar Scalar;
134			typedef typename MatrixLike::RealScalar RealScalar;
135
136			static bool run(
137			const Eigen::MatrixBase<MatrixLike> & /vec/,
138			const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
139			{
140			PINOCCHIO_UNUSED_VARIABLE(prec);
141			return true;
142			}
143			};
144			} // namespace internal
145
146			///
147			/// \brief Check whether the input matrix is Unitary within the given precision.
148			///
149			/// \param[in] mat Input matrix
150			/// \param[in] prec Required precision
151			///
152			/// \returns true if mat is unitary within the precision prec
153			///
154			template<typename MatrixLike>
155		65498	inline bool isUnitary(
156			const Eigen::MatrixBase<MatrixLike> & mat,
157			const typename MatrixLike::RealScalar & prec =
158			Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
159			{
160		65498	return internal::isUnitaryAlgo<MatrixLike>::run(mat, prec);
161			}
162
163			namespace internal
164			{
165			template<
166			typename VectorLike,
167			bool value = is_floating_point<typename VectorLike::Scalar>::value>
168			struct isNormalizedAlgo
169			{
170			typedef typename VectorLike::Scalar Scalar;
171			typedef typename VectorLike::RealScalar RealScalar;
172
173		13922	static bool run(
174			const Eigen::MatrixBase<VectorLike> & vec,
175			const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
176			{
177		13922	return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
178			}
179			};
180
181			template<typename VectorLike>
182			struct isNormalizedAlgo<VectorLike, false>
183			{
184			typedef typename VectorLike::Scalar Scalar;
185			typedef typename VectorLike::RealScalar RealScalar;
186
187			static bool run(
188			const Eigen::MatrixBase<VectorLike> & /vec/,
189			const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
190			{
191			PINOCCHIO_UNUSED_VARIABLE(prec);
192			return true;
193			}
194			};
195			} // namespace internal
196
197			///
198			/// \brief Check whether the input vector is Normalized within the given precision.
199			///
200			/// \param[in] vec Input vector
201			/// \param[in] prec Required precision
202			///
203			/// \returns true if vec is normalized within the precision prec.
204			///
205			template<typename VectorLike>
206		13103	inline bool isNormalized(
207			const Eigen::MatrixBase<VectorLike> & vec,
208			const typename VectorLike::RealScalar & prec =
209			Eigen::NumTraits<typename VectorLike::Scalar>::dummy_precision())
210			{
211			EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
212		13103	return internal::isNormalizedAlgo<VectorLike>::run(vec, prec);
213			}
214
215			namespace internal
216			{
217			template<
218			typename VectorLike,
219			bool value = is_floating_point<typename VectorLike::Scalar>::value>
220			struct normalizeAlgo
221			{
222		54	static void run(const Eigen::MatrixBase<VectorLike> & vec)
223			{
224		54	return vec.const_cast_derived().normalize();
225			}
226			};
227
228			template<typename VectorLike>
229			struct normalizeAlgo<VectorLike, false>
230			{
231			static void run(const Eigen::MatrixBase<VectorLike> & vec)
232			{
233			using namespace internal;
234			typedef typename VectorLike::RealScalar RealScalar;
235			typedef typename VectorLike::Scalar Scalar;
236			const RealScalar z = vec.squaredNorm();
237			const Scalar sqrt_z = if_then_else(GT, z, Scalar(0), math::sqrt(z), Scalar(1));
238			vec.const_cast_derived() /= sqrt_z;
239			}
240			};
241			} // namespace internal
242
243			///
244			/// \brief Normalize the input vector.
245			///
246			/// \param[in] vec Input vector
247			///
248			template<typename VectorLike>
249		54	inline void normalize(const Eigen::MatrixBase<VectorLike> & vec)
250			{
251			EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
252		54	internal::normalizeAlgo<VectorLike>::run(vec.const_cast_derived());
253			}
254
255			namespace internal
256			{
257			template<typename Scalar>
258			struct CallCorrectMatrixInverseAccordingToScalar
259			{
260			template<typename MatrixIn, typename MatrixOut>
261			static void
262		✗	run(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
263			{
264		✗	MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
265		✗	dest_.noalias() = m_in.inverse();
266			}
267			};
268
269			} // namespace internal
270
271			template<typename MatrixIn, typename MatrixOut>
272			inline void
273		✗	inverse(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
274			{
275		✗	MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
276		✗	internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(
277			m_in, dest_);
278			}
279
280			} // namespace pinocchio
281
282			#endif // #ifndef __pinocchio_math_matrix_hpp__
283