| 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 |