5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
12 #include <boost/type_traits.hpp>
17 template<
typename Derived>
18 inline bool hasNaN(
const Eigen::DenseBase<Derived> & m)
20 return !((
m.derived().array()==
m.derived().array()).all());
25 template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
31 static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
33 Eigen::NumTraits< Scalar >::dummy_precision())
35 return mat.isZero(prec);
39 template<
typename MatrixLike>
45 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
47 Eigen::NumTraits< Scalar >::dummy_precision())
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())
63 template<
typename M1,
typename M2>
66 #if EIGEN_VERSION_AT_LEAST(3,2,90)
67 typedef typename Eigen::Product<M1,M2>
type;
69 typedef typename Eigen::ProductReturnType<M1,M2>::Type
type;
73 template<
typename Scalar,
typename Matrix>
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;
82 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix>
type;
86 template<
typename Matrix,
typename Scalar>
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;
95 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix>
type;
101 template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
107 static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
109 Eigen::NumTraits< Scalar >::dummy_precision())
111 return mat.isUnitary(prec);
115 template<
typename MatrixLike>
121 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
123 Eigen::NumTraits< Scalar >::dummy_precision())
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())
149 template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
155 static bool run(
const Eigen::MatrixBase<VectorLike> & vec,
157 Eigen::NumTraits<RealScalar>::dummy_precision())
163 template<
typename VectorLike>
169 static bool run(
const Eigen::MatrixBase<VectorLike> & ,
171 Eigen::NumTraits<RealScalar>::dummy_precision())
187 template<
typename VectorLike>
189 const typename VectorLike::RealScalar & prec =
190 Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
192 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
198 template<
typename Scalar>
201 template<
typename MatrixIn,
typename MatrixOut>
202 static void run(
const Eigen::MatrixBase<MatrixIn> & m_in,
203 const Eigen::MatrixBase<MatrixOut> & dest)
206 dest_.noalias() = m_in.inverse();
212 template<
typename MatrixIn,
typename MatrixOut>
213 inline void inverse(
const Eigen::MatrixBase<MatrixIn> & m_in,
214 const Eigen::MatrixBase<MatrixOut> & dest)
222 #endif //#ifndef __pinocchio_math_matrix_hpp__