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__