| Directory: | ./ |
|---|---|
| File: | include/pinocchio/math/tridiagonal-matrix.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 162 | 172 | 94.2% |
| Branches: | 88 | 372 | 23.7% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2024 INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_math_tridiagonal_matrix_hpp__ | ||
| 6 | #define __pinocchio_math_tridiagonal_matrix_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/fwd.hpp" | ||
| 9 | #include <Eigen/Dense> | ||
| 10 | |||
| 11 | namespace pinocchio | ||
| 12 | { | ||
| 13 | template<typename Scalar, int Options = 0> | ||
| 14 | struct TridiagonalSymmetricMatrixTpl; | ||
| 15 | |||
| 16 | template<typename _Scalar, int _Options> | ||
| 17 | struct traits<TridiagonalSymmetricMatrixTpl<_Scalar, _Options>> | ||
| 18 | { | ||
| 19 | typedef _Scalar Scalar; | ||
| 20 | enum | ||
| 21 | { | ||
| 22 | Options = _Options | ||
| 23 | }; | ||
| 24 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> PlainMatrixType; | ||
| 25 | }; | ||
| 26 | |||
| 27 | template<typename TridiagonalSymmetricMatrix, typename MatrixDerived> | ||
| 28 | struct TridiagonalSymmetricMatrixApplyOnTheRightReturnType; | ||
| 29 | |||
| 30 | template<typename TridiagonalSymmetricMatrix, typename MatrixDerived> | ||
| 31 | struct traits< | ||
| 32 | TridiagonalSymmetricMatrixApplyOnTheRightReturnType<TridiagonalSymmetricMatrix, MatrixDerived>> | ||
| 33 | : public traits<TridiagonalSymmetricMatrix> | ||
| 34 | { | ||
| 35 | }; | ||
| 36 | |||
| 37 | template<typename MatrixDerived, typename TridiagonalSymmetricMatrix> | ||
| 38 | struct TridiagonalSymmetricMatrixApplyOnTheLeftReturnType; | ||
| 39 | |||
| 40 | template<typename MatrixDerived, typename TridiagonalSymmetricMatrix> | ||
| 41 | struct traits< | ||
| 42 | TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<MatrixDerived, TridiagonalSymmetricMatrix>> | ||
| 43 | : public traits<TridiagonalSymmetricMatrix> | ||
| 44 | { | ||
| 45 | }; | ||
| 46 | |||
| 47 | template<typename TridiagonalSymmetricMatrix> | ||
| 48 | struct TridiagonalSymmetricMatrixInverse; | ||
| 49 | |||
| 50 | template<typename TridiagonalSymmetricMatrix> | ||
| 51 | struct traits<TridiagonalSymmetricMatrixInverse<TridiagonalSymmetricMatrix>> | ||
| 52 | : public traits<TridiagonalSymmetricMatrix> | ||
| 53 | { | ||
| 54 | }; | ||
| 55 | |||
| 56 | template<typename TridiagonalSymmetricMatrixInverse, typename MatrixDerived> | ||
| 57 | struct TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType; | ||
| 58 | |||
| 59 | template<typename TridiagonalSymmetricMatrixInverse, typename MatrixDerived> | ||
| 60 | struct traits<TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType< | ||
| 61 | TridiagonalSymmetricMatrixInverse, | ||
| 62 | MatrixDerived>> : public traits<TridiagonalSymmetricMatrixInverse> | ||
| 63 | { | ||
| 64 | }; | ||
| 65 | } // namespace pinocchio | ||
| 66 | |||
| 67 | namespace Eigen | ||
| 68 | { | ||
| 69 | namespace internal | ||
| 70 | { | ||
| 71 | |||
| 72 | template<typename Scalar, int Options> | ||
| 73 | struct traits<pinocchio::TridiagonalSymmetricMatrixTpl<Scalar, Options>> | ||
| 74 | : public traits<typename pinocchio::traits< | ||
| 75 | pinocchio::TridiagonalSymmetricMatrixTpl<Scalar, Options>>::PlainMatrixType> | ||
| 76 | { | ||
| 77 | typedef pinocchio::traits<pinocchio::TridiagonalSymmetricMatrixTpl<Scalar, Options>> Base; | ||
| 78 | typedef typename Base::PlainMatrixType ReturnType; | ||
| 79 | enum | ||
| 80 | { | ||
| 81 | Flags = 0 | ||
| 82 | }; | ||
| 83 | }; | ||
| 84 | |||
| 85 | template<typename TridiagonalSymmetricMatrix, typename MatrixDerived> | ||
| 86 | struct traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheRightReturnType< | ||
| 87 | TridiagonalSymmetricMatrix, | ||
| 88 | MatrixDerived>> | ||
| 89 | : public traits< | ||
| 90 | typename pinocchio::traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheRightReturnType< | ||
| 91 | TridiagonalSymmetricMatrix, | ||
| 92 | MatrixDerived>>::PlainMatrixType> | ||
| 93 | { | ||
| 94 | typedef pinocchio::traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheRightReturnType< | ||
| 95 | TridiagonalSymmetricMatrix, | ||
| 96 | MatrixDerived>> | ||
| 97 | Base; | ||
| 98 | typedef typename Base::PlainMatrixType ReturnType; | ||
| 99 | enum | ||
| 100 | { | ||
| 101 | Flags = 0 | ||
| 102 | }; | ||
| 103 | }; | ||
| 104 | |||
| 105 | template<typename MatrixDerived, typename TridiagonalSymmetricMatrix> | ||
| 106 | struct traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< | ||
| 107 | MatrixDerived, | ||
| 108 | TridiagonalSymmetricMatrix>> | ||
| 109 | : public traits< | ||
| 110 | typename pinocchio::traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< | ||
| 111 | MatrixDerived, | ||
| 112 | TridiagonalSymmetricMatrix>>::PlainMatrixType> | ||
| 113 | { | ||
| 114 | typedef pinocchio::traits<pinocchio::TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< | ||
| 115 | MatrixDerived, | ||
| 116 | TridiagonalSymmetricMatrix>> | ||
| 117 | Base; | ||
| 118 | typedef typename Base::PlainMatrixType ReturnType; | ||
| 119 | enum | ||
| 120 | { | ||
| 121 | Flags = 0 | ||
| 122 | }; | ||
| 123 | }; | ||
| 124 | |||
| 125 | template<typename TridiagonalSymmetricMatrix> | ||
| 126 | struct traits<pinocchio::TridiagonalSymmetricMatrixInverse<TridiagonalSymmetricMatrix>> | ||
| 127 | : public traits<TridiagonalSymmetricMatrix> | ||
| 128 | { | ||
| 129 | }; | ||
| 130 | |||
| 131 | template<typename TridiagonalSymmetricMatrixInverse, typename MatrixDerived> | ||
| 132 | struct traits<pinocchio::TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType< | ||
| 133 | TridiagonalSymmetricMatrixInverse, | ||
| 134 | MatrixDerived>> | ||
| 135 | : public traits<typename pinocchio::traits< | ||
| 136 | pinocchio::TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType< | ||
| 137 | TridiagonalSymmetricMatrixInverse, | ||
| 138 | MatrixDerived>>::PlainMatrixType> | ||
| 139 | { | ||
| 140 | typedef pinocchio::traits< | ||
| 141 | pinocchio::TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType< | ||
| 142 | TridiagonalSymmetricMatrixInverse, | ||
| 143 | MatrixDerived>> | ||
| 144 | Base; | ||
| 145 | typedef typename Base::PlainMatrixType ReturnType; | ||
| 146 | enum | ||
| 147 | { | ||
| 148 | Flags = 0 | ||
| 149 | }; | ||
| 150 | }; | ||
| 151 | |||
| 152 | } // namespace internal | ||
| 153 | } // namespace Eigen | ||
| 154 | |||
| 155 | namespace pinocchio | ||
| 156 | { | ||
| 157 | |||
| 158 | /// \brief Dynamic size Tridiagonal symmetric matrix type | ||
| 159 | /// This class is in practice used in Lanczos decomposition | ||
| 160 | template<typename _Scalar, int _Options> | ||
| 161 | struct TridiagonalSymmetricMatrixTpl | ||
| 162 | : public Eigen::ReturnByValue<TridiagonalSymmetricMatrixTpl<_Scalar, _Options>> | ||
| 163 | { | ||
| 164 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 165 | typedef TridiagonalSymmetricMatrixTpl Self; | ||
| 166 | typedef _Scalar Scalar; | ||
| 167 | enum | ||
| 168 | { | ||
| 169 | Options = _Options | ||
| 170 | }; | ||
| 171 | |||
| 172 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> CoeffVectorType; | ||
| 173 | typedef typename traits<Self>::PlainMatrixType PlainMatrixType; | ||
| 174 | |||
| 175 | /// \brief Default constructor from a given size | ||
| 176 | 4 | explicit TridiagonalSymmetricMatrixTpl(const Eigen::DenseIndex size) | |
| 177 | 4 | : m_size(size) | |
| 178 | 4 | , m_diagonal(size) | |
| 179 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | , m_sub_diagonal(size - 1) |
| 180 | { | ||
| 181 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
4 | assert(size > 0 && "size should be greater than 0."); |
| 182 | 4 | } | |
| 183 | |||
| 184 | ✗ | bool operator==(const TridiagonalSymmetricMatrixTpl & other) const | |
| 185 | { | ||
| 186 | ✗ | if (this == &other) | |
| 187 | ✗ | return true; | |
| 188 | ✗ | return diagonal() == other.diagonal() && subDiagonal() == other.subDiagonal(); | |
| 189 | } | ||
| 190 | |||
| 191 | ✗ | bool operator!=(const TridiagonalSymmetricMatrixTpl & other) const | |
| 192 | { | ||
| 193 | ✗ | return !(*this == other); | |
| 194 | } | ||
| 195 | |||
| 196 | 1 | TridiagonalSymmetricMatrixInverse<Self> inverse() const | |
| 197 | { | ||
| 198 | 1 | return TridiagonalSymmetricMatrixInverse<Self>(*this); | |
| 199 | } | ||
| 200 | |||
| 201 | 8 | CoeffVectorType & diagonal() | |
| 202 | { | ||
| 203 | 8 | return m_diagonal; | |
| 204 | } | ||
| 205 | 7009 | const CoeffVectorType & diagonal() const | |
| 206 | { | ||
| 207 | 7009 | return m_diagonal; | |
| 208 | } | ||
| 209 | 9 | CoeffVectorType & subDiagonal() | |
| 210 | { | ||
| 211 | 9 | return m_sub_diagonal; | |
| 212 | } | ||
| 213 | 14020 | const CoeffVectorType & subDiagonal() const | |
| 214 | { | ||
| 215 | 14020 | return m_sub_diagonal; | |
| 216 | } | ||
| 217 | |||
| 218 | 1 | void setIdentity() | |
| 219 | { | ||
| 220 | 1 | diagonal().setOnes(); | |
| 221 | 1 | subDiagonal().setZero(); | |
| 222 | 1 | } | |
| 223 | |||
| 224 | 1 | bool isIdentity(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 225 | { | ||
| 226 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | return subDiagonal().isZero(prec) && diagonal().isOnes(prec); |
| 227 | } | ||
| 228 | |||
| 229 | 1 | void setZero() | |
| 230 | { | ||
| 231 | 1 | diagonal().setZero(); | |
| 232 | 1 | subDiagonal().setZero(); | |
| 233 | 1 | } | |
| 234 | |||
| 235 | 1 | bool isZero(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 236 | { | ||
| 237 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | return subDiagonal().isZero(prec) && diagonal().isZero(prec); |
| 238 | } | ||
| 239 | |||
| 240 | 2 | void setRandom() | |
| 241 | { | ||
| 242 | 2 | diagonal().setRandom(); | |
| 243 | 2 | subDiagonal().setRandom(); | |
| 244 | 2 | } | |
| 245 | |||
| 246 | 2 | bool isDiagonal(const Scalar prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 247 | { | ||
| 248 | 2 | return subDiagonal().isZero(prec); | |
| 249 | } | ||
| 250 | |||
| 251 | template<typename VectorCoeffType> | ||
| 252 | ✗ | void setDiagonal(const Eigen::MatrixBase<VectorCoeffType> & diagonal_coefficients) | |
| 253 | { | ||
| 254 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(diagonal_coefficients.size(), cols()); | |
| 255 | static_assert( | ||
| 256 | VectorCoeffType::IsVectorAtCompileTime, | ||
| 257 | "VectorCoeffType should be a vector type at compile time"); | ||
| 258 | |||
| 259 | ✗ | diagonal() = diagonal_coefficients; | |
| 260 | ✗ | subDiagonal().setZero(); | |
| 261 | } | ||
| 262 | |||
| 263 | 12012 | EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT | |
| 264 | { | ||
| 265 | 12012 | return m_size; | |
| 266 | } | ||
| 267 | 7008 | EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT | |
| 268 | { | ||
| 269 | 7008 | return m_size; | |
| 270 | } | ||
| 271 | |||
| 272 | 1002 | PlainMatrixType matrix() const | |
| 273 | { | ||
| 274 | 1002 | return PlainMatrixType(*this); | |
| 275 | } | ||
| 276 | |||
| 277 | template<typename ResultType> | ||
| 278 | 3005 | inline void evalTo(ResultType & result) const | |
| 279 | { | ||
| 280 | 3005 | result.setZero(); | |
| 281 |
1/2✓ Branch 4 taken 3005 times.
✗ Branch 5 not taken.
|
3005 | result.template diagonal<1>() = subDiagonal().conjugate(); |
| 282 |
1/2✓ Branch 3 taken 3005 times.
✗ Branch 4 not taken.
|
3005 | result.diagonal() = diagonal(); |
| 283 |
1/2✓ Branch 3 taken 3005 times.
✗ Branch 4 not taken.
|
3005 | result.template diagonal<-1>() = subDiagonal(); |
| 284 | 3005 | } | |
| 285 | |||
| 286 | template<typename MatrixDerived> | ||
| 287 | TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived> | ||
| 288 | 6002 | applyOnTheRight(const Eigen::MatrixBase<MatrixDerived> & mat) const | |
| 289 | { | ||
| 290 | typedef TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived> ReturnType; | ||
| 291 | 6002 | return ReturnType(*this, mat.derived()); | |
| 292 | } | ||
| 293 | |||
| 294 | template<typename MatrixDerived> | ||
| 295 | TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<MatrixDerived, Self> | ||
| 296 | 1000 | applyOnTheLeft(const Eigen::MatrixBase<MatrixDerived> & mat) const | |
| 297 | { | ||
| 298 | typedef TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<MatrixDerived, Self> ReturnType; | ||
| 299 | 1000 | return ReturnType(mat.derived(), *this); | |
| 300 | } | ||
| 301 | |||
| 302 | template<typename MatrixDerived> | ||
| 303 | inline TridiagonalSymmetricMatrixApplyOnTheRightReturnType<Self, MatrixDerived> | ||
| 304 | 6002 | operator*(const Eigen::MatrixBase<MatrixDerived> & mat) const | |
| 305 | { | ||
| 306 | 6002 | return this->applyOnTheRight(mat.derived()); | |
| 307 | } | ||
| 308 | |||
| 309 | protected: | ||
| 310 | Eigen::DenseIndex m_size; | ||
| 311 | CoeffVectorType m_diagonal; | ||
| 312 | CoeffVectorType m_sub_diagonal; | ||
| 313 | }; | ||
| 314 | |||
| 315 | template<typename LhsMatrixType, typename S, int O> | ||
| 316 | TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< | ||
| 317 | LhsMatrixType, | ||
| 318 | TridiagonalSymmetricMatrixTpl<S, O>> | ||
| 319 | 1000 | operator*( | |
| 320 | const Eigen::MatrixBase<LhsMatrixType> & lhs, const TridiagonalSymmetricMatrixTpl<S, O> & rhs) | ||
| 321 | { | ||
| 322 | 1000 | return rhs.applyOnTheLeft(lhs); | |
| 323 | } | ||
| 324 | |||
| 325 | template<typename TridiagonalSymmetricMatrix, typename RhsMatrixType> | ||
| 326 | struct TridiagonalSymmetricMatrixApplyOnTheRightReturnType | ||
| 327 | : public Eigen::ReturnByValue<TridiagonalSymmetricMatrixApplyOnTheRightReturnType< | ||
| 328 | TridiagonalSymmetricMatrix, | ||
| 329 | RhsMatrixType>> | ||
| 330 | { | ||
| 331 | typedef TridiagonalSymmetricMatrixApplyOnTheRightReturnType Self; | ||
| 332 | typedef typename traits<Self>::PlainMatrixType PlainMatrixType; | ||
| 333 | |||
| 334 | 6002 | TridiagonalSymmetricMatrixApplyOnTheRightReturnType( | |
| 335 | const TridiagonalSymmetricMatrix & lhs, const RhsMatrixType & rhs) | ||
| 336 | 6002 | : m_lhs(lhs) | |
| 337 | 6002 | , m_rhs(rhs) | |
| 338 | { | ||
| 339 | 6002 | } | |
| 340 | |||
| 341 | template<typename ResultType> | ||
| 342 | 6002 | inline void evalTo(ResultType & result) const | |
| 343 | { | ||
| 344 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 3001 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
6002 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.rows(), rows()); |
| 345 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 3001 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
6002 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.cols(), cols()); |
| 346 | |||
| 347 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3001 times.
|
6002 | assert(cols() >= 1); |
| 348 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3001 times.
|
6002 | assert(rows() >= 1); |
| 349 | |||
| 350 | 6002 | const Eigen::DenseIndex reduced_size = cols() - 1; | |
| 351 | // Main diagonal | ||
| 352 |
3/6✓ Branch 3 taken 3001 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3001 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3001 times.
✗ Branch 10 not taken.
|
6002 | result.noalias() = m_lhs.diagonal().asDiagonal() * m_rhs; |
| 353 | // Upper diagonal | ||
| 354 |
4/8✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
|
6002 | result.topRows(reduced_size).noalias() += |
| 355 |
3/6✓ Branch 3 taken 3001 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3001 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2001 times.
✗ Branch 10 not taken.
|
6002 | m_lhs.subDiagonal().conjugate().asDiagonal() * m_rhs.bottomRows(reduced_size); |
| 356 | // Sub diagonal | ||
| 357 |
4/8✓ Branch 1 taken 3001 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3001 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3001 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
|
6002 | result.bottomRows(reduced_size).noalias() += |
| 358 |
2/4✓ Branch 3 taken 3001 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2001 times.
✗ Branch 7 not taken.
|
6002 | m_lhs.subDiagonal().asDiagonal() * m_rhs.topRows(reduced_size); |
| 359 | 6002 | } | |
| 360 | |||
| 361 | 18006 | EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT | |
| 362 | { | ||
| 363 | 18006 | return m_lhs.rows(); | |
| 364 | } | ||
| 365 | 12004 | EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT | |
| 366 | { | ||
| 367 | 24008 | return m_rhs.cols(); | |
| 368 | } | ||
| 369 | |||
| 370 | protected: | ||
| 371 | const TridiagonalSymmetricMatrix & m_lhs; | ||
| 372 | const RhsMatrixType & m_rhs; | ||
| 373 | }; | ||
| 374 | |||
| 375 | template<typename LhsMatrixType, typename TridiagonalSymmetricMatrix> | ||
| 376 | struct TridiagonalSymmetricMatrixApplyOnTheLeftReturnType | ||
| 377 | : public Eigen::ReturnByValue< | ||
| 378 | TridiagonalSymmetricMatrixApplyOnTheLeftReturnType<LhsMatrixType, TridiagonalSymmetricMatrix>> | ||
| 379 | { | ||
| 380 | typedef TridiagonalSymmetricMatrixApplyOnTheLeftReturnType Self; | ||
| 381 | typedef typename traits<Self>::PlainMatrixType PlainMatrixType; | ||
| 382 | |||
| 383 | 1000 | TridiagonalSymmetricMatrixApplyOnTheLeftReturnType( | |
| 384 | const LhsMatrixType & lhs, const TridiagonalSymmetricMatrix & rhs) | ||
| 385 | 1000 | : m_lhs(lhs) | |
| 386 | 1000 | , m_rhs(rhs) | |
| 387 | { | ||
| 388 | 1000 | } | |
| 389 | |||
| 390 | template<typename ResultType> | ||
| 391 | 1000 | inline void evalTo(ResultType & result) const | |
| 392 | { | ||
| 393 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1000 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.rows(), rows()); |
| 394 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1000 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.cols(), cols()); |
| 395 | |||
| 396 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1000 times.
|
1000 | assert(cols() >= 1); |
| 397 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1000 times.
|
1000 | assert(rows() >= 1); |
| 398 | |||
| 399 | 1000 | const Eigen::DenseIndex reduced_size = cols() - 1; | |
| 400 | // Main diagonal | ||
| 401 |
3/6✓ Branch 3 taken 1000 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1000 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1000 times.
✗ Branch 10 not taken.
|
1000 | result.noalias() = m_lhs * m_rhs.diagonal().asDiagonal(); |
| 402 | // Upper diagonal | ||
| 403 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | result.rightCols(reduced_size).noalias() += |
| 404 |
2/4✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | m_lhs.leftCols(reduced_size) * m_rhs.subDiagonal().conjugate().asDiagonal(); |
| 405 | // Sub diagonal | ||
| 406 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | result.leftCols(reduced_size).noalias() += |
| 407 |
2/4✓ Branch 3 taken 1000 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1000 times.
✗ Branch 7 not taken.
|
1000 | m_lhs.rightCols(reduced_size) * m_rhs.subDiagonal().asDiagonal(); |
| 408 | 1000 | } | |
| 409 | |||
| 410 | 3000 | EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT | |
| 411 | { | ||
| 412 | 3000 | return m_lhs.rows(); | |
| 413 | } | ||
| 414 | 4000 | EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT | |
| 415 | { | ||
| 416 | 4000 | return m_rhs.cols(); | |
| 417 | } | ||
| 418 | |||
| 419 | protected: | ||
| 420 | const LhsMatrixType & m_lhs; | ||
| 421 | const TridiagonalSymmetricMatrix & m_rhs; | ||
| 422 | }; | ||
| 423 | |||
| 424 | template<typename _TridiagonalSymmetricMatrix> | ||
| 425 | struct TridiagonalSymmetricMatrixInverse | ||
| 426 | : public Eigen::ReturnByValue<TridiagonalSymmetricMatrixInverse<_TridiagonalSymmetricMatrix>> | ||
| 427 | { | ||
| 428 | typedef _TridiagonalSymmetricMatrix TridiagonalSymmetricMatrix; | ||
| 429 | typedef TridiagonalSymmetricMatrixInverse Self; | ||
| 430 | typedef typename TridiagonalSymmetricMatrix::Scalar Scalar; | ||
| 431 | enum | ||
| 432 | { | ||
| 433 | Options = TridiagonalSymmetricMatrix::Options | ||
| 434 | }; | ||
| 435 | |||
| 436 | typedef typename TridiagonalSymmetricMatrix::CoeffVectorType CoeffVectorType; | ||
| 437 | typedef typename traits<Self>::PlainMatrixType PlainMatrixType; | ||
| 438 | |||
| 439 | 1 | explicit TridiagonalSymmetricMatrixInverse( | |
| 440 | const TridiagonalSymmetricMatrix & tridiagonal_matrix) | ||
| 441 | 1 | : tridiagonal_matrix(tridiagonal_matrix) | |
| 442 | 1 | , m_size(tridiagonal_matrix.rows()) | |
| 443 | 1 | , m_diagonal(m_size) | |
| 444 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | , m_sub_diagonal(m_size - 1) |
| 445 | { | ||
| 446 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | eval(); |
| 447 | 1 | } | |
| 448 | |||
| 449 | 1 | const TridiagonalSymmetricMatrix & inverse() const | |
| 450 | { | ||
| 451 | 1 | return tridiagonal_matrix; | |
| 452 | } | ||
| 453 | |||
| 454 | template<typename MatrixDerived> | ||
| 455 | TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType<Self, MatrixDerived> | ||
| 456 | 1 | applyOnTheRight(const Eigen::MatrixBase<MatrixDerived> & mat) const | |
| 457 | { | ||
| 458 | typedef TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType<Self, MatrixDerived> | ||
| 459 | ReturnType; | ||
| 460 | 1 | return ReturnType(*this, mat.derived()); | |
| 461 | } | ||
| 462 | |||
| 463 | template<typename MatrixDerived> | ||
| 464 | inline TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType<Self, MatrixDerived> | ||
| 465 | 1 | operator*(const Eigen::MatrixBase<MatrixDerived> & mat) const | |
| 466 | { | ||
| 467 | 1 | return this->applyOnTheRight(mat.derived()); | |
| 468 | } | ||
| 469 | |||
| 470 | template<typename ResultType> | ||
| 471 | 1 | inline void evalTo(ResultType & result) const | |
| 472 | { | ||
| 473 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.rows(), rows()); |
| 474 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.cols(), cols()); |
| 475 | |||
| 476 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(cols() >= 1); |
| 477 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(rows() >= 1); |
| 478 | |||
| 479 | 1 | const auto & b = m_diagonal; | |
| 480 | 1 | const auto & c = tridiagonal_matrix.subDiagonal(); | |
| 481 | 1 | const auto & w = m_sub_diagonal; | |
| 482 | |||
| 483 | // Forward sweep | ||
| 484 | 1 | result.setIdentity(); | |
| 485 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (Eigen::DenseIndex i = 1; i < m_size; ++i) |
| 486 | { | ||
| 487 |
6/12✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
|
9 | result.row(i).head(i) -= w[i - 1] * result.row(i - 1).head(i); |
| 488 | } | ||
| 489 | |||
| 490 | // Backward sweep | ||
| 491 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | result.row(m_size - 1) /= b[m_size - 1]; |
| 492 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (Eigen::DenseIndex i = m_size - 2; i >= 0; --i) |
| 493 | { | ||
| 494 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
|
9 | result.row(i) -= c[i] * result.row(i + 1); |
| 495 |
1/2✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
|
9 | result.row(i) /= b[i]; |
| 496 | } | ||
| 497 | 1 | } | |
| 498 | |||
| 499 | 8 | EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT | |
| 500 | { | ||
| 501 | 8 | return m_size; | |
| 502 | } | ||
| 503 | 4 | EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT | |
| 504 | { | ||
| 505 | 4 | return m_size; | |
| 506 | } | ||
| 507 | |||
| 508 | protected: | ||
| 509 | template<typename T, typename MatrixDerived> | ||
| 510 | friend struct TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType; | ||
| 511 | |||
| 512 | /// \brief Forward sweep of https://en.wikipedia.org/wiki/Tridiagonal_matrix_algorithm | ||
| 513 | 1 | void eval() | |
| 514 | { | ||
| 515 | 1 | m_diagonal = tridiagonal_matrix.diagonal(); | |
| 516 | 1 | m_sub_diagonal = tridiagonal_matrix.subDiagonal(); | |
| 517 | 1 | auto & w = m_sub_diagonal; | |
| 518 | 1 | auto & b = m_diagonal; | |
| 519 | 1 | const auto & c = tridiagonal_matrix.subDiagonal(); | |
| 520 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (Eigen::DenseIndex i = 1; i < m_size; ++i) |
| 521 | { | ||
| 522 | 9 | w.coeffRef(i - 1) /= b[i - 1]; | |
| 523 | 9 | m_diagonal.coeffRef(i) -= w[i - 1] * c[i - 1]; | |
| 524 | } | ||
| 525 | 1 | } | |
| 526 | |||
| 527 | const TridiagonalSymmetricMatrix & tridiagonal_matrix; | ||
| 528 | Eigen::DenseIndex m_size; | ||
| 529 | CoeffVectorType m_diagonal; | ||
| 530 | CoeffVectorType m_sub_diagonal; | ||
| 531 | }; | ||
| 532 | |||
| 533 | template<typename TridiagonalSymmetricMatrixInverse, typename RhsMatrixType> | ||
| 534 | struct TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType | ||
| 535 | : public Eigen::ReturnByValue<TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType< | ||
| 536 | TridiagonalSymmetricMatrixInverse, | ||
| 537 | RhsMatrixType>> | ||
| 538 | { | ||
| 539 | typedef TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType Self; | ||
| 540 | typedef typename traits<Self>::PlainMatrixType PlainMatrixType; | ||
| 541 | |||
| 542 | 1 | TridiagonalSymmetricMatrixInverseApplyOnTheRightReturnType( | |
| 543 | const TridiagonalSymmetricMatrixInverse & lhs, const RhsMatrixType & rhs) | ||
| 544 | 1 | : m_lhs(lhs) | |
| 545 | 1 | , m_rhs(rhs) | |
| 546 | { | ||
| 547 | 1 | } | |
| 548 | |||
| 549 | template<typename ResultType> | ||
| 550 | 1 | inline void evalTo(ResultType & result) const | |
| 551 | { | ||
| 552 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.rows(), rows()); |
| 553 |
1/24✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
|
1 | PINOCCHIO_CHECK_ARGUMENT_SIZE(result.cols(), cols()); |
| 554 | |||
| 555 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(cols() >= 1); |
| 556 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(rows() >= 1); |
| 557 | |||
| 558 | 1 | const Eigen::DenseIndex size = m_lhs.rows(); | |
| 559 | 1 | const auto & b = m_lhs.m_diagonal; | |
| 560 | 1 | const auto & c = m_lhs.tridiagonal_matrix.subDiagonal(); | |
| 561 | 1 | const auto & w = m_lhs.m_sub_diagonal; | |
| 562 | |||
| 563 | // Forward sweep | ||
| 564 | 1 | result = m_rhs; | |
| 565 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (Eigen::DenseIndex i = 1; i < size; ++i) |
| 566 | { | ||
| 567 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
|
9 | result.row(i) -= w[i - 1] * result.row(i - 1); |
| 568 | } | ||
| 569 | |||
| 570 | // Backward sweep | ||
| 571 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | result.row(size - 1) /= b[size - 1]; |
| 572 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
10 | for (Eigen::DenseIndex i = size - 2; i >= 0; --i) |
| 573 | { | ||
| 574 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
|
9 | result.row(i) -= c[i] * result.row(i + 1); |
| 575 |
1/2✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
|
9 | result.row(i) /= b[i]; |
| 576 | } | ||
| 577 | 1 | } | |
| 578 | |||
| 579 | 3 | EIGEN_CONSTEXPR Eigen::Index rows() const EIGEN_NOEXCEPT | |
| 580 | { | ||
| 581 | 3 | return m_lhs.rows(); | |
| 582 | } | ||
| 583 | 3 | EIGEN_CONSTEXPR Eigen::Index cols() const EIGEN_NOEXCEPT | |
| 584 | { | ||
| 585 | 3 | return m_rhs.cols(); | |
| 586 | } | ||
| 587 | |||
| 588 | protected: | ||
| 589 | const TridiagonalSymmetricMatrixInverse & m_lhs; | ||
| 590 | const RhsMatrixType & m_rhs; | ||
| 591 | }; | ||
| 592 | } // namespace pinocchio | ||
| 593 | |||
| 594 | #endif // #ifndef __pinocchio_math_tridiagonal_matrix_hpp__ | ||
| 595 |