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__
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...
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.
Main pinocchio namespace.