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__