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