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>
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>
45 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
46 const RealScalar & prec =
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,
108 const RealScalar & prec =
109 Eigen::NumTraits< Scalar >::dummy_precision())
111 return mat.isUnitary(prec);
115 template<
typename MatrixLike>
121 static bool run(
const Eigen::MatrixBase<MatrixLike> & ,
122 const RealScalar & prec =
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,
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>
169 static bool run(
const Eigen::MatrixBase<VectorLike> & ,
170 const RealScalar & prec =
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__ MatrixLike::Scalar Scalar
MatrixLike::Scalar Scalar
MatrixLike::Scalar Scalar
MatrixLike::RealScalar RealScalar
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.
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
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.
VectorLike::Scalar Scalar
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())
static bool run(const Eigen::MatrixBase< MatrixLike > &mat, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
MatrixLike::RealScalar RealScalar
static bool run(const Eigen::MatrixBase< MatrixLike > &mat, const RealScalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
VectorLike::RealScalar RealScalar
Main pinocchio namespace.
VectorLike::RealScalar RealScalar
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
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_multiple_op< Scalar >, const Matrix > type
Eigen::ProductReturnType< M1, M2 >::Type type
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
MatrixLike::Scalar Scalar
bool hasNaN(const Eigen::DenseBase< Derived > &m)
VectorLike::Scalar Scalar