| Directory: | ./ |
|---|---|
| File: | include/hpp/constraints/matrix-view.hh |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 236 | 241 | 97.9% |
| Branches: | 63 | 109 | 57.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // Copyright (c) 2017, Joseph Mirabel | ||
| 2 | // Authors: Joseph Mirabel (joseph.mirabel@laas.fr) | ||
| 3 | // | ||
| 4 | |||
| 5 | // Redistribution and use in source and binary forms, with or without | ||
| 6 | // modification, are permitted provided that the following conditions are | ||
| 7 | // met: | ||
| 8 | // | ||
| 9 | // 1. Redistributions of source code must retain the above copyright | ||
| 10 | // notice, this list of conditions and the following disclaimer. | ||
| 11 | // | ||
| 12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 13 | // notice, this list of conditions and the following disclaimer in the | ||
| 14 | // documentation and/or other materials provided with the distribution. | ||
| 15 | // | ||
| 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 27 | // DAMAGE. | ||
| 28 | |||
| 29 | #ifndef HPP_CONSTRAINTS_MATRIX_VIEW_HH | ||
| 30 | #define HPP_CONSTRAINTS_MATRIX_VIEW_HH | ||
| 31 | |||
| 32 | #include <Eigen/Core> | ||
| 33 | #include <hpp/constraints/fwd.hh> | ||
| 34 | #include <hpp/pinocchio/util.hh> | ||
| 35 | #include <hpp/util/indent.hh> | ||
| 36 | #include <iostream> | ||
| 37 | #include <vector> | ||
| 38 | |||
| 39 | #define HPP_EIGEN_USE_EVALUATOR EIGEN_VERSION_AT_LEAST(3, 2, 92) | ||
| 40 | |||
| 41 | namespace Eigen { | ||
| 42 | |||
| 43 | /// \addtogroup hpp_constraints_tools | ||
| 44 | /// \{ | ||
| 45 | |||
| 46 | /// List of integer intervals | ||
| 47 | /// | ||
| 48 | /// Used to select blocks in a vector or in a matrix. | ||
| 49 | struct BlockIndex { | ||
| 50 | /// Index of vector or matrix | ||
| 51 | typedef hpp::constraints::size_type size_type; | ||
| 52 | /// Interval of indices [first, first + second - 1] | ||
| 53 | typedef hpp::constraints::segment_t segment_t; | ||
| 54 | /// vector of segments | ||
| 55 | typedef hpp::constraints::segments_t segments_t; | ||
| 56 | |||
| 57 | /// Return the number of indices in the vector of segments. | ||
| 58 | /// \param a vector of segments | ||
| 59 | static size_type cardinal(const segments_t& a); | ||
| 60 | |||
| 61 | /// Build a vector of segments from an array of Boolean. | ||
| 62 | /// \param array array of Boolean values | ||
| 63 | /// \return the vector of segments corresponding to true values in the | ||
| 64 | /// input. | ||
| 65 | template <typename Derived> | ||
| 66 | static segments_t fromLogicalExpression( | ||
| 67 | const Eigen::ArrayBase<Derived>& array); | ||
| 68 | |||
| 69 | /// Sort segments in increasing order. | ||
| 70 | /// Compare lower bounds of intervals and lengths if lower bounds are equal. | ||
| 71 | static void sort(segments_t& a); | ||
| 72 | |||
| 73 | /// Build a sequence of non overlapping segments. | ||
| 74 | /// \param a a vector of segments | ||
| 75 | /// \note assumes a is sorted | ||
| 76 | static void shrink(segments_t& a); | ||
| 77 | |||
| 78 | /// Whether two segments overlap. | ||
| 79 | static bool overlap(const segment_t& a, const segment_t& b); | ||
| 80 | |||
| 81 | /// Compute the union of tws segments. | ||
| 82 | static segments_t sum(const segment_t& a, const segment_t& b); | ||
| 83 | |||
| 84 | /// In place addition of a segment_t to segments_t. | ||
| 85 | static void add(segments_t& a, const segment_t& b); | ||
| 86 | |||
| 87 | /// In place addition of segments_t to segments_t. | ||
| 88 | static void add(segments_t& a, const segments_t& b); | ||
| 89 | |||
| 90 | /// Compute the set difference between two segments. | ||
| 91 | static segments_t difference(const segment_t& a, const segment_t& b); | ||
| 92 | |||
| 93 | /// Compute the set difference between a vector of segments and a segment. | ||
| 94 | /// \note assumes a is sorted | ||
| 95 | static segments_t difference(const segments_t& a, const segment_t& b); | ||
| 96 | |||
| 97 | /// Compute the set difference between a segment and a vector of segments. | ||
| 98 | /// \note assume b is sorted | ||
| 99 | static segments_t difference(const segment_t& a, const segments_t& b); | ||
| 100 | |||
| 101 | /// Compute the set difference between two vectors of segments. | ||
| 102 | /// \note assume a and b are sorted | ||
| 103 | static segments_t difference(const segments_t& a, const segments_t& b); | ||
| 104 | |||
| 105 | /// Split a set of segment into two sets of segments | ||
| 106 | /// \param segments input set of segments, | ||
| 107 | /// \param cardinal cardinal of the first set of segments, | ||
| 108 | /// \return the first set of segments. | ||
| 109 | /// | ||
| 110 | /// The second set is stored in the input set of segments. | ||
| 111 | static segments_t split(segments_t& segments, const size_type& cardinal); | ||
| 112 | |||
| 113 | /// Extract a subset of a set of segments | ||
| 114 | /// \param segments input set of segments | ||
| 115 | /// \param start beginning of extracted set of segments (cardinal of subset | ||
| 116 | /// left behind in input set of segments) | ||
| 117 | /// \param cardinal cardinal of extracted set of segments, | ||
| 118 | /// \return subset of segments. | ||
| 119 | static segments_t extract(const segments_t& segments, size_type start, | ||
| 120 | size_type cardinal); | ||
| 121 | }; // struct BlockIndex | ||
| 122 | |||
| 123 | template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols> | ||
| 124 | class MatrixBlockView; | ||
| 125 | |||
| 126 | /// Collection of indices of matrix blocks | ||
| 127 | /// \param _allRows whether the collection is composed of full columns | ||
| 128 | /// \param _allCols whether the collection is composed of full rows | ||
| 129 | /// | ||
| 130 | /// This class enables a user to virtually create a matrix that concatenates | ||
| 131 | /// blocks of a larger matrix. | ||
| 132 | /// | ||
| 133 | /// The smaller matrix is built by methods \ref lview and \ref rview | ||
| 134 | /// \li \ref lview returns a smaller matrix that can be written in, | ||
| 135 | /// \li \ref rview returns a smaller matrix that cannot be written in. | ||
| 136 | template <bool _allRows = false, bool _allCols = false> | ||
| 137 | class MatrixBlocks; | ||
| 138 | |||
| 139 | /// \} | ||
| 140 | |||
| 141 | template <bool _allRows = false, bool _allCols = false> | ||
| 142 | class MatrixBlocksRef; | ||
| 143 | |||
| 144 | namespace internal { | ||
| 145 | template <bool condition> | ||
| 146 | struct static_if { | ||
| 147 | template <class Then, class Else> | ||
| 148 | 14498 | static constexpr inline Then& rr(Then& f, Else&) { | |
| 149 | 14498 | return f; | |
| 150 | } | ||
| 151 | template <class Then, class Else> | ||
| 152 | 452840 | static constexpr inline Then pp(Then f, Else) { | |
| 153 | 452840 | return f; | |
| 154 | } | ||
| 155 | }; | ||
| 156 | template <> | ||
| 157 | struct static_if<false> { | ||
| 158 | template <class Then, class Else> | ||
| 159 | 330606 | static constexpr inline Else& rr(Then&, Else& s) { | |
| 160 | 330606 | return s; | |
| 161 | } | ||
| 162 | template <class Then, class Else> | ||
| 163 | 823303 | static constexpr inline Else pp(Then, Else s) { | |
| 164 | 823303 | return s; | |
| 165 | } | ||
| 166 | }; | ||
| 167 | |||
| 168 | struct empty_struct { | ||
| 169 | typedef MatrixXd::Index Index; | ||
| 170 | constexpr empty_struct() = default; | ||
| 171 | template <typename In_t> | ||
| 172 | 265821 | constexpr empty_struct(In_t) {} | |
| 173 | template <typename In0_t, typename In1_t> | ||
| 174 | 2 | constexpr empty_struct(In0_t, In1_t) {} | |
| 175 | 266 | static constexpr inline Index size() { return 1; } | |
| 176 | 452833 | inline constexpr const Index& operator[](const Index& i) const { return i; } | |
| 177 | }; | ||
| 178 | |||
| 179 | template <bool _allRows, bool _allCols> | ||
| 180 | struct traits<MatrixBlocks<_allRows, _allCols> > { | ||
| 181 | enum { AllRows = _allRows, AllCols = _allCols }; | ||
| 182 | typedef | ||
| 183 | typename internal::conditional<_allRows, internal::empty_struct, | ||
| 184 | BlockIndex::segments_t>::type RowIndices_t; | ||
| 185 | typedef | ||
| 186 | typename internal::conditional<_allCols, internal::empty_struct, | ||
| 187 | BlockIndex::segments_t>::type ColIndices_t; | ||
| 188 | }; | ||
| 189 | |||
| 190 | template <bool _allRows, bool _allCols> | ||
| 191 | struct traits<MatrixBlocksRef<_allRows, _allCols> > { | ||
| 192 | enum { AllRows = _allRows, AllCols = _allCols }; | ||
| 193 | typedef typename internal::conditional<_allRows, internal::empty_struct, | ||
| 194 | const BlockIndex::segments_t&>::type | ||
| 195 | RowIndices_t; | ||
| 196 | typedef typename internal::conditional<_allCols, internal::empty_struct, | ||
| 197 | const BlockIndex::segments_t&>::type | ||
| 198 | ColIndices_t; | ||
| 199 | }; | ||
| 200 | |||
| 201 | template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols> | ||
| 202 | struct traits<MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > { | ||
| 203 | #if HPP_EIGEN_USE_EVALUATOR | ||
| 204 | typedef typename ArgType::StorageIndex StorageIndex; | ||
| 205 | #else // HPP_EIGEN_USE_EVALUATOR | ||
| 206 | typedef typename ArgType::Index Index; | ||
| 207 | #endif // HPP_EIGEN_USE_EVALUATOR | ||
| 208 | typedef typename traits<ArgType>::StorageKind StorageKind; | ||
| 209 | typedef typename traits<ArgType>::XprKind XprKind; | ||
| 210 | typedef typename ArgType::Scalar Scalar; | ||
| 211 | enum { | ||
| 212 | #if !HPP_EIGEN_USE_EVALUATOR | ||
| 213 | CoeffReadCost = ArgType::CoeffReadCost, | ||
| 214 | #endif // !HPP_EIGEN_USE_EVALUATOR | ||
| 215 | Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit & | ||
| 216 | ~LinearAccessBit & ArgType::Flags, | ||
| 217 | RowsAtCompileTime = (_allRows ? ArgType::RowsAtCompileTime : _Rows), | ||
| 218 | ColsAtCompileTime = (_allCols ? ArgType::ColsAtCompileTime : _Cols), | ||
| 219 | MaxRowsAtCompileTime = ArgType::MaxRowsAtCompileTime, | ||
| 220 | MaxColsAtCompileTime = ArgType::MaxColsAtCompileTime | ||
| 221 | }; | ||
| 222 | }; | ||
| 223 | |||
| 224 | #if HPP_EIGEN_USE_EVALUATOR | ||
| 225 | template <typename Derived, typename ArgType, int _Rows, int _Cols, | ||
| 226 | bool _allRows, bool _allCols, typename Functor, typename Scalar> | ||
| 227 | struct Assignment<Derived, | ||
| 228 | MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, | ||
| 229 | Functor, Dense2Dense, Scalar> { | ||
| 230 | typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> | ||
| 231 | OtherDerived; | ||
| 232 | 169387 | static EIGEN_STRONG_INLINE void run(Derived& dst, const OtherDerived& src, | |
| 233 | const Functor& func) { | ||
| 234 | 169387 | dst.resize(src.rows(), src.cols()); | |
| 235 | typedef Block<Derived> BlockDerived; | ||
| 236 | typedef Assignment<BlockDerived, typename OtherDerived::BlockConstXprType, | ||
| 237 | Functor> | ||
| 238 | AssignmentType; | ||
| 239 |
2/2✓ Branch 2 taken 89169 times.
✓ Branch 3 taken 89043 times.
|
339031 | for (typename OtherDerived::block_iterator b(src); b.valid(); ++b) { |
| 240 |
3/6✓ Branch 1 taken 89162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89163 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 89168 times.
✗ Branch 10 not taken.
|
169644 | BlockDerived bdst(dst.block(b.ro(), b.co(), b.rs(), b.cs())); |
| 241 |
3/6✓ Branch 1 taken 89168 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 89162 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 89165 times.
✗ Branch 8 not taken.
|
169642 | AssignmentType::run(bdst, src._block(b), func); |
| 242 | } | ||
| 243 | 169393 | } | |
| 244 | }; | ||
| 245 | #else // HPP_EIGEN_USE_EVALUATOR | ||
| 246 | template <typename Derived, typename ArgType, int _Rows, int _Cols, | ||
| 247 | bool _allRows, bool _allCols> | ||
| 248 | struct assign_selector< | ||
| 249 | Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false, | ||
| 250 | false> { | ||
| 251 | typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> | ||
| 252 | OtherDerived; | ||
| 253 | static EIGEN_STRONG_INLINE Derived& run(Derived& dst, | ||
| 254 | const OtherDerived& other) { | ||
| 255 | other.writeTo(dst); | ||
| 256 | return dst; | ||
| 257 | } | ||
| 258 | template <typename ActualDerived, typename ActualOtherDerived> | ||
| 259 | static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, | ||
| 260 | const ActualOtherDerived& other) { | ||
| 261 | other.evalTo(dst); | ||
| 262 | return dst; | ||
| 263 | } | ||
| 264 | }; | ||
| 265 | template <typename Derived, typename ArgType, int _Rows, int _Cols, | ||
| 266 | bool _allRows, bool _allCols> | ||
| 267 | struct assign_selector< | ||
| 268 | Derived, MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols>, false, | ||
| 269 | true> { | ||
| 270 | typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> | ||
| 271 | OtherDerived; | ||
| 272 | static EIGEN_STRONG_INLINE Derived& run(Derived& dst, | ||
| 273 | const OtherDerived& other) { | ||
| 274 | other.writeTo(dst.transpose()); | ||
| 275 | return dst; | ||
| 276 | } | ||
| 277 | template <typename ActualDerived, typename ActualOtherDerived> | ||
| 278 | static EIGEN_STRONG_INLINE Derived& evalTo(ActualDerived& dst, | ||
| 279 | const ActualOtherDerived& other) { | ||
| 280 | Transpose<ActualDerived> dstTrans(dst); | ||
| 281 | other.evalTo(dstTrans); | ||
| 282 | return dst; | ||
| 283 | } | ||
| 284 | }; | ||
| 285 | #endif // HPP_EIGEN_USE_EVALUATOR | ||
| 286 | |||
| 287 | template <typename Src, typename Dst> | ||
| 288 | struct eval_matrix_block_view_to {}; | ||
| 289 | template <typename Src, typename _ArgType, int _Rows, int _Cols, bool _allRows, | ||
| 290 | bool _allCols> | ||
| 291 | struct eval_matrix_block_view_to< | ||
| 292 | Src, MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > { | ||
| 293 | // MatrixBlockView <- matrix | ||
| 294 | typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Dst; | ||
| 295 | 112702 | static void run(const Src& src, Dst& dst) { | |
| 296 |
5/5✓ Branch 1 taken 3 times.
✓ Branch 2 taken 44991 times.
✓ Branch 3 taken 69613 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 3 times.
|
201094 | for (typename Dst::block_iterator b(dst); b.valid(); ++b) |
| 297 |
6/12✓ Branch 1 taken 44997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 44997 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 44997 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 44997 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 44997 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 44997 times.
✗ Branch 19 not taken.
|
88392 | dst._block(b) = src.block(b.ro(), b.co(), b.rs(), b.cs()); |
| 298 | 112702 | } | |
| 299 | }; | ||
| 300 | template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols, | ||
| 301 | typename Dst> | ||
| 302 | struct eval_matrix_block_view_to< | ||
| 303 | MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>, Dst> { | ||
| 304 | // matrix <- MatrixBlockView | ||
| 305 | typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Src; | ||
| 306 | 10044 | static void run(const Src& src, Dst& dst) { | |
| 307 |
5/5✓ Branch 1 taken 3 times.
✓ Branch 2 taken 8369 times.
✓ Branch 3 taken 8251 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 3 times.
|
20330 | for (typename Src::block_iterator b(src); b.valid(); ++b) |
| 308 |
6/12✓ Branch 1 taken 8375 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8375 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8375 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 8375 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 8375 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 8375 times.
✗ Branch 19 not taken.
|
10286 | dst.block(b.ro(), b.co(), b.rs(), b.cs()) = src._block(b); |
| 309 | 10044 | } | |
| 310 | }; | ||
| 311 | template <typename _ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols, | ||
| 312 | typename _ArgType2, int _Rows2, int _Cols2, bool _allRows2, | ||
| 313 | bool _allCols2> | ||
| 314 | struct eval_matrix_block_view_to< | ||
| 315 | MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols>, | ||
| 316 | MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> > { | ||
| 317 | // MatrixBlockView <- MatrixBlockView | ||
| 318 | typedef MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> Src; | ||
| 319 | typedef MatrixBlockView<_ArgType2, _Rows2, _Cols2, _allRows2, _allCols2> Dst; | ||
| 320 | 13432 | static void run(const Src& src, Dst& dst) { | |
| 321 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
13432 | typename Dst::block_iterator db(dst); |
| 322 |
3/4✓ Branch 2 taken 25238 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 25238 times.
✓ Branch 6 taken 13424 times.
|
38690 | for (typename Src::block_iterator sb(src); sb.valid(); ++sb) { |
| 323 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 25238 times.
|
25258 | assert(db.valid()); |
| 324 |
3/6✓ Branch 1 taken 25238 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25238 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25238 times.
✗ Branch 8 not taken.
|
25258 | dst._block(db) = src._block(sb); |
| 325 |
1/2✓ Branch 1 taken 25238 times.
✗ Branch 2 not taken.
|
25258 | ++db; |
| 326 | } | ||
| 327 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 13424 times.
|
13432 | assert(!db.valid()); |
| 328 | 13432 | } | |
| 329 | }; | ||
| 330 | |||
| 331 | template <typename ReturnType, typename View, bool AllRows = View::AllRows, | ||
| 332 | bool AllCols = View::AllCols> | ||
| 333 | struct access_block_from_matrix_block_view { | ||
| 334 | typedef typename View::size_type size_type; | ||
| 335 | template <typename Derived> | ||
| 336 | 50702 | static ReturnType run(Derived& d, size_type r, size_type c, size_type rs, | |
| 337 | size_type cs) { | ||
| 338 | 50702 | return ReturnType(d, r, c, rs, cs); | |
| 339 | } | ||
| 340 | }; | ||
| 341 | template <typename ReturnType, typename View> | ||
| 342 | struct access_block_from_matrix_block_view<ReturnType, View, false, true> { | ||
| 343 | typedef typename View::size_type size_type; | ||
| 344 | template <typename Derived> | ||
| 345 | 303702 | static ReturnType run(Derived& d, size_type r, size_type, size_type rs, | |
| 346 | size_type) { | ||
| 347 | 303702 | return d.middleRows(r, rs); | |
| 348 | } | ||
| 349 | }; | ||
| 350 | template <typename ReturnType, typename View> | ||
| 351 | struct access_block_from_matrix_block_view<ReturnType, View, true, false> { | ||
| 352 | typedef typename View::size_type size_type; | ||
| 353 | template <typename Derived> | ||
| 354 | 352 | static ReturnType run(Derived& d, size_type, size_type c, size_type, | |
| 355 | size_type cs) { | ||
| 356 | 352 | return d.middleCols(c, cs); | |
| 357 | } | ||
| 358 | }; | ||
| 359 | |||
| 360 | struct dont_print_indices { | ||
| 361 | template <typename BlockIndexType> | ||
| 362 | static void run(std::ostream&, const BlockIndexType&) {} | ||
| 363 | }; | ||
| 364 | struct print_indices { | ||
| 365 | template <typename BlockIndexType> | ||
| 366 | 63 | static void run(std::ostream& os, const BlockIndexType& bi) { | |
| 367 |
2/2✓ Branch 1 taken 86 times.
✓ Branch 2 taken 63 times.
|
149 | for (std::size_t i = 0; i < bi.size(); ++i) |
| 368 | 86 | os << "[ " << bi[i].first << ", " << bi[i].second << "], "; | |
| 369 | 63 | } | |
| 370 | }; | ||
| 371 | |||
| 372 | #if HPP_EIGEN_USE_EVALUATOR | ||
| 373 | template <typename ArgType, int _Rows, int _Cols, bool _allRows, bool _allCols> | ||
| 374 | struct unary_evaluator< | ||
| 375 | MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > | ||
| 376 | : evaluator_base< | ||
| 377 | MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> > { | ||
| 378 | typedef MatrixBlockView<ArgType, _Rows, _Cols, _allRows, _allCols> XprType; | ||
| 379 | |||
| 380 | enum { | ||
| 381 | CoeffReadCost = evaluator<ArgType>::CoeffReadCost, | ||
| 382 | Flags = ~PacketAccessBit & ~DirectAccessBit & ~ActualPacketAccessBit & | ||
| 383 | ~LinearAccessBit & ArgType::Flags, | ||
| 384 | Alignment = 0 | ||
| 385 | }; | ||
| 386 | EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& view) | ||
| 387 | : m_view(view) {} | ||
| 388 | |||
| 389 | const XprType& m_view; | ||
| 390 | }; | ||
| 391 | #endif // HPP_EIGEN_USE_EVALUATOR | ||
| 392 | } // namespace internal | ||
| 393 | |||
| 394 | #define EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(Derived) \ | ||
| 395 | enum { AllRows = _allRows, AllCols = _allCols }; \ | ||
| 396 | typedef MatrixBlocksBase<Derived> Base; \ | ||
| 397 | typedef typename Base::size_type size_type; \ | ||
| 398 | typedef typename Base::segments_t segments_t; \ | ||
| 399 | typedef typename Base::segment_t segment_t; \ | ||
| 400 | typedef typename Base::RowIndices_t RowIndices_t; \ | ||
| 401 | typedef typename Base::ColIndices_t ColIndices_t; | ||
| 402 | |||
| 403 | /// \addtogroup hpp_constraints_tools | ||
| 404 | /// \{ | ||
| 405 | |||
| 406 | template <typename Derived> | ||
| 407 | class MatrixBlocksBase { | ||
| 408 | public: | ||
| 409 | enum { | ||
| 410 | AllRows = internal::traits<Derived>::AllRows, | ||
| 411 | AllCols = internal::traits<Derived>::AllCols, | ||
| 412 | OneDimension = bool(AllRows) || bool(AllCols) | ||
| 413 | }; | ||
| 414 | /// Index of vector or matrix | ||
| 415 | typedef hpp::constraints::size_type size_type; | ||
| 416 | /// Interval of indices [first, first + second - 1] | ||
| 417 | typedef BlockIndex::segment_t segment_t; | ||
| 418 | /// vector of segments | ||
| 419 | typedef BlockIndex::segments_t segments_t; | ||
| 420 | typedef typename internal::traits<Derived>::RowIndices_t RowIndices_t; | ||
| 421 | typedef typename internal::traits<Derived>::ColIndices_t ColIndices_t; | ||
| 422 | |||
| 423 | /// Smaller matrix composed by concatenation of the blocks | ||
| 424 | template <typename MatrixType, int _Rows = MatrixType::RowsAtCompileTime, | ||
| 425 | int _Cols = MatrixType::ColsAtCompileTime> | ||
| 426 | struct View { | ||
| 427 | typedef MatrixBlockView<MatrixType, _Rows, _Cols, AllRows, AllCols> type; | ||
| 428 | }; // struct View | ||
| 429 | |||
| 430 | 1720577 | Derived const& derived() const { return static_cast<Derived const&>(*this); } | |
| 431 | Derived& derived() { return static_cast<Derived&>(*this); } | ||
| 432 | |||
| 433 | /// Writable view of the smaller matrix | ||
| 434 | /// \param other matrix to whick block are extracted | ||
| 435 | /// \return writable view of the smaller matrix composed by concatenation | ||
| 436 | /// of blocks. | ||
| 437 | template <typename MatrixType> | ||
| 438 | 121828 | EIGEN_STRONG_INLINE typename View<MatrixType>::type lview( | |
| 439 | const MatrixBase<MatrixType>& other) const { | ||
| 440 | 121828 | MatrixType& o = const_cast<MatrixBase<MatrixType>&>(other).derived(); | |
| 441 | if (Derived::OneDimension) | ||
| 442 | 120906 | return typename View<MatrixType>::type(o, nbIndices(), indices()); | |
| 443 | else | ||
| 444 | 922 | return typename View<MatrixType>::type(o, nbRows(), rows(), nbCols(), | |
| 445 | 1844 | cols()); | |
| 446 | } | ||
| 447 | |||
| 448 | /// Non-writable view of the smaller matrix | ||
| 449 | /// \param other matrix to whick block are extracted | ||
| 450 | /// \return non-writable view of the smaller matrix composed by | ||
| 451 | /// concatenation of blocks. | ||
| 452 | template <typename MatrixType> | ||
| 453 | 221180 | EIGEN_STRONG_INLINE typename View<const MatrixType>::type rview( | |
| 454 | const MatrixBase<MatrixType>& other) const { | ||
| 455 | if (Derived::OneDimension) | ||
| 456 | 172734 | return typename View<const MatrixType>::type(other.derived(), nbIndices(), | |
| 457 | 345476 | indices()); | |
| 458 | else | ||
| 459 | 48444 | return typename View<const MatrixType>::type(other.derived(), nbRows(), | |
| 460 | 96888 | rows(), nbCols(), cols()); | |
| 461 | } | ||
| 462 | |||
| 463 | 55 | MatrixBlocksRef<AllCols, AllRows> transpose() const { | |
| 464 | 73 | return MatrixBlocksRef<AllCols, AllRows>(nbCols(), cols(), nbRows(), | |
| 465 | 104 | rows()); | |
| 466 | } | ||
| 467 | |||
| 468 | 24296 | MatrixBlocksRef<AllRows, true> keepRows() const { | |
| 469 | assert(!AllRows); | ||
| 470 | 24296 | return MatrixBlocksRef<AllRows, true>(nbRows(), rows()); | |
| 471 | } | ||
| 472 | |||
| 473 | 1 | MatrixBlocksRef<true, AllCols> keepCols() const { | |
| 474 | assert(!AllCols); | ||
| 475 | 1 | return MatrixBlocksRef<true, AllCols>(nbCols(), cols()); | |
| 476 | } | ||
| 477 | |||
| 478 | /// Return row or column indices as a vector of segments | ||
| 479 | /// | ||
| 480 | /// \return rows indices if not all rows are selected | ||
| 481 | /// (see template parameter _allRows), | ||
| 482 | /// column indices if all rows are selected. | ||
| 483 | 555901 | inline const segments_t& indices() const { | |
| 484 | 555901 | return internal::static_if<AllRows>::rr(cols(), rows()); | |
| 485 | } | ||
| 486 | |||
| 487 | /// Return row indices | ||
| 488 | /// \warning _allRows should be false | ||
| 489 | 654050 | inline const RowIndices_t& rows() const { return derived().rows(); } | |
| 490 | |||
| 491 | /// Return column indices | ||
| 492 | /// \warning _allCols should be false | ||
| 493 | 605423 | inline const ColIndices_t& cols() const { return derived().cols(); } | |
| 494 | |||
| 495 | /// Return number of row or column indices | ||
| 496 | /// | ||
| 497 | /// \return number of rows indices if not all rows are selected | ||
| 498 | /// (see template parameter _allRows), | ||
| 499 | /// number of column indices if all rows are selected. | ||
| 500 | 313493 | inline const size_type& nbIndices() const { | |
| 501 | 313493 | return AllRows ? nbCols() : nbRows(); | |
| 502 | } | ||
| 503 | |||
| 504 | /// Return number of row indices | ||
| 505 | /// \warning _allRows should be false | ||
| 506 | 293102 | inline const size_type& nbRows() const { return derived().nbRows(); } | |
| 507 | |||
| 508 | /// Return number of column indices | ||
| 509 | /// \warning _allCols should be false | ||
| 510 | 27378 | inline const size_type& nbCols() const { return derived().nbCols(); } | |
| 511 | |||
| 512 | /// Extract a block | ||
| 513 | /// \param i, j, ni, nj upper left corner and lengths of the block | ||
| 514 | /// \return new instance | ||
| 515 | ✗ | MatrixBlocks<AllRows, AllCols> block(size_type i, size_type j, size_type ni, | |
| 516 | size_type nj) const { | ||
| 517 | ✗ | return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni), | |
| 518 | ✗ | BlockIndex::extract(cols(), j, nj)); | |
| 519 | } | ||
| 520 | |||
| 521 | /// Extract a set of rows | ||
| 522 | /// \param i, ni start and length of the set of rows | ||
| 523 | /// \return new instance | ||
| 524 | MatrixBlocks<AllRows, AllCols> middleRows(size_type i, size_type ni) const { | ||
| 525 | return MatrixBlocks<AllRows, AllCols>(BlockIndex::extract(rows(), i, ni), | ||
| 526 | cols()); | ||
| 527 | } | ||
| 528 | |||
| 529 | /// Extract a set of cols | ||
| 530 | /// \param j, nj start and length of the set of rows | ||
| 531 | /// \return new instance | ||
| 532 | MatrixBlocks<AllRows, AllCols> middleCols(size_type j, size_type nj) const { | ||
| 533 | return MatrixBlocks<AllRows, AllCols>(rows(), | ||
| 534 | BlockIndex::extract(cols(), j, nj)); | ||
| 535 | } | ||
| 536 | |||
| 537 | protected: | ||
| 538 | /// Empty constructor | ||
| 539 | 317454 | MatrixBlocksBase() {} | |
| 540 | |||
| 541 | /// Copy constructor | ||
| 542 | 8802 | MatrixBlocksBase(const MatrixBlocksBase&) {} | |
| 543 | }; // class MatrixBlocks | ||
| 544 | |||
| 545 | template <bool _allRows, bool _allCols> | ||
| 546 | class MatrixBlocks | ||
| 547 | : public MatrixBlocksBase<MatrixBlocks<_allRows, _allCols> > { | ||
| 548 | public: | ||
| 549 | EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(MatrixBlocks) | ||
| 550 | |||
| 551 | /// Empty constructor | ||
| 552 | 121252 | MatrixBlocks() : m_nbRows(0), m_nbCols(0), m_rows(), m_cols() {} | |
| 553 | |||
| 554 | /// Constructor by vectors of segments | ||
| 555 | /// \param rows set of row indices, | ||
| 556 | /// \param cols set of column indices, | ||
| 557 | /// \warning rows and cols must be sorted | ||
| 558 | 1181 | MatrixBlocks(const segments_t& rows, const segments_t& cols) | |
| 559 | 1181 | : m_nbRows(BlockIndex::cardinal(rows)), | |
| 560 | 1181 | m_nbCols(BlockIndex::cardinal(cols)), | |
| 561 | 1181 | m_rows(rows), | |
| 562 |
1/2✓ Branch 2 taken 1181 times.
✗ Branch 3 not taken.
|
2362 | m_cols(cols) {} |
| 563 | |||
| 564 | /// Constructor by vectors of segments | ||
| 565 | /// \param nbRows number of rows, | ||
| 566 | /// \param nbCols number of columns, | ||
| 567 | /// \param rows set of row indices, | ||
| 568 | /// \param cols set of column indices, | ||
| 569 | /// \warning rows and cols must be sorted | ||
| 570 | MatrixBlocks(const size_type& nbRows, const RowIndices_t& rows, | ||
| 571 | const size_type& nbCols, const ColIndices_t& cols) | ||
| 572 | : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {} | ||
| 573 | |||
| 574 | /// Constructor of single block | ||
| 575 | /// \param start indice for row and column | ||
| 576 | /// \param size number of indices in the block (row and column) | ||
| 577 | /// \note if all rows or all columns are selected (template parameter) | ||
| 578 | /// the block will contain all rows, respectively all columns. | ||
| 579 | 2 | MatrixBlocks(size_type start, size_type size) | |
| 580 | 2 | : m_nbRows(_allRows ? 0 : size), | |
| 581 | 2 | m_nbCols(_allCols ? 0 : size), | |
| 582 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | m_rows(1, BlockIndex::segment_t(start, size)), |
| 583 | 4 | m_cols(1, BlockIndex::segment_t(start, size)) {} | |
| 584 | |||
| 585 | /// Constructor by a collection of indices | ||
| 586 | /// \param idx collections of indices (for rows and columns) | ||
| 587 | /// \warning idx must be sorted and shrinked | ||
| 588 | /// \note if all rows or all columns are selected (template parameter) | ||
| 589 | /// the block will contain all rows, respectively all columns. | ||
| 590 | 137104 | MatrixBlocks(const segments_t& idx) | |
| 591 | 137104 | : m_nbRows(_allRows ? 0 : BlockIndex::cardinal(idx)), | |
| 592 | 137104 | m_nbCols(_allCols ? 0 : BlockIndex::cardinal(idx)), | |
| 593 | 137104 | m_rows(idx), | |
| 594 |
1/2✓ Branch 2 taken 52910 times.
✗ Branch 3 not taken.
|
274208 | m_cols(idx) {} |
| 595 | |||
| 596 | /// Constructor of a single block | ||
| 597 | /// \param idx segment of row and column indices | ||
| 598 | /// \note if all rows or all columns are selected (template parameter) | ||
| 599 | /// the block will contain all rows, respectively all columns. | ||
| 600 | MatrixBlocks(const segment_t& idx) | ||
| 601 | : m_nbRows(_allRows ? 0 : idx.second), | ||
| 602 | m_nbCols(_allCols ? 0 : idx.second), | ||
| 603 | m_rows(segments_t(1, idx)), | ||
| 604 | m_cols(segments_t(1, idx)) {} | ||
| 605 | |||
| 606 | /// Copy constructor | ||
| 607 | template <typename MBDerived> | ||
| 608 | 31 | MatrixBlocks(const MatrixBlocksBase<MBDerived>& other) | |
| 609 | 31 | : m_nbRows(other.nbRows()), | |
| 610 | 31 | m_nbCols(other.nbCols()), | |
| 611 | 31 | m_rows(other.rows()), | |
| 612 |
1/2✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
|
62 | m_cols(other.cols()) { |
| 613 | EIGEN_STATIC_ASSERT((bool(AllRows) == bool(MBDerived::AllRows)) && | ||
| 614 | (bool(AllCols) == bool(MBDerived::AllCols)), | ||
| 615 | YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); | ||
| 616 | 31 | } | |
| 617 | |||
| 618 |
1/2✓ Branch 3 taken 1146 times.
✗ Branch 4 not taken.
|
8792 | MatrixBlocks(const MatrixBlocks& other) = default; |
| 619 | |||
| 620 | 61252 | MatrixBlocks& operator=(const MatrixBlocks& other) { | |
| 621 | 61252 | m_nbRows = other.m_nbRows; | |
| 622 | 61252 | m_nbCols = other.m_nbCols; | |
| 623 | 35458 | m_rows = other.m_rows; | |
| 624 | 26869 | m_cols = other.m_cols; | |
| 625 | 61252 | return *this; | |
| 626 | } | ||
| 627 | /// Clear rows | ||
| 628 | 10655 | inline void clearRows() { | |
| 629 | 10655 | m_rows.clear(); | |
| 630 | 10655 | m_nbRows = 0; | |
| 631 | 10655 | } | |
| 632 | |||
| 633 | /// Clear cols | ||
| 634 | inline void clearCols() { | ||
| 635 | m_cols.clear(); | ||
| 636 | m_nbCols = 0; | ||
| 637 | } | ||
| 638 | |||
| 639 | /// Add consecutive rows | ||
| 640 | /// \param row first row to add | ||
| 641 | /// \param size number of rows to add | ||
| 642 | 78093 | inline void addRow(const size_type& row, const size_type size) { | |
| 643 |
1/2✓ Branch 2 taken 78093 times.
✗ Branch 3 not taken.
|
78093 | m_rows.push_back(segment_t(row, size)); |
| 644 | 78093 | m_nbRows += size; | |
| 645 | 78093 | } | |
| 646 | |||
| 647 | /// Add consecutive columns | ||
| 648 | /// \param col first column to add | ||
| 649 | /// \param size number of columns to add | ||
| 650 | 7557 | inline void addCol(const size_type& col, const size_type size) { | |
| 651 |
1/2✓ Branch 2 taken 7557 times.
✗ Branch 3 not taken.
|
7557 | m_cols.push_back(segment_t(col, size)); |
| 652 | 7557 | m_nbCols += size; | |
| 653 | 7557 | } | |
| 654 | |||
| 655 | /// Selectively recompute set of rows | ||
| 656 | /// \tparam Sort whether set of rows should be sorted, | ||
| 657 | /// \tparam Shrink whether set of rows should be shrunk, | ||
| 658 | /// \tparam Cardinal whether number of rows should be recomputed | ||
| 659 | template <bool Sort, bool Shrink, bool Cardinal> | ||
| 660 | 19233 | inline void updateRows() { | |
| 661 | 19233 | update<Sort, Shrink, Cardinal>(m_rows, m_nbRows); | |
| 662 | 19233 | } | |
| 663 | |||
| 664 | /// Selectively recompute set of columns | ||
| 665 | /// \tparam Sort whether set of columns should be sorted, | ||
| 666 | /// \tparam Shrink whether set of columns should be shrunk, | ||
| 667 | /// \tparam Cardinal whether number of columns should be recomputed | ||
| 668 | template <bool Sort, bool Shrink, bool Cardinal> | ||
| 669 | inline void updateCols() { | ||
| 670 | update<Sort, Shrink, Cardinal>(m_cols, m_nbCols); | ||
| 671 | } | ||
| 672 | |||
| 673 | /// Return row indices | ||
| 674 | /// \warning _allRows should be false | ||
| 675 | 655165 | inline const RowIndices_t& rows() const { return m_rows; } | |
| 676 | |||
| 677 | /// Return column indices | ||
| 678 | /// \warning _allCols should be false | ||
| 679 | 580595 | inline const ColIndices_t& cols() const { return m_cols; } | |
| 680 | |||
| 681 | /// Return number of row indices | ||
| 682 | /// \warning _allRows should be false | ||
| 683 | 246817 | inline const size_type& nbRows() const { return m_nbRows; } | |
| 684 | |||
| 685 | /// Return number of column indices | ||
| 686 | /// \warning _allCols should be false | ||
| 687 | 40896 | inline const size_type& nbCols() const { return m_nbCols; } | |
| 688 | |||
| 689 | template <bool Sort, bool Shrink, bool Cardinal> | ||
| 690 | 51688 | inline void updateIndices() { | |
| 691 | 51688 | update<Sort, Shrink, Cardinal>( | |
| 692 | 51688 | internal::static_if<_allRows>::rr(m_cols, m_rows), | |
| 693 | 51688 | _allRows ? m_nbCols : m_nbRows); | |
| 694 | 51688 | } | |
| 695 | |||
| 696 | size_type m_nbRows, m_nbCols; | ||
| 697 | RowIndices_t m_rows; | ||
| 698 | ColIndices_t m_cols; | ||
| 699 | |||
| 700 | private: | ||
| 701 | template <bool Sort, bool Shrink, bool Cardinal> | ||
| 702 | 63343 | static inline void update(segments_t& b, size_type& idx) { | |
| 703 | 37499 | if (Sort) BlockIndex::sort(b); | |
| 704 | 63343 | if (Shrink) BlockIndex::shrink(b); | |
| 705 | 63343 | if (Cardinal) idx = BlockIndex::cardinal(b); | |
| 706 | 63343 | } | |
| 707 | }; // class MatrixBlocks | ||
| 708 | |||
| 709 | /// \cond | ||
| 710 | template <bool _allRows, bool _allCols> | ||
| 711 | class MatrixBlocksRef | ||
| 712 | : public MatrixBlocksBase<MatrixBlocksRef<_allRows, _allCols> > { | ||
| 713 | public: | ||
| 714 | EIGEN_MATRIX_BLOCKS_PUBLIC_INTERFACE(MatrixBlocksRef) | ||
| 715 | |||
| 716 | /// Constructor by vectors of segments | ||
| 717 | /// \param rows set of row indices, | ||
| 718 | /// \param cols set of column indices, | ||
| 719 | /// \warning rows and cols must be sorted | ||
| 720 | 55 | MatrixBlocksRef(const size_type& nbRows, const RowIndices_t& rows, | |
| 721 | const size_type& nbCols, const ColIndices_t& cols) | ||
| 722 | 55 | : m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols) {} | |
| 723 | |||
| 724 | /// Constructor by two (row or col) MatrixBlocks | ||
| 725 | template <typename Derived1, typename Derived2> | ||
| 726 | 1620 | MatrixBlocksRef(const MatrixBlocksBase<Derived1>& rows, | |
| 727 | const MatrixBlocksBase<Derived2>& cols) | ||
| 728 | 1620 | : m_nbRows(rows.nbIndices()), | |
| 729 | 1620 | m_nbCols(cols.nbIndices()), | |
| 730 | 1620 | m_rows(rows.indices()), | |
| 731 | 3240 | m_cols(cols.indices()) { | |
| 732 | EIGEN_STATIC_ASSERT( | ||
| 733 | bool(Derived1::OneDimension) && bool(Derived2::OneDimension), | ||
| 734 | YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES); | ||
| 735 | 1620 | } | |
| 736 | |||
| 737 | /// Constructor by a collection of indices | ||
| 738 | /// \param idx collections of indices (for rows and columns) | ||
| 739 | /// \warning idx must be sorted and shrinked | ||
| 740 | /// \note if all rows or all columns are selected (template parameter) | ||
| 741 | /// the block will contain all rows, respectively all columns. | ||
| 742 | 24299 | MatrixBlocksRef(const size_type& nidx, const segments_t& idx) | |
| 743 | 24299 | : m_nbRows(_allRows ? 0 : nidx), | |
| 744 | 24299 | m_nbCols(_allCols ? 0 : nidx), | |
| 745 | 24299 | m_rows(idx), | |
| 746 | 24299 | m_cols(idx) {} | |
| 747 | |||
| 748 | /// Copy constructor | ||
| 749 | 10 | MatrixBlocksRef(const MatrixBlocksRef& other) | |
| 750 | : Base(other), | ||
| 751 | 10 | m_nbRows(other.nbRows()), | |
| 752 | 10 | m_nbCols(other.nbCols()), | |
| 753 | 10 | m_rows(other.rows()), | |
| 754 | 20 | m_cols(other.cols()) {} | |
| 755 | |||
| 756 | /// Return row indices | ||
| 757 | /// \warning _allRows should be false | ||
| 758 | 50690 | inline const RowIndices_t& rows() const { return m_rows; } | |
| 759 | |||
| 760 | /// Return column indices | ||
| 761 | /// \warning _allCols should be false | ||
| 762 | 50690 | inline const ColIndices_t& cols() const { return m_cols; } | |
| 763 | |||
| 764 | /// Return number of row indices | ||
| 765 | /// \warning _allRows should be false | ||
| 766 | 50604 | inline const size_type& nbRows() const { return m_nbRows; } | |
| 767 | |||
| 768 | /// Return number of column indices | ||
| 769 | /// \warning _allCols should be false | ||
| 770 | 2014 | inline const size_type& nbCols() const { return m_nbCols; } | |
| 771 | |||
| 772 | const size_type m_nbRows, m_nbCols; | ||
| 773 | RowIndices_t m_rows; | ||
| 774 | ColIndices_t m_cols; | ||
| 775 | }; // class MatrixBlocksRef | ||
| 776 | /// \endcond | ||
| 777 | |||
| 778 | template <typename Derived> | ||
| 779 | 54 | std::ostream& operator<<(std::ostream& os, | |
| 780 | const MatrixBlocksBase<Derived>& mbi) { | ||
| 781 | typedef | ||
| 782 | typename internal::conditional<Derived::AllRows, | ||
| 783 | internal::dont_print_indices, | ||
| 784 | internal::print_indices>::type row_printer; | ||
| 785 | typedef | ||
| 786 | typename internal::conditional<Derived::AllCols, | ||
| 787 | internal::dont_print_indices, | ||
| 788 | internal::print_indices>::type col_printer; | ||
| 789 | if (!Derived::AllRows) { | ||
| 790 | 48 | os << "Rows: "; | |
| 791 | 48 | row_printer::run(os, mbi.rows()); | |
| 792 | 6 | if (!Derived::AllCols) os << hpp::iendl; | |
| 793 | } | ||
| 794 | if (!Derived::AllCols) { | ||
| 795 | 12 | os << "Cols: "; | |
| 796 | 12 | col_printer::run(os, mbi.cols()); | |
| 797 | } | ||
| 798 | 54 | return os; | |
| 799 | } | ||
| 800 | |||
| 801 | typedef Eigen::MatrixBlocks<false, true> RowBlockIndices; | ||
| 802 | typedef Eigen::MatrixBlocks<true, false> ColBlockIndices; | ||
| 803 | |||
| 804 | /// A view of an Eigen matrix. | ||
| 805 | /// | ||
| 806 | /// Instances of MatrixBlockView are easily built from a MatrixBlocks object. | ||
| 807 | /// | ||
| 808 | /// As of the date of this documentation, this class does not support all | ||
| 809 | /// operations on matrices. | ||
| 810 | /// See tests/matrix-view.cc for a list of supported features. | ||
| 811 | /// | ||
| 812 | /// Although it is usually not useful to iterate over the blocks, it is | ||
| 813 | /// possible to do it as follows: | ||
| 814 | /// \code | ||
| 815 | /// MatrixBlockView view = ...; | ||
| 816 | /// for (MatrixBlockView::block_iterator block (view); block.valid(); ++block) | ||
| 817 | /// { | ||
| 818 | /// // Access the current block | ||
| 819 | /// view._block(block); // Returns an Eigen::Block object. | ||
| 820 | /// // For the current block | ||
| 821 | /// block.ri() // row, input | ||
| 822 | /// block.ci() // col, input | ||
| 823 | /// block.ro() // row, output | ||
| 824 | /// block.co() // col, output | ||
| 825 | /// block.rs() // number of rows | ||
| 826 | /// block.cs() // number of cols | ||
| 827 | /// } | ||
| 828 | /// \endcode | ||
| 829 | /// \sa MatrixBlocks, MatrixBlockView::block_iterator | ||
| 830 | template <typename _ArgType, int _Rows = _ArgType::RowsAtCompileTime, | ||
| 831 | int _Cols = _ArgType::ColsAtCompileTime, bool _allRows = false, | ||
| 832 | bool _allCols = false> | ||
| 833 | class MatrixBlockView | ||
| 834 | : public MatrixBase< | ||
| 835 | MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > { | ||
| 836 | public: | ||
| 837 | typedef hpp::constraints::size_type size_type; | ||
| 838 | enum { Rows = _Rows, Cols = _Cols, AllRows = _allRows, AllCols = _allCols }; | ||
| 839 | struct block_iterator { | ||
| 840 | const MatrixBlockView& view; | ||
| 841 | size_type row, col; | ||
| 842 | internal::variable_if_dynamic<size_type, (_allRows ? 0 : Dynamic)> _ro; | ||
| 843 | internal::variable_if_dynamic<size_type, (_allCols ? 0 : Dynamic)> _co; | ||
| 844 | 387622 | block_iterator(const MatrixBlockView& v) | |
| 845 | 387622 | : view(v), row(0), col(0), _ro(0), _co(0) {} | |
| 846 | /// <b>R</b>ow in the <b>O</b>utput matrix | ||
| 847 | 285368 | size_type ro() const { return _ro.value(); } | |
| 848 | /// <b>C</b>ol in the <b>O</b>utput matrix | ||
| 849 | 285366 | size_type co() const { return _co.value(); } | |
| 850 | /// <b>R</b>ow in the <b>I</b>nput matrix | ||
| 851 | 386272 | size_type ri() const { | |
| 852 |
1/2✓ Branch 1 taken 193138 times.
✗ Branch 2 not taken.
|
386276 | return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows), |
| 853 | 386272 | view.m_rows[row]) | |
| 854 | 386270 | .first; | |
| 855 | } | ||
| 856 | /// <b>C</b>ol in the <b>I</b>nput matrix | ||
| 857 | 386268 | size_type ci() const { | |
| 858 |
1/2✓ Branch 1 taken 193141 times.
✗ Branch 2 not taken.
|
386270 | return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols), |
| 859 | 386268 | view.m_cols[col]) | |
| 860 | 386276 | .first; | |
| 861 | } | ||
| 862 | /// number of <b>R</b>ow<b>S</b> | ||
| 863 | 1057362 | size_type rs() const { | |
| 864 |
1/2✓ Branch 1 taken 528686 times.
✗ Branch 2 not taken.
|
1057382 | return internal::static_if<AllRows>::pp(std::make_pair(0, view.m_nbRows), |
| 865 | 1057362 | view.m_rows[row]) | |
| 866 | 1057358 | .second; | |
| 867 | } | ||
| 868 | /// number of <b>C</b>ol<b>S</b> | ||
| 869 | 722368 | size_type cs() const { | |
| 870 |
1/2✓ Branch 1 taken 361184 times.
✗ Branch 2 not taken.
|
722360 | return internal::static_if<AllCols>::pp(std::make_pair(0, view.m_nbCols), |
| 871 | 722368 | view.m_cols[col]) | |
| 872 | 722366 | .second; | |
| 873 | } | ||
| 874 | // ++it | ||
| 875 | 386240 | block_iterator& operator++() { | |
| 876 | if (!AllRows) { | ||
| 877 | 385732 | _ro.setValue(_ro.value() + rs()); | |
| 878 | } else { | ||
| 879 | assert(view.m_rows.size() == 1); | ||
| 880 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 254 times.
|
508 | assert(row == 0); |
| 881 | } | ||
| 882 | 386240 | ++row; | |
| 883 |
2/2✓ Branch 1 taken 168898 times.
✓ Branch 2 taken 24225 times.
|
386240 | if (row == (size_type)view.m_rows.size()) { |
| 884 | 337796 | row = 0; | |
| 885 | 337796 | _ro.setValue(0); | |
| 886 | if (!AllCols) { | ||
| 887 | 50744 | _co.setValue(_co.value() + cs()); | |
| 888 | } | ||
| 889 | 337794 | ++col; | |
| 890 | if (AllCols) { | ||
| 891 | // All the blocks have been iterated on. | ||
| 892 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 143525 times.
|
287050 | assert(!valid()); |
| 893 | } | ||
| 894 | // if (col < (size_type)view.m_cols.size()) _co.setValue(0); | ||
| 895 | } | ||
| 896 | 386244 | return *this; | |
| 897 | }; | ||
| 898 | // it++ | ||
| 899 | block_iterator operator++(int) { | ||
| 900 | block_iterator copy(*this); | ||
| 901 | operator++(); | ||
| 902 | return copy; | ||
| 903 | }; | ||
| 904 | 1060886 | constexpr bool valid() const { | |
| 905 |
2/2✓ Branch 1 taken 504758 times.
✓ Branch 2 taken 25235 times.
|
2069496 | return (AllRows || view.m_rows.size() > 0) && |
| 906 |
3/3✓ Branch 0 taken 167499 times.
✓ Branch 1 taken 312415 times.
✓ Branch 2 taken 24844 times.
|
2070408 | (col < (size_type)(AllCols ? 1 : view.m_cols.size())); |
| 907 | } | ||
| 908 | }; | ||
| 909 | typedef MatrixBase< | ||
| 910 | MatrixBlockView<_ArgType, _Rows, _Cols, _allRows, _allCols> > | ||
| 911 | Base; | ||
| 912 | EIGEN_GENERIC_PUBLIC_INTERFACE(MatrixBlockView) | ||
| 913 | |||
| 914 | typedef Matrix<Scalar, RowsAtCompileTime, ColsAtCompileTime> PlainObject; | ||
| 915 | // typedef typename internal::ref_selector<MatrixBlockView>::type Nested; | ||
| 916 | typedef _ArgType ArgType; | ||
| 917 | typedef typename internal::ref_selector<ArgType>::type ArgTypeNested; | ||
| 918 | typedef typename internal::remove_all<ArgType>::type NestedExpression; | ||
| 919 | // typedef typename Base::CoeffReturnType CoeffReturnType; | ||
| 920 | // typedef typename Base::Scalar Scalar; | ||
| 921 | |||
| 922 | template <typename Derived> | ||
| 923 | struct block_t { | ||
| 924 | typedef Block<Derived, | ||
| 925 | (AllRows ? Derived::RowsAtCompileTime : Eigen::Dynamic), | ||
| 926 | (AllCols ? Derived::ColsAtCompileTime : Eigen::Dynamic), | ||
| 927 | (AllCols ? (bool)Derived::IsRowMajor | ||
| 928 | : (AllRows ? (bool)!Derived::IsRowMajor : false))> | ||
| 929 | type; | ||
| 930 | }; | ||
| 931 | typedef typename block_t<ArgType>::type BlockXprType; | ||
| 932 | typedef typename block_t<const ArgType>::type BlockConstXprType; | ||
| 933 | |||
| 934 | typedef MatrixBlocks<_allRows, _allCols> MatrixIndices_t; | ||
| 935 | typedef typename MatrixIndices_t::segments_t Indices_t; | ||
| 936 | typedef typename internal::conditional<_allRows, const internal::empty_struct, | ||
| 937 | const Indices_t&>::type RowIndices_t; | ||
| 938 | typedef typename internal::conditional<_allCols, const internal::empty_struct, | ||
| 939 | const Indices_t&>::type ColIndices_t; | ||
| 940 | |||
| 941 | // using Base::operator=; | ||
| 942 | |||
| 943 | 49735 | MatrixBlockView(ArgType& arg, const size_type& nbRows, | |
| 944 | const RowIndices_t rows, const size_type& nbCols, | ||
| 945 | const ColIndices_t cols) | ||
| 946 | 49735 | : m_arg(arg), | |
| 947 | 49735 | m_nbRows(nbRows), | |
| 948 | 49735 | m_rows(rows), | |
| 949 | 49735 | m_nbCols(nbCols), | |
| 950 | 49735 | m_cols(cols) {} | |
| 951 | |||
| 952 | /// Valid only when _allRows or _allCols is true | ||
| 953 | 337916 | MatrixBlockView(ArgType& arg, const size_type& nbIndices, | |
| 954 | const Indices_t& indices) | ||
| 955 | 337916 | : m_arg(arg), | |
| 956 | 337916 | m_nbRows(_allRows ? arg.rows() : nbIndices), | |
| 957 | 337916 | m_rows(indices), | |
| 958 | 337916 | m_nbCols(_allCols ? arg.cols() : nbIndices), | |
| 959 | 675418 | m_cols(indices) {} | |
| 960 | |||
| 961 | 256037 | EIGEN_STRONG_INLINE size_type rows() const { return m_nbRows; } | |
| 962 | 142214 | EIGEN_STRONG_INLINE size_type cols() const { return m_nbCols; } | |
| 963 | |||
| 964 | EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type index) const { | ||
| 965 | assert(false && | ||
| 966 | "It is not possible to access the coefficients of " | ||
| 967 | "MatrixBlockView this way."); | ||
| 968 | } | ||
| 969 | EIGEN_STRONG_INLINE CoeffReturnType coeff(size_type row, | ||
| 970 | size_type col) const { | ||
| 971 | assert(false && | ||
| 972 | "It is not possible to access the coefficients of " | ||
| 973 | "MatrixBlockView this way."); | ||
| 974 | } | ||
| 975 | EIGEN_STRONG_INLINE Scalar& coeffRef(size_type index) { | ||
| 976 | assert(false && | ||
| 977 | "It is not possible to access the coefficients of " | ||
| 978 | "MatrixBlockView this way."); | ||
| 979 | } | ||
| 980 | EIGEN_STRONG_INLINE Scalar& coeffRef(size_type row, const size_type& col) { | ||
| 981 | assert(false && | ||
| 982 | "It is not possible to access the coefficients of " | ||
| 983 | "MatrixBlockView this way."); | ||
| 984 | } | ||
| 985 | template <typename Dest> | ||
| 986 | 10044 | EIGEN_STRONG_INLINE void evalTo(Dest& dst) const { | |
| 987 | 10044 | internal::eval_matrix_block_view_to<MatrixBlockView, Dest>::run(*this, dst); | |
| 988 | 10044 | } | |
| 989 | |||
| 990 | template <typename Dest> | ||
| 991 | 10044 | EIGEN_STRONG_INLINE void writeTo(Dest& dst) const { | |
| 992 | 10044 | dst.resize(rows(), cols()); | |
| 993 | 10044 | evalTo(dst.derived()); | |
| 994 | 10044 | } | |
| 995 | |||
| 996 | 9719 | EIGEN_STRONG_INLINE PlainObject eval() const { | |
| 997 | 9719 | PlainObject dst; | |
| 998 |
1/2✓ Branch 1 taken 8145 times.
✗ Branch 2 not taken.
|
9719 | writeTo(dst); |
| 999 | 9719 | return dst; | |
| 1000 | } | ||
| 1001 | |||
| 1002 | template <typename OtherDerived> | ||
| 1003 | 165481 | EIGEN_STRONG_INLINE MatrixBlockView& operator=( | |
| 1004 | const EigenBase<OtherDerived>& other) { | ||
| 1005 | EIGEN_STATIC_ASSERT_LVALUE(ArgType); | ||
| 1006 | 165481 | internal::eval_matrix_block_view_to<OtherDerived, MatrixBlockView>::run( | |
| 1007 | 26848 | other.derived(), *this); | |
| 1008 | 165481 | return *this; | |
| 1009 | } | ||
| 1010 | |||
| 1011 | 32 | EIGEN_STRONG_INLINE size_type _blocks() const { | |
| 1012 | 32 | return m_rows.size() * m_cols.size(); | |
| 1013 | } | ||
| 1014 | 140438 | EIGEN_STRONG_INLINE BlockXprType _block(const block_iterator& b) { | |
| 1015 | return internal::access_block_from_matrix_block_view< | ||
| 1016 | 140486 | BlockXprType, MatrixBlockView>::template run<ArgType>(m_arg, b.ri(), | |
| 1017 | b.ci(), b.rs(), | ||
| 1018 | 140486 | b.cs()); | |
| 1019 | } | ||
| 1020 | EIGEN_STRONG_INLINE const BlockConstXprType | ||
| 1021 | 123163 | _block(const block_iterator& b) const { | |
| 1022 | return internal::access_block_from_matrix_block_view< | ||
| 1023 | const BlockConstXprType, | ||
| 1024 | 123163 | MatrixBlockView>::template run<const ArgType>(m_arg, b.ri(), b.ci(), | |
| 1025 | 123167 | b.rs(), b.cs()); | |
| 1026 | } | ||
| 1027 | EIGEN_STRONG_INLINE block_iterator _block_iterator() const { | ||
| 1028 | return block_iterator(*this); | ||
| 1029 | } | ||
| 1030 | |||
| 1031 | 16 | EIGEN_STRONG_INLINE bool isZero( | |
| 1032 | const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const { | ||
| 1033 | 36 | for (block_iterator block(*this); block.valid(); ++block) | |
| 1034 | 28 | if (!m_arg.block(block.ri(), block.ci(), block.rs(), block.cs()) | |
| 1035 | 28 | .isZero(prec)) | |
| 1036 | 8 | return false; | |
| 1037 | 8 | return true; | |
| 1038 | } | ||
| 1039 | |||
| 1040 | ArgType& m_arg; | ||
| 1041 | size_type m_nbRows; | ||
| 1042 | RowIndices_t m_rows; | ||
| 1043 | size_type m_nbCols; | ||
| 1044 | ColIndices_t m_cols; | ||
| 1045 | }; // MatrixBlockView | ||
| 1046 | |||
| 1047 | ///\} | ||
| 1048 | |||
| 1049 | } // namespace Eigen | ||
| 1050 | |||
| 1051 | #include <hpp/constraints/impl/matrix-view-operation.hh> | ||
| 1052 | #include <hpp/constraints/impl/matrix-view.hh> | ||
| 1053 | |||
| 1054 | #undef HPP_EIGEN_USE_EVALUATOR | ||
| 1055 | |||
| 1056 | namespace hpp { | ||
| 1057 | template <int Option> | ||
| 1058 | struct prettyPrint<constraints::segment_t, Option> { | ||
| 1059 | ✗ | static std::ostream& run(std::ostream& os, const constraints::segment_t& s) { | |
| 1060 | ✗ | return os << "[ " << s.first << ", " << s.first + s.second << " ]"; | |
| 1061 | } | ||
| 1062 | }; | ||
| 1063 | } // namespace hpp | ||
| 1064 | |||
| 1065 | #endif // HPP_CONSTRAINTS_MATRIX_VIEW_HH | ||
| 1066 |