GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/skew.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 72 72 100.0%
Branches: 105 238 44.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_skew_hpp__
6 #define __pinocchio_spatial_skew_hpp__
7
8 #include "pinocchio/macros.hpp"
9
10 namespace pinocchio
11 {
12
13 ///
14 /// \brief Computes the skew representation of a given 3d vector,
15 /// i.e. the antisymmetric matrix representation of the cross product operator (\f$
16 /// [v]_{\times} x = v \times x \f$)
17 ///
18 /// \param[in] v a vector of dimension 3.
19 /// \param[out] M the skew matrix representation of dimension 3x3.
20 ///
21 template<typename Vector3, typename Matrix3>
22 33516 inline void skew(const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3> & M)
23 {
24 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
25 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
26
27 33516 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
28 typedef typename Matrix3::RealScalar Scalar;
29
30
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(0, 0) = Scalar(0);
31
3/8
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 33516 M_(0, 1) = -v[2];
32
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(0, 2) = v[1];
33
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(1, 0) = v[2];
34
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(1, 1) = Scalar(0);
35
3/8
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 33516 M_(1, 2) = -v[0];
36
3/8
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 33516 M_(2, 0) = -v[1];
37
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(2, 1) = v[0];
38
2/4
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
 33516 M_(2, 2) = Scalar(0);
39 33516 }
40
41 ///
42 /// \brief Computes the skew representation of a given 3D vector,
43 /// i.e. the antisymmetric matrix representation of the cross product operator.
44 ///
45 /// \param[in] v a vector of dimension 3.
46 ///
47 /// \return The skew matrix representation of v.
48 ///
49 template<typename D>
50 inline Eigen::Matrix<typename D::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
51 33366 skew(const Eigen::MatrixBase<D> & v)
52 {
53 33366 Eigen::Matrix<typename D::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options> M;
54
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 33366 skew(v, M);
55 33366 return M;
56 }
57
58 ///
59 /// \brief Add skew matrix represented by a 3d vector to a given matrix,
60 /// i.e. add the antisymmetric matrix representation of the cross product operator (\f$
61 /// [v]_{\times} x = v \times x \f$)
62 ///
63 /// \param[in] v a vector of dimension 3.
64 /// \param[out] M the 3x3 matrix to which the skew matrix is added.
65 ///
66 template<typename Vector3Like, typename Matrix3Like>
67 inline void
68 26079 addSkew(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix3Like> & M)
69 {
70 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
71
2/4
✓ Branch 1 taken 25240 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25240 times.
✗ Branch 5 not taken.
 26079 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3);
72
73 26079 Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, M);
74
75
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(0, 1) -= v[2];
76
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(0, 2) += v[1];
77
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(1, 0) += v[2];
78
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(1, 2) -= v[0];
79
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(2, 0) -= v[1];
80
2/4
✓ Branch 2 taken 81 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 81 times.
✗ Branch 6 not taken.
 26079 M_(2, 1) += v[0];
