math/matrix.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/fwd.hpp"
10 
11 #include <Eigen/Core>
12 #include <boost/type_traits.hpp>
13 
14 namespace pinocchio
15 {
16 
17  template<typename Derived>
18  inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
19  {
20  return !((m.derived().array()==m.derived().array()).all());
21  }
22 
23  namespace internal
24  {
25  template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
26  struct isZeroAlgo
27  {
28  typedef typename MatrixLike::Scalar Scalar;
29  typedef typename MatrixLike::RealScalar RealScalar;
30 
31  static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
32  const RealScalar & prec =
33  Eigen::NumTraits< Scalar >::dummy_precision())
34  {
35  return mat.isZero(prec);
36  }
37  };
38 
39  template<typename MatrixLike>
40  struct isZeroAlgo<MatrixLike,false>
41  {
42  typedef typename MatrixLike::Scalar Scalar;
43  typedef typename MatrixLike::RealScalar RealScalar;
44 
45  static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
46  const RealScalar & prec =
47  Eigen::NumTraits< Scalar >::dummy_precision())
48  {
50  return true;
51  }
52  };
53  }
54 
55  template<typename MatrixLike>
56  inline bool isZero(const Eigen::MatrixBase<MatrixLike> & m,
57  const typename MatrixLike::RealScalar & prec =
58  Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
59  {
61  }
62 
63  template<typename M1, typename M2>
65  {
66 #if EIGEN_VERSION_AT_LEAST(3,2,90)
67  typedef typename Eigen::Product<M1,M2> type;
68 #else
69  typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
70 #endif
71  };
72 
73  template<typename Scalar, typename Matrix>
75  {
76 #if EIGEN_VERSION_AT_LEAST(3,3,0)
79 #elif EIGEN_VERSION_AT_LEAST(3,2,90)
80  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
81 #else
82  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
83 #endif
84  };
85 
86  template<typename Matrix, typename Scalar>
88  {
89 #if EIGEN_VERSION_AT_LEAST(3,3,0)
92 #elif EIGEN_VERSION_AT_LEAST(3,2,90)
93  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
94 #else
95  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
96 #endif
97  };
98 
99  namespace internal
100  {
101  template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
103  {
104  typedef typename MatrixLike::Scalar Scalar;
105  typedef typename MatrixLike::RealScalar RealScalar;
106 
107  static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
108  const RealScalar & prec =
109  Eigen::NumTraits< Scalar >::dummy_precision())
110  {
111  return mat.isUnitary(prec);
112  }
113  };
114 
115  template<typename MatrixLike>
116  struct isUnitaryAlgo<MatrixLike,false>
117  {
118  typedef typename MatrixLike::Scalar Scalar;
119  typedef typename MatrixLike::RealScalar RealScalar;
120 
121  static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
122  const RealScalar & prec =
123  Eigen::NumTraits< Scalar >::dummy_precision())
124  {
126  return true;
127  }
128  };
129  }
130 
139  template<typename MatrixLike>
140  inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
141  const typename MatrixLike::RealScalar & prec =
142  Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
143  {
145  }
146 
147  namespace internal
148  {
149  template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
151  {
152  typedef typename VectorLike::Scalar Scalar;
153  typedef typename VectorLike::RealScalar RealScalar;
154 
155  static bool run(const Eigen::MatrixBase<VectorLike> & vec,
156  const RealScalar & prec =
157  Eigen::NumTraits<RealScalar>::dummy_precision())
158  {
159  return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
160  }
161  };
162 
163  template<typename VectorLike>
164  struct isNormalizedAlgo<VectorLike,false>
165  {
166  typedef typename VectorLike::Scalar Scalar;
167  typedef typename VectorLike::RealScalar RealScalar;
168 
169  static bool run(const Eigen::MatrixBase<VectorLike> & /*vec*/,
170  const RealScalar & prec =
171  Eigen::NumTraits<RealScalar>::dummy_precision())
172  {
174  return true;
175  }
176  };
177  }
178 
187  template<typename VectorLike>
188  inline bool isNormalized(const Eigen::MatrixBase<VectorLike> & vec,
189  const typename VectorLike::RealScalar & prec =
190  Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
191  {
192  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
194  }
195 
196  namespace internal
197  {
198  template<typename Scalar>
200  {
201  template<typename MatrixIn, typename MatrixOut>
202  static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
203  const Eigen::MatrixBase<MatrixOut> & dest)
204  {
205  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
206  dest_.noalias() = m_in.inverse();
207  }
208  };
209 
210  }
211 
212  template<typename MatrixIn, typename MatrixOut>
213  inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
214  const Eigen::MatrixBase<MatrixOut> & dest)
215  {
216  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
218  }
219 
220 }
221 
222 #endif //#ifndef __pinocchio_math_matrix_hpp__
static bool run(const Eigen::MatrixBase< MatrixLike > &, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: src/macros.hpp:51
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:56
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec...
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
SE3::Scalar Scalar
Definition: conversions.cpp:13
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
MatrixLike::RealScalar RealScalar
static bool run(const Eigen::MatrixBase< MatrixLike > &, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Definition: math/matrix.hpp:45
static bool run(const Eigen::MatrixBase< MatrixLike > &mat, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Definition: math/matrix.hpp:31
static bool run(const Eigen::MatrixBase< MatrixLike > &mat, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Main pinocchio namespace.
Definition: timings.cpp:30
static bool run(const Eigen::MatrixBase< VectorLike > &vec, const RealScalar &prec=Eigen::NumTraits< RealScalar >::dummy_precision())
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_multiple_op< Scalar >, const Matrix > type
Definition: math/matrix.hpp:95
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_multiple_op< Scalar >, const Matrix > type
Definition: math/matrix.hpp:82
Eigen::ProductReturnType< M1, M2 >::Type type
Definition: math/matrix.hpp:69
static void run(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
static bool run(const Eigen::MatrixBase< VectorLike > &, const RealScalar &prec=Eigen::NumTraits< RealScalar >::dummy_precision())
MatrixLike::RealScalar RealScalar
Definition: math/matrix.hpp:29
bool hasNaN(const Eigen::DenseBase< Derived > &m)
Definition: math/matrix.hpp:18


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04