Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/skew.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 67 | 71 | 94.4% |
Branches: | 36 | 72 | 50.0% |
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 | 20889 | 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 | 20889 | Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M); | |
28 | typedef typename Matrix3::RealScalar Scalar; | ||
29 | |||
30 | 20889 | M_(0, 0) = Scalar(0); | |
31 | 20889 | M_(0, 1) = -v[2]; | |
32 | 20889 | M_(0, 2) = v[1]; | |
33 | 20889 | M_(1, 0) = v[2]; | |
34 | 20889 | M_(1, 1) = Scalar(0); | |
35 | 20889 | M_(1, 2) = -v[0]; | |
36 | 20889 | M_(2, 0) = -v[1]; | |
37 | 20889 | M_(2, 1) = v[0]; | |
38 | 20889 | M_(2, 2) = Scalar(0); | |
39 | 20889 | } | |
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 | 20835 | skew(const Eigen::MatrixBase<D> & v) | |
52 | { | ||
53 | 20835 | Eigen::Matrix<typename D::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options> M; | |
54 | 20835 | skew(v, M); | |
55 | 20835 | 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 | 12425 | 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 12228 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12228 times.
✗ Branch 5 not taken.
|
12425 | PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3); |
72 | |||
73 | 12425 | Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, M); | |
74 | |||
75 | 12425 | M_(0, 1) -= v[2]; | |
76 | 12425 | M_(0, 2) += v[1]; | |
77 | 12425 | M_(1, 0) += v[2]; | |
78 | 12425 | M_(1, 2) -= v[0]; | |
79 | 12425 | M_(2, 0) -= v[1]; | |
80 | 12425 | M_(2, 1) += v[0]; | |
81 | ; | ||
82 | 12425 | } | |
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 | 2659 | 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 | 2659 | Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3, v); | |
100 | |||
101 | 2659 | v_[0] = Scalar(0.5) * (M(2, 1) - M(1, 2)); | |
102 | 2659 | v_[1] = Scalar(0.5) * (M(0, 2) - M(2, 0)); | |
103 | 2659 | v_[2] = Scalar(0.5) * (M(1, 0) - M(0, 1)); | |
104 | 2659 | } | |
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 | ✗ | unSkew(const Eigen::MatrixBase<Matrix3> & M) | |
118 | { | ||
119 | ✗ | Eigen::Matrix<typename Matrix3::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options> v; | |
120 | ✗ | unSkew(M, v); | |
121 | ✗ | 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 | 1175 | 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 | 1175 | Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M); | |
141 | typedef typename Matrix3::RealScalar RealScalar; | ||
142 | |||
143 | 1175 | M_(0, 0) = RealScalar(0); | |
144 | 1175 | M_(0, 1) = -v[2] * alpha; | |
145 | 1175 | M_(0, 2) = v[1] * alpha; | |
146 | 1175 | M_(1, 0) = -M_(0, 1); | |
147 | 1175 | M_(1, 1) = RealScalar(0); | |
148 | 1175 | M_(1, 2) = -v[0] * alpha; | |
149 | 1175 | M_(2, 0) = -M_(0, 2); | |
150 | 1175 | M_(2, 1) = -M_(1, 2); | |
151 | 1175 | M_(2, 2) = RealScalar(0); | |
152 | 1175 | } | |
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 | 901 | alphaSkew(const Scalar alpha, const Eigen::MatrixBase<Vector3> & v) | |
167 | { | ||
168 | 901 | Eigen::Matrix<typename Vector3::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options> M; | |
169 | 901 | alphaSkew(alpha, v, M); | |
170 | 901 | 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 | 83312 | 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 | 83312 | Matrix3 & C_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, C); | |
192 | typedef typename Matrix3::RealScalar Scalar; | ||
193 | |||
194 |
4/8✓ Branch 1 taken 41656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41656 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 41656 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 41656 times.
✗ Branch 11 not taken.
|
83312 | C_.noalias() = v * u.transpose(); |
195 |
1/2✓ Branch 1 taken 41656 times.
✗ Branch 2 not taken.
|
83312 | const Scalar udotv(u.dot(v)); |
196 |
3/6✓ Branch 1 taken 41656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 41656 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 41656 times.
✗ Branch 8 not taken.
|
83312 | C_.diagonal().array() -= udotv; |
197 | 83312 | } | |
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 | 83312 | skewSquare(const Eigen::MatrixBase<V1> & u, const Eigen::MatrixBase<V2> & v) | |
211 | { | ||
212 | |||
213 | 83312 | Eigen::Matrix<typename V1::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options> M; | |
214 | 83312 | skewSquare(u, v, M); | |
215 | 83312 | 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 | 20956 | 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 | 20956 | Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, Mout); | |
240 | |||
241 |
8/16✓ Branch 2 taken 10478 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10478 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10478 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10478 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10478 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10478 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10478 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10478 times.
✗ Branch 24 not taken.
|
20956 | Mout_.row(0) = v[1] * Min.row(2) - v[2] * Min.row(1); |
242 |
8/16✓ Branch 2 taken 10478 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10478 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10478 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10478 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10478 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10478 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10478 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10478 times.
✗ Branch 24 not taken.
|
20956 | Mout_.row(1) = v[2] * Min.row(0) - v[0] * Min.row(2); |
243 |
8/16✓ Branch 2 taken 10478 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10478 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10478 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10478 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10478 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10478 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10478 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10478 times.
✗ Branch 24 not taken.
|
20956 | Mout_.row(2) = v[0] * Min.row(1) - v[1] * Min.row(0); |
244 | } | ||
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 | 10474 | cross(const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3x> & M) | |
257 | { | ||
258 |
1/2✓ Branch 2 taken 10416 times.
✗ Branch 3 not taken.
|
10474 | typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x) res(3, M.cols()); |
259 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
10474 | cross(v, M, res); |
260 | 10474 | return res; | |
261 | } | ||
262 | |||
263 | } // namespace pinocchio | ||
264 | |||
265 | #endif // ifndef __pinocchio_spatial_skew_hpp__ | ||
266 |