Program Listing for File skew.hpp
↰ Return to documentation for file (include/pinocchio/spatial/skew.hpp
)
//
// Copyright (c) 2015-2021 CNRS INRIA
//
#ifndef __pinocchio_spatial_skew_hpp__
#define __pinocchio_spatial_skew_hpp__
#include "pinocchio/macros.hpp"
namespace pinocchio
{
template<typename Vector3, typename Matrix3>
inline void skew(const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
typedef typename Matrix3::RealScalar Scalar;
M_(0, 0) = Scalar(0);
M_(0, 1) = -v[2];
M_(0, 2) = v[1];
M_(1, 0) = v[2];
M_(1, 1) = Scalar(0);
M_(1, 2) = -v[0];
M_(2, 0) = -v[1];
M_(2, 1) = v[0];
M_(2, 2) = Scalar(0);
}
template<typename D>
inline Eigen::Matrix<typename D::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options>
skew(const Eigen::MatrixBase<D> & v)
{
Eigen::Matrix<typename D::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(D)::Options> M;
skew(v, M);
return M;
}
template<typename Vector3Like, typename Matrix3Like>
inline void
addSkew(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix3Like> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, M, 3, 3);
Matrix3Like & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, M);
M_(0, 1) -= v[2];
M_(0, 2) += v[1];
M_(1, 0) += v[2];
M_(1, 2) -= v[0];
M_(2, 0) -= v[1];
M_(2, 1) += v[0];
;
}
template<typename Matrix3, typename Vector3>
inline void unSkew(const Eigen::MatrixBase<Matrix3> & M, const Eigen::MatrixBase<Vector3> & v)
{
typedef typename Vector3::RealScalar Scalar;
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
Vector3 & v_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3, v);
v_[0] = Scalar(0.5) * (M(2, 1) - M(1, 2));
v_[1] = Scalar(0.5) * (M(0, 2) - M(2, 0));
v_[2] = Scalar(0.5) * (M(1, 0) - M(0, 1));
}
template<typename Matrix3>
inline Eigen::Matrix<typename Matrix3::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options>
unSkew(const Eigen::MatrixBase<Matrix3> & M)
{
Eigen::Matrix<typename Matrix3::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options> v;
unSkew(M, v);
return v;
}
template<typename Scalar, typename Vector3, typename Matrix3>
void alphaSkew(
const Scalar alpha, const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3> & M)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
typedef typename Matrix3::RealScalar RealScalar;
M_(0, 0) = RealScalar(0);
M_(0, 1) = -v[2] * alpha;
M_(0, 2) = v[1] * alpha;
M_(1, 0) = -M_(0, 1);
M_(1, 1) = RealScalar(0);
M_(1, 2) = -v[0] * alpha;
M_(2, 0) = -M_(0, 2);
M_(2, 1) = -M_(1, 2);
M_(2, 2) = RealScalar(0);
}
template<typename Scalar, typename Vector3>
inline Eigen::Matrix<typename Vector3::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options>
alphaSkew(const Scalar alpha, const Eigen::MatrixBase<Vector3> & v)
{
Eigen::Matrix<typename Vector3::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3)::Options> M;
alphaSkew(alpha, v, M);
return M;
}
template<typename V1, typename V2, typename Matrix3>
inline void skewSquare(
const Eigen::MatrixBase<V1> & u,
const Eigen::MatrixBase<V2> & v,
const Eigen::MatrixBase<Matrix3> & C)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V1, 3);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V2, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3);
Matrix3 & C_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, C);
typedef typename Matrix3::RealScalar Scalar;
C_.noalias() = v * u.transpose();
const Scalar udotv(u.dot(v));
C_.diagonal().array() -= udotv;
}
template<typename V1, typename V2>
inline Eigen::Matrix<typename V1::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options>
skewSquare(const Eigen::MatrixBase<V1> & u, const Eigen::MatrixBase<V2> & v)
{
Eigen::Matrix<typename V1::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(V1)::Options> M;
skewSquare(u, v, M);
return M;
}
template<typename Vector3, typename Matrix3xIn, typename Matrix3xOut>
inline void cross(
const Eigen::MatrixBase<Vector3> & v,
const Eigen::MatrixBase<Matrix3xIn> & Min,
const Eigen::MatrixBase<Matrix3xOut> & Mout)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3);
EIGEN_STATIC_ASSERT(
Matrix3xIn::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(
Matrix3xOut::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
Matrix3xOut & Mout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, Mout);
Mout_.row(0) = v[1] * Min.row(2) - v[2] * Min.row(1);
Mout_.row(1) = v[2] * Min.row(0) - v[0] * Min.row(2);
Mout_.row(2) = v[0] * Min.row(1) - v[1] * Min.row(0);
}
template<typename Vector3, typename Matrix3x>
inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x)
cross(const Eigen::MatrixBase<Vector3> & v, const Eigen::MatrixBase<Matrix3x> & M)
{
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3x) res(3, M.cols());
cross(v, M, res);
return res;
}
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_skew_hpp__