81 ;
82 26079 }
83
84 ///
85 /// \brief Inverse of skew operator. From a given skew-symmetric matrix M
86 /// of dimension 3x3, it extracts the supporting vector, i.e. the entries of M.
87 /// Mathematically speacking, it computes \f$ v \f$ such that \f$ M x = v \times x \f$.
88 ///
89 /// \param[in] M a 3x3 skew symmetric matrix.
90 /// \param[out] v the 3d vector representation of M.
91 ///
92 template<typename Matrix3, typename Vector3>
93 31282 inline void unSkew(const Eigen::MatrixBase<Matrix3> & M, const Eigen::MatrixBase<Vector3> & v)
94 {
95 typedef typename Vector3::RealScalar Scalar;
96 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
97 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
98
99 31282 Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3, v);
100
101
4/8
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 31282 v_[0] = Scalar(0.5) * (M(2, 1) - M(1, 2));
102
4/8
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 31282 v_[1] = Scalar(0.5) * (M(0, 2) - M(2, 0));
103
4/8
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 31282 v_[2] = Scalar(0.5) * (M(1, 0) - M(0, 1));
104 31282 }
105
106 ///
107 /// \brief Inverse of skew operator. From a given skew-symmetric matrix M
108 /// of dimension 3x3, it extracts the supporting vector, i.e. the entries of M.
109 /// Mathematically speacking, it computes \f$ v \f$ such that \f$ M x = v \times x \f$.
110 ///
111 /// \param[in] M a 3x3 matrix.
112 ///
113 /// \return The vector entries of the skew-symmetric matrix.
114 ///
115 template<typename Matrix3>
116 inline Eigen::Matrix<typename Matrix3::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options>
117 10 unSkew(const Eigen::MatrixBase<Matrix3> & M)
118 {
119 10 Eigen::Matrix<typename Matrix3::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options> v;
120
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 10 unSkew(M, v);
121 10 return v;
122 }
123
124 ///
125 /// \brief Computes the skew representation of a given 3d vector multiplied by a given scalar.
126 /// i.e. the antisymmetric matrix representation of the cross product operator (\f$ [\alpha
127 /// v]_{\times} x = \alpha v \times x \f$)
128 ///
129 /// \param[in] alpha a real scalar.
130 /// \param[in] v a vector of dimension 3.
131 /// \param[out] M the skew matrix representation of dimension 3x3.
132 ///
133 template<typename Scalar, typename Vector3, typename Matrix3>
134 11950 void alphaSkew(
135 const Scalar alpha, const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3> & M)
136 {
137 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
138 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
139
140 11950 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
141 typedef typename Matrix3::RealScalar RealScalar;
142
143
2/4
✓ Branch 2 taken 207 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 207 times.
✗ Branch 6 not taken.
 11950 M_(0, 0) = RealScalar(0);
144
3/13
✗ Branch 2 not taken.
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 207 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 11950 M_(0, 1) = -v[2] * alpha;
145
2/10
✗ Branch 2 not taken.
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 11950 M_(0, 2) = v[1] * alpha;
146
2/4
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
 11950 M_(1, 0) = -M_(0, 1);
147
2/4
✓ Branch 2 taken 207 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 207 times.
✗ Branch 6 not taken.
 11950 M_(1, 1) = RealScalar(0);
148
3/13
✗ Branch 2 not taken.
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 207 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 11950 M_(1, 2) = -v[0] * alpha;
149
2/4
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
 11950 M_(2, 0) = -M_(0, 2);
150
2/4
✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 207 times.
✗ Branch 7 not taken.
 11950 M_(2, 1) = -M_(1, 2);
151
2/4
✓ Branch 2 taken 207 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 207 times.
✗ Branch 6 not taken.
 11950 M_(2, 2) = RealScalar(0);
152 11950 }
153
154 ///
155 /// \brief Computes the skew representation of a given 3d vector multiplied by a given scalar.
156 /// i.e. the antisymmetric matrix representation of the cross product operator (\f$ [\alpha
157 /// v]_{\times} x = \alpha v \times x \f$)
158 ///
159 /// \param[in] alpha a real scalar.
160 /// \param[in] v a vector of dimension 3.
161 ///
162 /// \returns the skew matrix representation of \f$ \alpha v \f$.
163 ///
164 template<typename Scalar, typename Vector3>
165 inline Eigen::Matrix<typename Vector3::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options>
166 9838 alphaSkew(const Scalar alpha, const Eigen::MatrixBase<Vector3> & v)
167 {
168 9838 Eigen::Matrix<typename Vector3::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options> M;
169
2/4
✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
 9838 alphaSkew(alpha, v, M);
170 9838 return M;
171 }
172
173 ///
174 /// \brief Computes the square cross product linear operator C(u,v) such that for any vector w,
175 /// \f$ u \times ( v \times w ) = C(u,v) w \f$.
176 ///
177 /// \param[in] u a 3 dimensional vector.
178 /// \param[in] v a 3 dimensional vector.
179 /// \param[out] C the skew square matrix representation of dimension 3x3.
180 ///
181 template<typename V1, typename V2, typename Matrix3>
182 119949 inline void skewSquare(
183 const Eigen::MatrixBase<V1> & u,
184 const Eigen::MatrixBase<V2> & v,
185 const Eigen::MatrixBase<Matrix3> & C)
186 {
187 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V1, 3);
188 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V2, 3);
189 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
190
191 119949 Matrix3 & C_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, C);
192 typedef typename Matrix3::RealScalar Scalar;
193
194
4/8
✓ Branch 1 taken 59975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59975 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59975 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59975 times.
✗ Branch 11 not taken.
 119949 C_.noalias() = v * u.transpose();
