skew.hpp

//
// Copyright (c) 2015-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_skew_hpp__
#define __se3_skew_hpp__

#include "pinocchio/macros.hpp"

namespace se3
{
  
  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_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
    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,EIGEN_PLAIN_TYPE(D)::Options>
  skew(const Eigen::MatrixBase<D> & v)
  {
    Eigen::Matrix<typename D::Scalar,3,3,EIGEN_PLAIN_TYPE(D)::Options> M;
    skew(v,M);
    return M;
  }
  
  template <typename Matrix3, typename Vector3>
  inline void unSkew(const Eigen::MatrixBase<Matrix3> & M,
                     const Eigen::MatrixBase<Vector3> & v)
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
    assert((M + M.transpose()).isMuchSmallerThan(M));
    
    Vector3 & v_ = const_cast<Eigen::MatrixBase<Vector3> &>(v).derived();
    typedef typename Vector3::RealScalar Scalar;
    
    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 EIGEN_PLAIN_TYPE(Matrix3)::Scalar,3,1,EIGEN_PLAIN_TYPE(Matrix3)::Options>
  unSkew(const Eigen::MatrixBase<Matrix3> & M)
  {
    Eigen::Matrix<typename EIGEN_PLAIN_TYPE(Matrix3)::Scalar,3,1,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_ = const_cast<Eigen::MatrixBase<Matrix3> &>(M).derived();
    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,EIGEN_PLAIN_TYPE(Vector3)::Options>
  alphaSkew(const Scalar alpha,
            const Eigen::MatrixBase<Vector3> & v)
  {
    Eigen::Matrix<typename Vector3::Scalar,3,3,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_ = const_cast<Eigen::MatrixBase<Matrix3> &>(C).derived();
    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,EIGEN_PLAIN_TYPE(V1)::Options>
  skewSquare(const Eigen::MatrixBase<V1> & u,
             const Eigen::MatrixBase<V2> & v)
  {
    
    Eigen::Matrix<typename V1::Scalar,3,3,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_ = const_cast<Eigen::MatrixBase<Matrix3xOut> &>(Mout).derived();
    
    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 EIGEN_PLAIN_TYPE(Matrix3x)
  cross(const Eigen::MatrixBase<Vector3> & v,
        const Eigen::MatrixBase<Matrix3x> & M)
  {
    typename EIGEN_PLAIN_TYPE(Matrix3x) res(3,M.cols());
    cross(v,M,res);
    return res;
  }
  
} // namespace se3

#endif // ifndef __se3_skew_hpp__