aligator  0.15.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
35 : m_data()
36 , m_rowDims()
37 , m_colDims()
38 , m_rowIndices()
39 , m_colIndices()
40 , m_totalRows(0)
41 , m_totalCols(0) {}
42
53
54 template <typename Other>
55 BlkMatrix(const Eigen::MatrixBase<Other> &data, const row_dim_t &rowDims,
56 const col_dim_t &colDims)
57 : m_data(data.derived())
58 , //
63 , m_totalRows(0)
64 , m_totalCols(0) {
65 initialize();
66 }
67
68 template <typename Other>
69 BlkMatrix(Eigen::MatrixBase<Other> &data, const row_dim_t &rowDims,
70 const col_dim_t &colDims)
71 : m_data(data.derived())
72 , //
77 , m_totalRows(0)
78 , m_totalCols(0) {
79 initialize();
80 }
81
83 template <typename Other>
84 BlkMatrix(const Eigen::MatrixBase<Other> &data, const row_dim_t &dims)
85 : BlkMatrix(data, dims, {data.cols()}) {}
86
88 template <typename Other>
89 BlkMatrix(Eigen::MatrixBase<Other> &data, const row_dim_t &dims)
90 : BlkMatrix(data, dims, {data.cols()}) {}
91
92 operator Eigen::Ref<PlainObject>() { return m_data; }
93 operator Eigen::Ref<const PlainObject>() const { return m_data; }
94
96 explicit BlkMatrix(const row_dim_t &dims)
97 : BlkMatrix(dims, {1}) {
98 static_assert(IsVectorAtCompileTime,
99 "Constructor only supported for vector types.");
100 }
101
103 inline auto operator()(size_t i, size_t j) {
104 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
105 m_colDims[j]);
106 }
107
109 inline auto operator()(size_t i, size_t j) const {
110 return m_data.block(m_rowIndices[i], m_colIndices[j], m_rowDims[i],
111 m_colDims[j]);
112 }
113
114 inline auto blockRow(size_t i) {
115 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
116 }
117
118 inline auto blockRow(size_t i) const {
119 return m_data.middleRows(m_rowIndices[i], m_rowDims[i]);
120 }
121
122 inline auto blockCol(size_t j) const {
123 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
124 }
125
126 inline auto blockCol(size_t j) {
127 return m_data.middleCols(m_colIndices[j], m_colDims[j]);
128 }
129
130 auto blockSegment(size_t i) {
131 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
132 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
133 }
134
135 auto blockSegment(size_t i) const {
136 EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixType);
137 return m_data.segment(m_rowIndices[i], m_rowDims[i]);
138 }
139
140 inline auto operator[](size_t i) { return blockSegment(i); }
141
142 inline auto operator[](size_t i) const { return blockSegment(i); }
143
145 template <typename Other>
146 BlkMatrix &operator=(const Eigen::MatrixBase<Other> &other) {
147 assert(other.rows() == m_data.rows());
148 assert(other.cols() == m_data.cols());
149 m_data = other;
150 return *this;
151 }
152
153 void setZero() { m_data.setZero(); }
155
157 out.setZero();
158 return out;
159 }
160
161 template <typename Other> inline void swap(BlkMatrix<Other, N, M> &other) {
162 m_data.swap(other.matrix());
163 }
164
165 MatrixType &matrix() { return m_data; }
166 const MatrixType &matrix() const { return m_data; }
167
168 const row_dim_t &rowDims() const { return m_rowDims; }
169 const row_dim_t &rowIndices() const { return m_rowIndices; }
170 const col_dim_t &colDims() const { return m_colDims; }
171 const col_dim_t &colIndices() const { return m_colIndices; }
172
173 long rows() const { return m_totalRows; }
174 long cols() const { return m_totalCols; }
175
176 auto topBlkRows(size_t n) {
177 using OutType = BlkMatrix<Eigen::Ref<MatrixType>, -1, M>;
178 std::vector<long> subRowDims;
179 subRowDims.resize(n);
180 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
181 long ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
182 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
183 }
184
185 template <size_t n> auto topBlkRows() {
186 static_assert(n <= N,
187 "Cannot take n block rows of matrix with <n block rows.");
188 using RefType = Eigen::Ref<MatrixType>;
189 using OutType = BlkMatrix<RefType, n, M>;
190 std::array<long, n> subRowDims;
191 std::copy_n(m_rowDims.cbegin(), n, subRowDims.begin());
192 long ntr = std::accumulate(subRowDims.begin(), subRowDims.end(), 0);
193 return OutType(m_data.topRows(ntr), subRowDims, m_colDims);
194 }
195
196 friend std::ostream &operator<<(std::ostream &oss, const BlkMatrix &self) {
197 return oss << self.m_data;
198 }
199
200protected:
208
209 void initialize() {
210 for (size_t i = 0; i < m_rowDims.size(); i++) {
213 }
214 for (size_t i = 0; i < m_colDims.size(); i++) {
217 }
219 }
220};
221
222} // 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.