pinocchio  2.3.1-dirty
matrix.hpp
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
7 
8 #include <Eigen/Core>
9 #include <boost/type_traits.hpp>
10 
11 namespace pinocchio
12 {
13 
14  template<typename Derived>
15  inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
16  {
17  return !((m.derived().array()==m.derived().array()).all());
18  }
19 
20  template<typename M1, typename M2>
22  {
23 #if EIGEN_VERSION_AT_LEAST(3,2,90)
24  typedef typename Eigen::Product<M1,M2> type;
25 #else
26  typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
27 #endif
28  };
29 
30  template<typename Scalar, typename Matrix>
32  {
33 #if EIGEN_VERSION_AT_LEAST(3,3,0)
34  typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<Scalar,typename Eigen::internal::traits<Matrix>::Scalar>,
35  const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type, const Matrix> type;
36 #elif EIGEN_VERSION_AT_LEAST(3,2,90)
37  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
38 #else
39  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
40 #endif
41  };
42 
43  template<typename Matrix, typename Scalar>
45  {
46 #if EIGEN_VERSION_AT_LEAST(3,3,0)
47  typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<typename Eigen::internal::traits<Matrix>::Scalar,Scalar>,
48  const Matrix, const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type> type;
49 #elif EIGEN_VERSION_AT_LEAST(3,2,90)
50  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
51 #else
52  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
53 #endif
54  };
55 
56  namespace internal
57  {
58  template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
59  struct isUnitaryAlgo
60  {
61  typedef typename MatrixLike::Scalar Scalar;
62  typedef typename MatrixLike::RealScalar RealScalar;
63 
64  static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
65  const RealScalar & prec =
66  Eigen::NumTraits< Scalar >::dummy_precision())
67  {
68  return mat.isUnitary(prec);
69  }
70  };
71 
72  template<typename MatrixLike>
73  struct isUnitaryAlgo<MatrixLike,false>
74  {
75  typedef typename MatrixLike::Scalar Scalar;
76  typedef typename MatrixLike::RealScalar RealScalar;
77 
78  static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
79  const RealScalar & prec =
80  Eigen::NumTraits< Scalar >::dummy_precision())
81  {
82  PINOCCHIO_UNUSED_VARIABLE(prec);
83  return true;
84  }
85  };
86  }
87 
88  template<typename MatrixLike>
89  inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
90  const typename MatrixLike::RealScalar & prec =
91  Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
92  {
93  return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
94  }
95 
96  namespace internal
97  {
98  template<typename Scalar>
99  struct CallCorrectMatrixInverseAccordingToScalar
100  {
101  template<typename MatrixIn, typename MatrixOut>
102  static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
103  const Eigen::MatrixBase<MatrixOut> & dest)
104  {
105  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
106  dest_.noalias() = m_in.inverse();
107  }
108  };
109 
110  }
111 
112  template<typename MatrixIn, typename MatrixOut>
113  inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
114  const Eigen::MatrixBase<MatrixOut> & dest)
115  {
116  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
117  internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
118  }
119 
120 }
121 
122 #endif //#ifndef __pinocchio_math_matrix_hpp__
Main pinocchio namespace.
Definition: treeview.dox:24