5 #ifndef __pinocchio_skew_hpp__
6 #define __pinocchio_skew_hpp__
20 template <
typename Vector3,
typename Matrix3>
21 inline void skew(
const Eigen::MatrixBase<Vector3> &
v,
22 const Eigen::MatrixBase<Matrix3> & M)
24 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
25 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
28 typedef typename Matrix3::RealScalar
Scalar;
30 M_(0,0) =
Scalar(0); M_(0,1) = -
v[2]; M_(0,2) =
v[1];
31 M_(1,0) =
v[2]; M_(1,1) =
Scalar(0); M_(1,2) = -
v[0];
32 M_(2,0) = -
v[1]; M_(2,1) =
v[0]; M_(2,2) =
Scalar(0);
59 template <
typename Vector3Like,
typename Matrix3Like>
60 inline void addSkew(
const Eigen::MatrixBase<Vector3Like> &
v,
61 const Eigen::MatrixBase<Matrix3Like> & M)
63 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
68 M_(0,1) -=
v[2]; M_(0,2) +=
v[1];
69 M_(1,0) +=
v[2]; M_(1,2) -=
v[0];
70 M_(2,0) -=
v[1]; M_(2,1) +=
v[0]; ;
81 template <
typename Matrix3,
typename Vector3>
82 inline void unSkew(
const Eigen::MatrixBase<Matrix3> & M,
83 const Eigen::MatrixBase<Vector3> &
v)
85 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
86 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
87 assert((
M +
M.transpose()).isMuchSmallerThan(
M));
90 typedef typename Vector3::RealScalar
Scalar;
92 v_[0] =
Scalar(0.5) * (
M(2,1) -
M(1,2));
93 v_[1] =
Scalar(0.5) * (
M(0,2) -
M(2,0));
94 v_[2] =
Scalar(0.5) * (
M(1,0) -
M(0,1));
106 template <
typename Matrix3>
123 template <
typename Scalar,
typename Vector3,
typename Matrix3>
125 const Eigen::MatrixBase<Vector3> &
v,
126 const Eigen::MatrixBase<Matrix3> & M)
128 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
129 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
132 typedef typename Matrix3::RealScalar RealScalar;
134 M_(0,0) = RealScalar(0); M_(0,1) = -
v[2] * alpha; M_(0,2) =
v[1] * alpha;
135 M_(1,0) = -M_(0,1); M_(1,1) = RealScalar(0); M_(1,2) = -
v[0] * alpha;
136 M_(2,0) = -M_(0,2); M_(2,1) = -M_(1,2); M_(2,2) = RealScalar(0);
148 template <
typename Scalar,
typename Vector3>
151 const
Eigen::MatrixBase<Vector3> &
v)
165 template <
typename V1,
typename V2,
typename Matrix3>
167 const Eigen::MatrixBase<V2> &
v,
168 const Eigen::MatrixBase<Matrix3> &
C)
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);
175 typedef typename Matrix3::RealScalar
Scalar;
177 C_.noalias() =
v*
u.transpose();
179 C_.diagonal().array() -= udotv;
190 template <
typename V1,
typename V2>
193 const
Eigen::MatrixBase<V2> &
v)
210 template <
typename Vector3,
typename Matrix3xIn,
typename Matrix3xOut>
211 inline void cross(
const Eigen::MatrixBase<Vector3> &
v,
212 const Eigen::MatrixBase<Matrix3xIn> & Min,
213 const Eigen::MatrixBase<Matrix3xOut> & Mout)
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);
221 Mout_.row(0) =
v[1]*Min.row(2) -
v[2]*Min.row(1);
222 Mout_.row(1) =
v[2]*Min.row(0) -
v[0]*Min.row(2);
223 Mout_.row(2) =
v[0]*Min.row(1) -
v[1]*Min.row(0);
234 template <
typename Vector3,
typename Matrix3x>
236 cross(
const Eigen::MatrixBase<Vector3> &
v,
237 const Eigen::MatrixBase<Matrix3x> & M)
246 #endif // ifndef __pinocchio_skew_hpp__