Program Listing for File matrix.hpp
↰ Return to documentation for file (include/pinocchio/math/matrix.hpp
)
//
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_math_matrix_hpp__
#define __pinocchio_math_matrix_hpp__
#include "pinocchio/macros.hpp"
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/utils/static-if.hpp"
#include <boost/type_traits.hpp>
#include <Eigen/Dense>
namespace pinocchio
{
template<typename Derived>
inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
{
return !((m.derived().array() == m.derived().array()).all());
}
namespace internal
{
template<
typename MatrixLike,
bool value = is_floating_point<typename MatrixLike::Scalar>::value>
struct isZeroAlgo
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
return mat.isZero(prec);
}
};
template<typename MatrixLike>
struct isZeroAlgo<MatrixLike, false>
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<MatrixLike> & /*vec*/,
const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
PINOCCHIO_UNUSED_VARIABLE(prec);
return true;
}
};
} // namespace internal
template<typename MatrixLike>
inline bool isZero(
const Eigen::MatrixBase<MatrixLike> & m,
const typename MatrixLike::RealScalar & prec =
Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
{
return internal::isZeroAlgo<MatrixLike>::run(m, prec);
}
template<typename M1, typename M2>
struct MatrixMatrixProduct
{
#if EIGEN_VERSION_AT_LEAST(3, 2, 90)
typedef typename Eigen::Product<M1, M2> type;
#else
typedef typename Eigen::ProductReturnType<M1, M2>::Type type;
#endif
};
template<typename Scalar, typename Matrix>
struct ScalarMatrixProduct
{
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
typedef Eigen::CwiseBinaryOp<
EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) < Scalar,
typename Eigen::internal::traits<Matrix>::Scalar>,
const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type,
const Matrix > type;
#elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
type;
#endif
};
template<typename Matrix, typename Scalar>
struct MatrixScalarProduct
{
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
typedef Eigen::CwiseBinaryOp<
EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) <
typename Eigen::internal::traits<Matrix>::Scalar,
Scalar>,
const Matrix,
const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type > type;
#elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
type;
#endif
};
namespace internal
{
template<
typename MatrixLike,
bool value = is_floating_point<typename MatrixLike::Scalar>::value>
struct isUnitaryAlgo
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<MatrixLike> & mat,
const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
return mat.isUnitary(prec);
}
};
template<typename MatrixLike>
struct isUnitaryAlgo<MatrixLike, false>
{
typedef typename MatrixLike::Scalar Scalar;
typedef typename MatrixLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<MatrixLike> & /*vec*/,
const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
{
PINOCCHIO_UNUSED_VARIABLE(prec);
return true;
}
};
} // namespace internal
template<typename MatrixLike>
inline bool isUnitary(
const Eigen::MatrixBase<MatrixLike> & mat,
const typename MatrixLike::RealScalar & prec =
Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
{
return internal::isUnitaryAlgo<MatrixLike>::run(mat, prec);
}
namespace internal
{
template<
typename VectorLike,
bool value = is_floating_point<typename VectorLike::Scalar>::value>
struct isNormalizedAlgo
{
typedef typename VectorLike::Scalar Scalar;
typedef typename VectorLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<VectorLike> & vec,
const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
{
return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
}
};
template<typename VectorLike>
struct isNormalizedAlgo<VectorLike, false>
{
typedef typename VectorLike::Scalar Scalar;
typedef typename VectorLike::RealScalar RealScalar;
static bool run(
const Eigen::MatrixBase<VectorLike> & /*vec*/,
const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
{
PINOCCHIO_UNUSED_VARIABLE(prec);
return true;
}
};
} // namespace internal
template<typename VectorLike>
inline bool isNormalized(
const Eigen::MatrixBase<VectorLike> & vec,
const typename VectorLike::RealScalar & prec =
Eigen::NumTraits<typename VectorLike::Scalar>::dummy_precision())
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
return internal::isNormalizedAlgo<VectorLike>::run(vec, prec);
}
namespace internal
{
template<
typename VectorLike,
bool value = is_floating_point<typename VectorLike::Scalar>::value>
struct normalizeAlgo
{
static void run(const Eigen::MatrixBase<VectorLike> & vec)
{
return vec.const_cast_derived().normalize();
}
};
template<typename VectorLike>
struct normalizeAlgo<VectorLike, false>
{
static void run(const Eigen::MatrixBase<VectorLike> & vec)
{
using namespace internal;
typedef typename VectorLike::RealScalar RealScalar;
typedef typename VectorLike::Scalar Scalar;
const RealScalar z = vec.squaredNorm();
const Scalar sqrt_z = if_then_else(GT, z, Scalar(0), math::sqrt(z), Scalar(1));
vec.const_cast_derived() /= sqrt_z;
}
};
} // namespace internal
template<typename VectorLike>
inline void normalize(const Eigen::MatrixBase<VectorLike> & vec)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
internal::normalizeAlgo<VectorLike>::run(vec.const_cast_derived());
}
namespace internal
{
template<typename Scalar>
struct CallCorrectMatrixInverseAccordingToScalar
{
template<typename MatrixIn, typename MatrixOut>
static void
run(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
dest_.noalias() = m_in.inverse();
}
};
} // namespace internal
template<typename MatrixIn, typename MatrixOut>
inline void
inverse(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
{
MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(
m_in, dest_);
}
} // namespace pinocchio
#endif // #ifndef __pinocchio_math_matrix_hpp__