195
1/2
✓ Branch 1 taken 59975 times.
✗ Branch 2 not taken.
 119949 const Scalar udotv(u.dot(v));
196
3/6
✓ Branch 1 taken 59975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59975 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59975 times.
✗ Branch 8 not taken.
 119949 C_.diagonal().array() -= udotv;
197 119949 }
198
199 ///
200 /// \brief Computes the square cross product linear operator C(u,v) such that for any vector w,
201 /// \f$ u \times ( v \times w ) = C(u,v) w \f$.
202 ///
203 /// \param[in] u A 3 dimensional vector.
204 /// \param[in] v A 3 dimensional vector.
205 ///
206 /// \return The square cross product matrix skew[u] * skew[v].
207 ///
208 template<typename V1, typename V2>
209 inline Eigen::Matrix<typename V1::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
210 119419 skewSquare(const Eigen::MatrixBase<V1> & u, const Eigen::MatrixBase<V2> & v)
211 {
212
213 119419 Eigen::Matrix<typename V1::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options> M;
214
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 119419 skewSquare(u, v, M);
215 119419 return M;
216 }
217
218 ///
219 /// \brief Applies the cross product onto the columns of M.
220 ///
221 /// \param[in] v a vector of dimension 3.
222 /// \param[in] Min a 3 rows matrix.
223 /// \param[out] Mout a 3 rows matrix.
224 ///
225 /// \return the results of \f$ Mout = [v]_{\times} Min \f$.
226 ///
227 template<typename Vector3, typename Matrix3xIn, typename Matrix3xOut>
228 30220 inline void cross(
229 const Eigen::MatrixBase<Vector3> & v,
230 const Eigen::MatrixBase<Matrix3xIn> & Min,
231 const Eigen::MatrixBase<Matrix3xOut> & Mout)
232 {
233 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
234 EIGEN_STATIC_ASSERT(
235 Matrix3xIn::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
236 EIGEN_STATIC_ASSERT(
237 Matrix3xOut::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
238
239 30220 Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, Mout);
240
241
8/16
✓ Branch 2 taken 15110 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15110 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15110 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 15110 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 15110 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 15110 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 15110 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 15110 times.
✗ Branch 24 not taken.
 30220 Mout_.row(0) = v[1] * Min.row(2) - v[2] * Min.row(1);
242
8/16
✓ Branch 2 taken 15110 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15110 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15110 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 15110 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 15110 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 15110 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 15110 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 15110 times.
✗ Branch 24 not taken.
 30220 Mout_.row(1) = v[2] * Min.row(0) - v[0] * Min.row(2);
243
8/16
✓ Branch 2 taken 15110 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15110 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15110 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 15110 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 15110 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 15110 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 15110 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 15110 times.
✗ Branch 24 not taken.
 30220 Mout_.row(2) = v[0] * Min.row(1) - v[1] * Min.row(0);
244 30220 }
245
246 ///
247 /// \brief Applies the cross product onto the columns of M.
248 ///
249 /// \param[in] v a vector of dimension 3.
250 /// \param[in] M a 3 rows matrix.
251 ///
252 /// \return the results of \f$ [v]_{\times} M \f$.
253 ///
254 template<typename Vector3, typename Matrix3x>
255 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x)
256 15016 cross(const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3x> & M)
257 {
258
1/2
✓ Branch 2 taken 14565 times.
✗ Branch 3 not taken.
 15016 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x) res(3, M.cols());
259
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 15016 cross(v, M, res);
260 15016 return res;
261 }
262
263 } // namespace pinocchio
264
265 #endif // ifndef __pinocchio_spatial_skew_hpp__
266