5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/fwd.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>
28 typedef typename MatrixLike::Scalar Scalar;
29 typedef typename MatrixLike::RealScalar RealScalar;
31 static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
32 const RealScalar & prec =
33 Eigen::NumTraits< Scalar >::dummy_precision())
35 return mat.isZero(prec);
39 template<
typename MatrixLike>
40 struct isZeroAlgo<MatrixLike,false>
42 typedef typename MatrixLike::Scalar Scalar;
43 typedef typename MatrixLike::RealScalar RealScalar;
45 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
46 const RealScalar & prec =
47 Eigen::NumTraits< Scalar >::dummy_precision())
49 PINOCCHIO_UNUSED_VARIABLE(prec);
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())
60 return internal::isZeroAlgo<MatrixLike>::run(m,prec);
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)
77 typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<Scalar,
typename Eigen::internal::traits<Matrix>::Scalar>,
78 const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type,
const Matrix> type;
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)
90 typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<
typename Eigen::internal::traits<Matrix>::Scalar,Scalar>,
91 const Matrix,
const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type> type;
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>
104 typedef typename MatrixLike::Scalar Scalar;
105 typedef typename MatrixLike::RealScalar RealScalar;
107 static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
108 const RealScalar & prec =
109 Eigen::NumTraits< Scalar >::dummy_precision())
111 return mat.isUnitary(prec);
115 template<
typename MatrixLike>
116 struct isUnitaryAlgo<MatrixLike,false>
118 typedef typename MatrixLike::Scalar Scalar;
119 typedef typename MatrixLike::RealScalar RealScalar;
121 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
122 const RealScalar & prec =
123 Eigen::NumTraits< Scalar >::dummy_precision())
125 PINOCCHIO_UNUSED_VARIABLE(prec);
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())
144 return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
149 template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
150 struct isNormalizedAlgo
152 typedef typename VectorLike::Scalar Scalar;
153 typedef typename VectorLike::RealScalar RealScalar;
155 static bool run(
const Eigen::MatrixBase<VectorLike> & vec,
156 const RealScalar & prec =
157 Eigen::NumTraits<RealScalar>::dummy_precision())
159 return math::fabs(
static_cast<RealScalar
>(vec.norm() - RealScalar(1))) <= prec;
163 template<
typename VectorLike>
164 struct isNormalizedAlgo<VectorLike,false>
166 typedef typename VectorLike::Scalar Scalar;
167 typedef typename VectorLike::RealScalar RealScalar;
169 static bool run(
const Eigen::MatrixBase<VectorLike> & ,
170 const RealScalar & prec =
171 Eigen::NumTraits<RealScalar>::dummy_precision())
173 PINOCCHIO_UNUSED_VARIABLE(prec);
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);
193 return internal::isNormalizedAlgo<VectorLike>::run(vec,prec);
198 template<
typename Scalar>
199 struct CallCorrectMatrixInverseAccordingToScalar
201 template<
typename MatrixIn,
typename MatrixOut>
202 static void run(
const Eigen::MatrixBase<MatrixIn> & m_in,
203 const Eigen::MatrixBase<MatrixOut> & dest)
205 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(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)
216 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
217 internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
222 #endif //#ifndef __pinocchio_math_matrix_hpp__