GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/spatial/skew.hpp Lines: 58 58 100.0 % Date: 2024-04-26 13:14:21 Branches: 38 76 50.0 %

 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  21 34268  inline void skew(const Eigen::MatrixBase & v, 22  const Eigen::MatrixBase & 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  44  inline Eigen::Matrix 45 34120  skew(const Eigen::MatrixBase & v) 46  { 47 34120  Eigen::Matrix 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  60 18893  inline void addSkew(const Eigen::MatrixBase & v, 61  const Eigen::MatrixBase & 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  82 3  inline void unSkew(const Eigen::MatrixBase & M, 83  const Eigen::MatrixBase & 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  107  inline Eigen::Matrix 108 3  unSkew(const Eigen::MatrixBase & M) 109  { 110 3  Eigen::Matrix 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  124 9380  void alphaSkew(const Scalar alpha, 125  const Eigen::MatrixBase & v, 126  const Eigen::MatrixBase & M) 127  { 128  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3); 129  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3); 130 131 9380  Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M); 132  typedef typename Matrix3::RealScalar RealScalar; 133 134 9380  M_(0,0) = RealScalar(0); M_(0,1) = -v[2] * alpha; M_(0,2) = v[1] * alpha; 135 9380  M_(1,0) = -M_(0,1); M_(1,1) = RealScalar(0); M_(1,2) = -v[0] * alpha; 136 9380  M_(2,0) = -M_(0,2); M_(2,1) = -M_(1,2); M_(2,2) = RealScalar(0); 137 9380  } 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  149  inline Eigen::Matrix 150 7672  alphaSkew(const Scalar alpha, 151  const Eigen::MatrixBase & v) 152  { 153 7672  Eigen::Matrix M; 154 7672  alphaSkew(alpha,v,M); 155 7672  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  166 94804  inline void skewSquare(const Eigen::MatrixBase & u, 167  const Eigen::MatrixBase & v, 168  const Eigen::MatrixBase & 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  191  inline Eigen::Matrix 192 94274  skewSquare(const Eigen::MatrixBase & u, 193  const Eigen::MatrixBase & v) 194  { 195 196 94274  Eigen::Matrix 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  211 23824  inline void cross(const Eigen::MatrixBase & v, 212  const Eigen::MatrixBase & Min, 213  const Eigen::MatrixBase & 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 23824  Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,Mout); 220 221 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 23824  Mout_.row(0) = v[1]*Min.row(2) - v[2]*Min.row(1); 222 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 23824  Mout_.row(1) = v[2]*Min.row(0) - v[0]*Min.row(2); 223 ✓✗✓✗✓✗✓✗✓✗✓✗✓✗✓✗ 23824  Mout_.row(2) = v[0]*Min.row(1) - v[1]*Min.row(0); 224 23824  } 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  235  inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x) 236 11842  cross(const Eigen::MatrixBase & v, 237  const Eigen::MatrixBase & 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)