5 #ifndef __pinocchio_math_matrix_hpp__ 6 #define __pinocchio_math_matrix_hpp__ 9 #include <boost/type_traits.hpp> 14 template<
typename Derived>
15 inline bool hasNaN(
const Eigen::DenseBase<Derived> & m)
17 return !((m.derived().array()==m.derived().array()).all());
20 template<
typename M1,
typename M2>
23 #if EIGEN_VERSION_AT_LEAST(3,2,90) 24 typedef typename Eigen::Product<M1,M2> type;
26 typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
30 template<
typename Scalar,
typename Matrix>
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;
39 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix> type;
43 template<
typename Matrix,
typename Scalar>
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;
52 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix> type;
58 template<typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
61 typedef typename MatrixLike::Scalar Scalar;
62 typedef typename MatrixLike::RealScalar RealScalar;
64 static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
65 const RealScalar & prec =
66 Eigen::NumTraits< Scalar >::dummy_precision())
68 return mat.isUnitary(prec);
72 template<
typename MatrixLike>
73 struct isUnitaryAlgo<MatrixLike,false>
75 typedef typename MatrixLike::Scalar Scalar;
76 typedef typename MatrixLike::RealScalar RealScalar;
78 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
79 const RealScalar & prec =
80 Eigen::NumTraits< Scalar >::dummy_precision())
82 PINOCCHIO_UNUSED_VARIABLE(prec);
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())
93 return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
98 template<
typename Scalar>
99 struct CallCorrectMatrixInverseAccordingToScalar
101 template<
typename MatrixIn,
typename MatrixOut>
102 static void run(
const Eigen::MatrixBase<MatrixIn> & m_in,
103 const Eigen::MatrixBase<MatrixOut> & dest)
105 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
106 dest_.noalias() = m_in.inverse();
112 template<
typename MatrixIn,
typename MatrixOut>
113 inline void inverse(
const Eigen::MatrixBase<MatrixIn> & m_in,
114 const Eigen::MatrixBase<MatrixOut> & dest)
116 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
117 internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
122 #endif //#ifndef __pinocchio_math_matrix_hpp__
Main pinocchio namespace.