5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/fwd.hpp"
10 #include "pinocchio/utils/static-if.hpp"
12 #include <boost/type_traits.hpp>
13 #include <Eigen/Dense>
18 template<
typename Derived>
19 inline bool hasNaN(
const Eigen::DenseBase<Derived> & m)
21 return !((m.derived().array() == m.derived().array()).all());
28 bool value = is_floating_point<typename MatrixLike::Scalar>::value>
31 typedef typename MatrixLike::Scalar Scalar;
32 typedef typename MatrixLike::RealScalar RealScalar;
35 const Eigen::MatrixBase<MatrixLike> & mat,
36 const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
38 return mat.isZero(prec);
42 template<
typename MatrixLike>
43 struct isZeroAlgo<MatrixLike, false>
45 typedef typename MatrixLike::Scalar Scalar;
46 typedef typename MatrixLike::RealScalar RealScalar;
49 const Eigen::MatrixBase<MatrixLike> & ,
50 const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
52 PINOCCHIO_UNUSED_VARIABLE(prec);
58 template<
typename MatrixLike>
60 const Eigen::MatrixBase<MatrixLike> & m,
61 const typename MatrixLike::RealScalar & prec =
62 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
64 return internal::isZeroAlgo<MatrixLike>::run(m, prec);
67 template<
typename M1,
typename M2>
70 #if EIGEN_VERSION_AT_LEAST(3, 2, 90)
71 typedef typename Eigen::Product<M1, M2> type;
73 typedef typename Eigen::ProductReturnType<M1, M2>::Type type;
77 template<
typename Scalar,
typename Matrix>
80 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
81 typedef Eigen::CwiseBinaryOp<
82 EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) < Scalar,
83 typename Eigen::internal::traits<Matrix>::Scalar>,
84 const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type,
86 #elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
87 typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix> type;
89 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix>
94 template<
typename Matrix,
typename Scalar>
97 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
98 typedef Eigen::CwiseBinaryOp<
99 EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) <
100 typename Eigen::internal::traits<Matrix>::Scalar,
103 const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type > type;
104 #elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
105 typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix> type;
107 typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>,
const Matrix>
119 typedef typename MatrixLike::Scalar Scalar;
120 typedef typename MatrixLike::RealScalar RealScalar;
123 const Eigen::MatrixBase<MatrixLike> & mat,
124 const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
126 return mat.isUnitary(prec);
130 template<
typename MatrixLike>
131 struct isUnitaryAlgo<MatrixLike, false>
133 typedef typename MatrixLike::Scalar Scalar;
134 typedef typename MatrixLike::RealScalar RealScalar;
137 const Eigen::MatrixBase<MatrixLike> & ,
138 const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
140 PINOCCHIO_UNUSED_VARIABLE(prec);
154 template<
typename MatrixLike>
156 const Eigen::MatrixBase<MatrixLike> & mat,
157 const typename MatrixLike::RealScalar & prec =
158 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
160 return internal::isUnitaryAlgo<MatrixLike>::run(mat, prec);
167 bool value = is_floating_point<typename VectorLike::Scalar>::value>
168 struct isNormalizedAlgo
170 typedef typename VectorLike::Scalar Scalar;
171 typedef typename VectorLike::RealScalar RealScalar;
174 const Eigen::MatrixBase<VectorLike> & vec,
175 const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
177 return math::fabs(
static_cast<RealScalar
>(vec.norm() - RealScalar(1))) <= prec;
181 template<
typename VectorLike>
182 struct isNormalizedAlgo<VectorLike, false>
184 typedef typename VectorLike::Scalar Scalar;
185 typedef typename VectorLike::RealScalar RealScalar;
188 const Eigen::MatrixBase<VectorLike> & ,
189 const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
191 PINOCCHIO_UNUSED_VARIABLE(prec);
205 template<
typename VectorLike>
207 const Eigen::MatrixBase<VectorLike> & vec,
208 const typename VectorLike::RealScalar & prec =
209 Eigen::NumTraits<typename VectorLike::Scalar>::dummy_precision())
211 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
212 return internal::isNormalizedAlgo<VectorLike>::run(vec, prec);
219 bool value = is_floating_point<typename VectorLike::Scalar>::value>
222 static void run(
const Eigen::MatrixBase<VectorLike> & vec)
224 return vec.const_cast_derived().normalize();
228 template<
typename VectorLike>
229 struct normalizeAlgo<VectorLike, false>
231 static void run(
const Eigen::MatrixBase<VectorLike> & vec)
233 using namespace internal;
234 typedef typename VectorLike::RealScalar RealScalar;
235 typedef typename VectorLike::Scalar Scalar;
236 const RealScalar z = vec.squaredNorm();
237 const Scalar sqrt_z = if_then_else(GT, z, Scalar(0), math::sqrt(z), Scalar(1));
238 vec.const_cast_derived() /= sqrt_z;
248 template<
typename VectorLike>
249 inline void normalize(
const Eigen::MatrixBase<VectorLike> & vec)
251 EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
252 internal::normalizeAlgo<VectorLike>::run(vec.const_cast_derived());
257 template<
typename Scalar>
258 struct CallCorrectMatrixInverseAccordingToScalar
260 template<
typename MatrixIn,
typename MatrixOut>
262 run(
const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
264 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
265 dest_.noalias() = m_in.inverse();
271 template<
typename MatrixIn,
typename MatrixOut>
273 inverse(
const Eigen::MatrixBase<MatrixIn> & m_in,
const Eigen::MatrixBase<MatrixOut> & dest)
275 MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
276 internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(
Main pinocchio namespace.
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.
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.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.