Directory: | ./ |
---|---|
File: | include/pinocchio/math/matrix.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 18 | 24 | 75.0% |
Branches: | 3 | 10 | 30.0% |
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 |