-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
vector_base.h
270 lines (236 loc) · 9.55 KB
/
vector_base.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
#pragma once
#include <initializer_list>
#include <ostream>
#include <stdexcept>
#include <utility>
#include <fmt/format.h>
#include "drake/common/default_scalars.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/drake_throw.h"
#include "drake/common/eigen_types.h"
#include "drake/common/nice_type_name.h"
#include "drake/common/unused.h"
namespace drake {
namespace systems {
/// VectorBase is an abstract base class that real-valued signals
/// between Systems and real-valued System state vectors must implement.
/// Classes that inherit from VectorBase will typically provide names
/// for the elements of the vector, and may also provide other
/// computations for the convenience of Systems handling the
/// signal. The vector is always a column vector. It may or may not
/// be contiguous in memory. Contiguous subclasses should typically
/// inherit from BasicVector, not from VectorBase directly.
///
/// @tparam_default_scalar
template <typename T>
class VectorBase {
public:
// VectorBase objects are neither copyable nor moveable.
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(VectorBase)
virtual ~VectorBase() {}
/// Returns the number of elements in the vector.
///
/// Implementations should ensure this operation is O(1) and allocates no
/// memory.
virtual int size() const = 0;
/// Returns the element at the given index in the vector.
/// @pre 0 <= `index` < size()
T& operator[](int index) {
DRAKE_ASSERT(index >= 0);
return DoGetAtIndexUnchecked(index);
}
/// Returns the element at the given index in the vector.
/// @pre 0 <= `index` < size()
const T& operator[](int index) const {
DRAKE_ASSERT(index >= 0);
return DoGetAtIndexUnchecked(index);
}
/// Returns the element at the given index in the vector.
/// @throws std::exception if the index is >= size() or negative.
/// Consider operator[]() instead if bounds-checking is unwanted.
const T& GetAtIndex(int index) const {
if (index < 0) { this->ThrowOutOfRange(index); }
return DoGetAtIndexChecked(index);
}
/// Returns the element at the given index in the vector.
/// @throws std::exception if the index is >= size() or negative.
/// Consider operator[]() instead if bounds-checking is unwanted.
T& GetAtIndex(int index) {
if (index < 0) { this->ThrowOutOfRange(index); }
return DoGetAtIndexChecked(index);
}
/// Replaces the state at the given index with the value.
/// @throws std::exception if the index is >= size().
/// Consider operator[]() instead if bounds-checking is unwanted.
void SetAtIndex(int index, const T& value) {
GetAtIndex(index) = value;
}
/// Replaces the entire vector with the contents of @p value.
/// @throws std::exception if @p value is not a column vector with size()
/// rows.
///
/// Implementations should ensure this operation is O(N) in the size of the
/// value and allocates no memory.
virtual void SetFrom(const VectorBase<T>& value) {
const int n = value.size();
if (n != size()) { this->ThrowMismatchedSize(n); }
for (int i = 0; i < n; ++i) {
(*this)[i] = value[i];
}
}
/// Replaces the entire vector with the contents of @p value.
/// @throws std::exception if @p value is not a column vector with size()
/// rows.
///
/// Implementations should ensure this operation is O(N) in the size of the
/// value and allocates no memory.
virtual void SetFromVector(const Eigen::Ref<const VectorX<T>>& value) {
const int n = value.rows();
if (n != size()) { this->ThrowMismatchedSize(n); }
for (int i = 0; i < n; ++i) {
(*this)[i] = value[i];
}
}
/// Sets all elements of this vector to zero.
virtual void SetZero() {
const int n = size();
for (int i = 0; i < n; ++i) {
(*this)[i] = T(0.0);
}
}
/// Copies this entire %VectorBase into a contiguous Eigen Vector.
///
/// Implementations should ensure this operation is O(N) in the size of the
/// value and allocates only the O(N) memory that it returns.
virtual VectorX<T> CopyToVector() const {
VectorX<T> vec(size());
for (int i = 0; i < size(); ++i) {
vec[i] = (*this)[i];
}
return vec;
}
/// Copies this entire %VectorBase into a pre-sized Eigen Vector.
///
/// Implementations should ensure this operation is O(N) in the size of the
/// value.
/// @throws std::exception if `vec` is the wrong size.
virtual void CopyToPreSizedVector(EigenPtr<VectorX<T>> vec) const {
DRAKE_THROW_UNLESS(vec != nullptr);
const int n = vec->rows();
if (n != size()) { this->ThrowMismatchedSize(n); }
for (int i = 0; i < n; ++i) {
(*vec)[i] = (*this)[i];
}
}
/// Adds a scaled version of this vector to Eigen vector @p vec.
/// @throws std::exception if `vec` is the wrong size.
///
/// Implementations should ensure this operation remains O(N) in the size of
/// the value and allocates no memory.
virtual void ScaleAndAddToVector(const T& scale,
EigenPtr<VectorX<T>> vec) const {
DRAKE_THROW_UNLESS(vec != nullptr);
const int n = vec->rows();
if (n != size()) { this->ThrowMismatchedSize(n); }
for (int i = 0; i < n; ++i) {
(*vec)[i] += scale * (*this)[i];
}
}
/// Add in scaled vector @p rhs to this vector.
/// @throws std::exception if @p rhs is a different size than this.
VectorBase& PlusEqScaled(const T& scale, const VectorBase<T>& rhs) {
return PlusEqScaled({{scale, rhs}});
}
/// Add in multiple scaled vectors to this vector.
/// @throws std::exception if any rhs are a different size than this.
VectorBase& PlusEqScaled(const std::initializer_list<
std::pair<T, const VectorBase<T>&>>& rhs_scale) {
const int n = size();
for (const auto& [scale, rhs] : rhs_scale) {
unused(scale);
const int rhs_n = rhs.size();
if (rhs_n != n) { this->ThrowMismatchedSize(rhs_n); }
}
DoPlusEqScaled(rhs_scale);
return *this;
}
/// Add in vector @p rhs to this vector.
/// @throws std::exception if @p rhs is a different size than this.
VectorBase& operator+=(const VectorBase<T>& rhs) {
return PlusEqScaled(T(1), rhs);
}
/// Subtract in vector @p rhs to this vector.
/// @throws std::exception if @p rhs is a different size than this.
VectorBase& operator-=(const VectorBase<T>& rhs) {
return PlusEqScaled(T(-1), rhs);
}
/// Get the bounds for the elements.
/// If lower and upper are both empty size vectors, then there are no bounds.
/// Otherwise, the bounds are (*lower)(i) <= GetAtIndex(i) <= (*upper)(i)
/// The default output is no bounds.
virtual void GetElementBounds(Eigen::VectorXd* lower,
Eigen::VectorXd* upper) const {
lower->resize(0);
upper->resize(0);
}
protected:
VectorBase() {}
/// Implementations should ensure this operation is O(1) and allocates no
/// memory. The index need not be validated when in release mode.
virtual const T& DoGetAtIndexUnchecked(int index) const = 0;
/// Implementations should ensure this operation is O(1) and allocates no
/// memory. The index need not be validated when in release mode.
virtual T& DoGetAtIndexUnchecked(int index) = 0;
/// Implementations should ensure this operation is O(1) and allocates no
/// memory. The index has already been checked for negative, but not size.
/// Implementations must throw an exception when index >= size().
virtual const T& DoGetAtIndexChecked(int index) const = 0;
/// Implementations should ensure this operation is O(1) and allocates no
/// memory. The index has already been checked for negative, but not size.
/// Implementations must throw an exception when index >= size().
virtual T& DoGetAtIndexChecked(int index) = 0;
/// Adds in multiple scaled vectors to this vector. All vectors
/// are guaranteed to be the same size.
///
/// You should override this method if possible with a more efficient
/// approach that leverages structure; the default implementation performs
/// element-by-element computations that are likely inefficient, but even
/// this implementation minimizes memory accesses for efficiency. If the
/// vector is contiguous, for example, implementations that leverage SIMD
/// operations should be far more efficient. Overriding implementations should
/// ensure that this operation remains O(N) in the size of
/// the value and allocates no memory.
virtual void DoPlusEqScaled(const std::initializer_list<
std::pair<T, const VectorBase<T>&>>& rhs_scale) {
const int n = size();
for (int i = 0; i < n; ++i) {
T value(0);
for (const auto& [scale, rhs] : rhs_scale) {
value += rhs[i] * scale;
}
(*this)[i] += value;
}
}
[[noreturn]] void ThrowOutOfRange(int index) const {
throw std::out_of_range(fmt::format(
"Index {} is not within [0, {}) while accessing {}.",
index, size(), NiceTypeName::Get(*this)));
}
[[noreturn]] void ThrowMismatchedSize(int other_size) const {
throw std::out_of_range(fmt::format(
"Operand vector size {} does not match this {} size {}",
other_size, NiceTypeName::Get(*this), size()));
}
};
/// Allows a VectorBase<T> to be streamed into a string as though it were a
/// RowVectorX<T>. This is useful for debugging purposes.
template <typename T>
std::ostream& operator<<(std::ostream& os, const VectorBase<T>& vec) {
os << vec.CopyToVector().transpose();
return os;
}
} // namespace systems
} // namespace drake
DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
class ::drake::systems::VectorBase)