aligator  0.13.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
 
Loading...
Searching...
No Matches
blk-matrix.hpp
Go to the documentation of this file.
1
3#pragma once
4
5#include "aligator/math.hpp"
6#include <array>
7#include <numeric>
8
9namespace aligator {
10
13template <typename _MatrixType, int _N, int _M = _N> class BlkMatrix {
14public:
15 using MatrixType = _MatrixType;
16 using PlainObject = typename MatrixType::PlainObject;
17 using Scalar = typename MatrixType::Scalar;
18 enum { N = _N, M = _M, Options = PlainObject::Options };
19 static constexpr bool IsVectorAtCompileTime =
20 MatrixType::IsVectorAtCompileTime;
21
22 using row_dim_t = std::conditional_t<N != -1, std::array<long, size_t(N)>,
23 std::vector<long>>;
24 using col_dim_t = std::conditional_t<M != -1, std::array<long, size_t(M)>,
25 std::vector<long>>;
26
27 static_assert(N != 0 && M != 0,
28 "The BlkMatrix template class only supports nonzero numbers of "
29 "blocks in either direction.");
30
31 static_assert(!IsVectorAtCompileTime || (M == 1),
32 "Compile-time vector cannot have more than one column block.");
33
37
43
44 template <typename Other>
45 BlkMatrix(const Eigen::MatrixBase<Other> &data, const row_dim_t &rowDims,
46 const col_dim_t &colDims)
47 : m_data(data.derived()), //
50 initialize();
51 }
52
53 template <typename Other>
54 BlkMatrix(Eigen::MatrixBase<Other> &data, const row_dim_t &rowDims,
55 const col_dim_t &colDims)
56 : m_data(data.derived()), //
59 initialize();
60 }
61
63 template <typename Other>
64 BlkMatrix(const Eigen::MatrixBase<Other> &data, const row_dim_t &dims)
65 : BlkMatrix(data, dims, {data.cols()}) {}
66
68 template <typename Other>
69 BlkMatrix(Eigen::MatrixBase<Other> &data, const row_dim_t &dims)
70 : BlkMatrix(data, dims, {data.cols()}) {}
71
72 operator Eigen::Ref<PlainObject>() { return m_data; }
73 operator Eigen::Ref<const PlainObject>() const { return m_data; }
74
76 explicit BlkMatrix(const row_dim_t &dims) : BlkMatrix(dims, {1}) {
77 static_assert(IsVectorAtCompileTime,
78 "Constructor only supported for vector types.");
79 }
80
82 inline auto operator()(size_t i, size_t j) {
83 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
84 m_colDims[j]);
85 }
86
88 inline auto operator()(size_t i, size_t j) const {
89 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
90 m_colDims[j]);
91 }
92
93 inline auto blockRow(size_t i) {
94 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
95 }
96
97 inline auto blockRow(size_t i) const {
98 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
99 }
100
101 inline auto blockCol(size_t j) const {
102 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
103 }
104
105 inline auto blockCol(size_t j) {
106 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
107 }
108
109 auto blockSegment(size_t i) {
110 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
111 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
112 }
113
114 auto blockSegment(size_t i) const {
115 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
116 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
117 }
118
119 inline auto operator[](size_t i) { return blockSegment(i); }
120
121 inline auto operator[](size_t i) const { return blockSegment(i); }
122
124 template <typename Other>
125 BlkMatrix &operator=(const Eigen::MatrixBase<Other> &other) {
126 assert(other.rows() == m_data.rows());
127 assert(other.cols() == m_data.cols());
128 m_data = other;
129 return *this;
130 }
131
132 void setZero() { m_data.setZero(); }
134
136 out.setZero();
137 return out;
138 }
139
140 template <typename Other> inline void swap(BlkMatrix<Other, N, M> &other) {
141 m_data.swap(other.matrix());
142 }
143
144 MatrixType &matrix() { return m_data; }
145 const MatrixType &matrix() const { return m_data; }
146
147 const row_dim_t &rowDims() const { return m_rowDims; }
148 const row_dim_t &rowIndices() const { return m_rowIndices; }
149 const col_dim_t &colDims() const { return m_colDims; }
150 const col_dim_t &colIndices() const { return m_colIndices; }
151
152 long rows() const { return m_totalRows; }
153 long cols() const { return m_totalCols; }
154
155 auto topBlkRows(size_t n) {
156 using OutType = BlkMatrix<Eigen::Ref<MatrixType>, -1, M>;
157 std::vector<long> subRowDims;
158 subRowDims.resize(n);
159 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
160 long ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
161 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
162 }
163
164 template <size_t n> auto topBlkRows() {
165 static_assert(n <= N,
166 "Cannot take n block rows of matrix with <n block rows.");
167 using RefType = Eigen::Ref<MatrixType>;
168 using OutType = BlkMatrix<RefType, n, M>;
169 std::array<long, n> subRowDims;
170 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
171 long ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
172 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
173 }
174
175 friend std::ostream &operator<<(std::ostream &oss, const BlkMatrix &self) {
176 return oss << self.m_data;
177 }
178
179protected:
187
188 void initialize() {
189 for (size_t i = 0; i < m_rowDims.size(); i++) {
192 }
193 for (size_t i = 0; i < m_colDims.size(); i++) {
196 }
198 }
199};
200
201} // namespace aligator
auto blockSegment(size_t i)
void swap(BlkMatrix< Other, N, M > &other)
friend std::ostream & operator<<(std::ostream &oss, const BlkMatrix &self)
auto blockSegment(size_t i) const
auto topBlkRows(size_t n)
typename MatrixType::PlainObject PlainObject
BlkMatrix(const Eigen::MatrixBase< Other > &data, const row_dim_t &dims)
Only-rows constructor (only for vectors)
auto blockRow(size_t i)
MatrixType & matrix()
BlkMatrix(const row_dim_t &dims)
Only-rows constructor (only for vectors)
auto blockCol(size_t j)
auto operator()(size_t i, size_t j)
Get the block in position ( i, j )
BlkMatrix(const Eigen::MatrixBase< Other > &data, const row_dim_t &rowDims, const col_dim_t &colDims)
static BlkMatrix Zero(const row_dim_t &rowDims, const col_dim_t &colDims)
auto operator[](size_t i) const
auto blockCol(size_t j) const
auto operator()(size_t i, size_t j) const
const row_dim_t & rowIndices() const
BlkMatrix(Eigen::MatrixBase< Other > &data, const row_dim_t &dims)
Only-rows constructor (only for vectors)
const MatrixType & matrix() const
auto blockRow(size_t i) const
std::conditional_t< M !=-1, std::array< long, size_t(M)>, std::vector< long > > col_dim_t
const col_dim_t & colIndices() const
BlkMatrix & operator=(const Eigen::MatrixBase< Other > &other)
Set the data to be equal to some other Eigen object.
BlkMatrix(const row_dim_t &rowDims, const col_dim_t &colDims)
std::conditional_t< N !=-1, std::array< long, size_t(N)>, std::vector< long > > row_dim_t
BlkMatrix(Eigen::MatrixBase< Other > &data, const row_dim_t &rowDims, const col_dim_t &colDims)
auto operator[](size_t i)
Math utilities.
Main package namespace.