| Directory: | ./ |
|---|---|
| File: | include/pinocchio/math/matrix.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 28 | 35 | 80.0% |
| Branches: | 3 | 22 | 13.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_math_matrix_hpp__ | ||
| 6 | #define __pinocchio_math_matrix_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/macros.hpp" | ||
| 9 | #include "pinocchio/math/fwd.hpp" | ||
| 10 | #include "pinocchio/utils/static-if.hpp" | ||
| 11 | |||
| 12 | #include <boost/type_traits.hpp> | ||
| 13 | #include <Eigen/Dense> | ||
| 14 | |||
| 15 | namespace pinocchio | ||
| 16 | { | ||
| 17 | |||
| 18 | template<typename Derived> | ||
| 19 | 9331 | inline bool hasNaN(const Eigen::DenseBase<Derived> & m) | |
| 20 | { | ||
| 21 |
3/6✓ Branch 4 taken 4667 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4667 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4667 times.
✗ Branch 11 not taken.
|
9331 | return !((m.derived().array() == m.derived().array()).all()); |
| 22 | } | ||
| 23 | |||
| 24 | namespace internal | ||
| 25 | { | ||
| 26 | template< | ||
| 27 | typename MatrixLike, | ||
| 28 | bool value = is_floating_point<typename MatrixLike::Scalar>::value> | ||
| 29 | struct isZeroAlgo | ||
| 30 | { | ||
| 31 | typedef typename MatrixLike::Scalar Scalar; | ||
| 32 | typedef typename MatrixLike::RealScalar RealScalar; | ||
| 33 | |||
| 34 | 6545 | static bool run( | |
| 35 | const Eigen::MatrixBase<MatrixLike> & mat, | ||
| 36 | const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) | ||
| 37 | { | ||
| 38 | 6545 | return mat.isZero(prec); | |
| 39 | } | ||
| 40 | }; | ||
| 41 | |||
| 42 | template<typename MatrixLike> | ||
| 43 | struct isZeroAlgo<MatrixLike, false> | ||
| 44 | { | ||
| 45 | typedef typename MatrixLike::Scalar Scalar; | ||
| 46 | typedef typename MatrixLike::RealScalar RealScalar; | ||
| 47 | |||
| 48 | 1 | static bool run( | |
| 49 | const Eigen::MatrixBase<MatrixLike> & /*vec*/, | ||
| 50 | const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) | ||
| 51 | { | ||
| 52 | PINOCCHIO_UNUSED_VARIABLE(prec); | ||
| 53 | 1 | return true; | |
| 54 | } | ||
| 55 | }; | ||
| 56 | } // namespace internal | ||
| 57 | |||
| 58 | template<typename MatrixLike> | ||
| 59 | 6546 | inline bool isZero( | |
| 60 | const Eigen::MatrixBase<MatrixLike> & m, | ||
| 61 | const typename MatrixLike::RealScalar & prec = | ||
| 62 | Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) | ||
| 63 | { | ||
| 64 | 6546 | return internal::isZeroAlgo<MatrixLike>::run(m, prec); | |
| 65 | } | ||
| 66 | |||
| 67 | template<typename M1, typename M2> | ||
| 68 | struct MatrixMatrixProduct | ||
| 69 | { | ||
| 70 | #if EIGEN_VERSION_AT_LEAST(3, 2, 90) | ||
| 71 | typedef typename Eigen::Product<M1, M2> type; | ||
| 72 | #else | ||
| 73 | typedef typename Eigen::ProductReturnType<M1, M2>::Type type; | ||
| 74 | #endif | ||
| 75 | }; | ||
| 76 | |||
| 77 | template<typename Scalar, typename Matrix> | ||
| 78 | struct ScalarMatrixProduct | ||
| 79 | { | ||
| 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, | ||
| 85 | const Matrix > type; | ||
| 86 | #elif EIGEN_VERSION_AT_LEAST(3, 2, 90) | ||
| 87 | typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type; | ||
| 88 | #else | ||
| 89 | typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> | ||
| 90 | type; | ||
| 91 | #endif | ||
| 92 | }; | ||
| 93 | |||
| 94 | template<typename Matrix, typename Scalar> | ||
| 95 | struct MatrixScalarProduct | ||
| 96 | { | ||
| 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, | ||
| 101 | Scalar>, | ||
| 102 | const Matrix, | ||
| 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; | ||
| 106 | #else | ||
| 107 | typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> | ||
| 108 | type; | ||
| 109 | #endif | ||
| 110 | }; | ||
| 111 | |||
| 112 | namespace internal | ||
| 113 | { | ||
| 114 | template< | ||
| 115 | typename MatrixLike, | ||
| 116 | bool value = is_floating_point<typename MatrixLike::Scalar>::value> | ||
| 117 | struct isUnitaryAlgo | ||
| 118 | { | ||
| 119 | typedef typename MatrixLike::Scalar Scalar; | ||
| 120 | typedef typename MatrixLike::RealScalar RealScalar; | ||
| 121 | |||
| 122 | 618599 | static bool run( | |
| 123 | const Eigen::MatrixBase<MatrixLike> & mat, | ||
| 124 | const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) | ||
| 125 | { | ||
| 126 | 618599 | return mat.isUnitary(prec); | |
| 127 | } | ||
| 128 | }; | ||
| 129 | |||
| 130 | template<typename MatrixLike> | ||
| 131 | struct isUnitaryAlgo<MatrixLike, false> | ||
| 132 | { | ||
| 133 | typedef typename MatrixLike::Scalar Scalar; | ||
| 134 | typedef typename MatrixLike::RealScalar RealScalar; | ||
| 135 | |||
| 136 | 252 | static bool run( | |
| 137 | const Eigen::MatrixBase<MatrixLike> & /*vec*/, | ||
| 138 | const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) | ||
| 139 | { | ||
| 140 | PINOCCHIO_UNUSED_VARIABLE(prec); | ||
| 141 | 252 | return true; | |
| 142 | } | ||
| 143 | }; | ||
| 144 | } // namespace internal | ||
| 145 | |||
| 146 | /// | ||
| 147 | /// \brief Check whether the input matrix is Unitary within the given precision. | ||
| 148 | /// | ||
| 149 | /// \param[in] mat Input matrix | ||
| 150 | /// \param[in] prec Required precision | ||
| 151 | /// | ||
| 152 | /// \returns true if mat is unitary within the precision prec | ||
| 153 | /// | ||
| 154 | template<typename MatrixLike> | ||
| 155 | 414778 | inline bool isUnitary( | |
| 156 | const Eigen::MatrixBase<MatrixLike> & mat, | ||
| 157 | const typename MatrixLike::RealScalar & prec = | ||
| 158 | Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) | ||
| 159 | { | ||
| 160 | 414778 | return internal::isUnitaryAlgo<MatrixLike>::run(mat, prec); | |
| 161 | } | ||
| 162 | |||
| 163 | namespace internal | ||
| 164 | { | ||
| 165 | template< | ||
| 166 | typename VectorLike, | ||
| 167 | bool value = is_floating_point<typename VectorLike::Scalar>::value> | ||
| 168 | struct isNormalizedAlgo | ||
| 169 | { | ||
| 170 | typedef typename VectorLike::Scalar Scalar; | ||
| 171 | typedef typename VectorLike::RealScalar RealScalar; | ||
| 172 | |||
| 173 | 208072 | static bool run( | |
| 174 | const Eigen::MatrixBase<VectorLike> & vec, | ||
| 175 | const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision()) | ||
| 176 | { | ||
| 177 | 208072 | return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec; | |
| 178 | } | ||
| 179 | }; | ||
| 180 | |||
| 181 | template<typename VectorLike> | ||
| 182 | struct isNormalizedAlgo<VectorLike, false> | ||
| 183 | { | ||
| 184 | typedef typename VectorLike::Scalar Scalar; | ||
| 185 | typedef typename VectorLike::RealScalar RealScalar; | ||
| 186 | |||
| 187 | 1694 | static bool run( | |
| 188 | const Eigen::MatrixBase<VectorLike> & /*vec*/, | ||
| 189 | const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision()) | ||
| 190 | { | ||
| 191 | PINOCCHIO_UNUSED_VARIABLE(prec); | ||
| 192 | 1694 | return true; | |
| 193 | } | ||
| 194 | }; | ||
| 195 | } // namespace internal | ||
| 196 | |||
| 197 | /// | ||
| 198 | /// \brief Check whether the input vector is Normalized within the given precision. | ||
| 199 | /// | ||
| 200 | /// \param[in] vec Input vector | ||
| 201 | /// \param[in] prec Required precision | ||
| 202 | /// | ||
| 203 | /// \returns true if vec is normalized within the precision prec. | ||
| 204 | /// | ||
| 205 | template<typename VectorLike> | ||
| 206 | 208407 | inline bool isNormalized( | |
| 207 | const Eigen::MatrixBase<VectorLike> & vec, | ||
| 208 | const typename VectorLike::RealScalar & prec = | ||
| 209 | Eigen::NumTraits<typename VectorLike::Scalar>::dummy_precision()) | ||
| 210 | { | ||
| 211 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); | ||
| 212 | 208407 | return internal::isNormalizedAlgo<VectorLike>::run(vec, prec); | |
| 213 | } | ||
| 214 | |||
| 215 | namespace internal | ||
| 216 | { | ||
| 217 | template< | ||
| 218 | typename VectorLike, | ||
| 219 | bool value = is_floating_point<typename VectorLike::Scalar>::value> | ||
| 220 | struct normalizeAlgo | ||
| 221 | { | ||
| 222 | 12420 | static void run(const Eigen::MatrixBase<VectorLike> & vec) | |
| 223 | { | ||
| 224 | 12420 | return vec.const_cast_derived().normalize(); | |
| 225 | } | ||
| 226 | }; | ||
| 227 | |||
| 228 | template<typename VectorLike> | ||
| 229 | struct normalizeAlgo<VectorLike, false> | ||
| 230 | { | ||
| 231 | ✗ | static void run(const Eigen::MatrixBase<VectorLike> & vec) | |
| 232 | { | ||
| 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; | |
| 239 | } | ||
| 240 | }; | ||
| 241 | } // namespace internal | ||
| 242 | |||
| 243 | /// | ||
| 244 | /// \brief Normalize the input vector. | ||
| 245 | /// | ||
| 246 | /// \param[in] vec Input vector | ||
| 247 | /// | ||
| 248 | template<typename VectorLike> | ||
| 249 | 12420 | inline void normalize(const Eigen::MatrixBase<VectorLike> & vec) | |
| 250 | { | ||
| 251 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); | ||
| 252 | 12420 | internal::normalizeAlgo<VectorLike>::run(vec.const_cast_derived()); | |
| 253 | 12420 | } | |
| 254 | |||
| 255 | namespace internal | ||
| 256 | { | ||
| 257 | template<typename Scalar> | ||
| 258 | struct CallCorrectMatrixInverseAccordingToScalar | ||
| 259 | { | ||
| 260 | template<typename MatrixIn, typename MatrixOut> | ||
| 261 | static void | ||
| 262 | ✗ | run(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest) | |
| 263 | { | ||
| 264 | ✗ | MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest); | |
| 265 | ✗ | dest_.noalias() = m_in.inverse(); | |
| 266 | } | ||
| 267 | }; | ||
| 268 | |||
| 269 | } // namespace internal | ||
| 270 | |||
| 271 | template<typename MatrixIn, typename MatrixOut> | ||
| 272 | inline void | ||
| 273 | 220 | inverse(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest) | |
| 274 | { | ||
| 275 | 220 | MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest); | |
| 276 | 220 | internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run( | |
| 277 | m_in, dest_); | ||
| 278 | } | ||
| 279 | |||
| 280 | } // namespace pinocchio | ||
| 281 | |||
| 282 | #endif // #ifndef __pinocchio_math_matrix_hpp__ | ||
| 283 |