dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

matrix: Slice templated on const and non-const matrix cases 

to avoid casting const to non-const with
`const_cast<Matrix<Type, M, N>*>(data)`
This commit is contained in:
Matthias Grob 2023-12-21 16:59:49 +01:00 committed by Daniel Agar
parent 88102d82db
commit 3e183feb49
6 changed files with 101 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/lib/matrix/matrix/Matrix.hpp
View File

@ -67,6 +67,18 @@ public:
}
}
template<size_t P, size_t Q>
Matrix(const ConstSlice<Type, M, N, P, Q> &in_slice)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = in_slice(i, j);
}
}
}
/**
* Accessors/ Assignment etc.
*/
@ -438,7 +450,7 @@ public:
}
template<size_t P, size_t Q>
const Slice<Type, P, Q, M, N> slice(size_t x0, size_t y0) const
ConstSlice<Type, P, Q, M, N> slice(size_t x0, size_t y0) const
{
return {x0, y0, this};
}
@ -449,7 +461,7 @@ public:
return {x0, y0, this};
}
const Slice<Type, 1, N, M, N> row(size_t i) const
ConstSlice<Type, 1, N, M, N> row(size_t i) const
{
return slice<1, N>(i, 0);
}
@ -459,7 +471,7 @@ public:
return slice<1, N>(i, 0);
}
const Slice<Type, M, 1, M, N> col(size_t j) const
ConstSlice<Type, M, 1, M, N> col(size_t j) const
{
return slice<M, 1>(0, j);
}

103
src/lib/matrix/matrix/Slice.hpp
View File

@ -21,14 +21,16 @@ class Matrix;
template<typename Type, size_t M>
class Vector;
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
class Slice
template <typename MatrixT, typename Type, size_t P, size_t Q, size_t M, size_t N>
class SliceT
{
public:
Slice(size_t x0, size_t y0, const Matrix<Type, M, N> *data) :
using Self = SliceT<MatrixT, Type, P, Q, M, N>;
SliceT(size_t x0, size_t y0, MatrixT *data) :
_x0(x0),
_y0(y0),
_data(const_cast<Matrix<Type, M, N>*>(data))
_data(data)
{
static_assert(P <= M, "Slice rows bigger than backing matrix");
static_assert(Q <= N, "Slice cols bigger than backing matrix");
@ -36,7 +38,7 @@ public:
assert(y0 + Q <= N);
}
Slice(const Slice<Type, P, Q, M, N> &other) = default;
SliceT(const Self &other) = default;
const Type &operator()(size_t i, size_t j) const
{
@ -47,7 +49,6 @@ public:
}
Type &operator()(size_t i, size_t j)
{
assert(i < P);
assert(j < Q);
@ -56,15 +57,15 @@ public:
}
// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
Self &operator=(const Self &other)
{
return this->operator=<M, N>(other);
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
Self &operator=(const SliceT<Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
Self &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -75,9 +76,10 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator=(const Matrix<Type, P, Q> &other)
template<size_t MM, size_t NN>
SliceT<MatrixT, Type, P, Q, M, N> &operator=(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -88,9 +90,22 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other(i, j);
}
}
return self;
}
SliceT<MatrixT, Type, P, Q, M, N> &operator=(const Type &other)
{
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -103,9 +118,9 @@ public:
// allow assigning vectors to a slice that are in the axis
template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
Slice<Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
{
Slice<Type, 1, Q, M, N> &self = *this;
SliceT<MatrixT, Type, 1, Q, M, N> &self = *this;
for (size_t j = 0; j < Q; j++) {
self(0, j) = other(j);
@ -115,9 +130,9 @@ public:
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N> &operator+=(const Slice<Type, P, Q, MM, NN> &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator+=(const SliceT<MatrixT, Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -128,9 +143,9 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator+=(const Matrix<Type, P, Q> &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator+=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -141,9 +156,9 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator+=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator+=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -155,9 +170,9 @@ public:
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N> &operator-=(const Slice<Type, P, Q, MM, NN> &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator-=(const SliceT<MatrixT, Type, P, Q, MM, NN> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -168,9 +183,9 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator-=(const Matrix<Type, P, Q> &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator-=(const Matrix<Type, P, Q> &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -181,9 +196,9 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator-=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator-=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -194,9 +209,9 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator*=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator*=(const Type &other)
{
Slice<Type, P, Q, M, N> &self = *this;
SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -207,14 +222,14 @@ public:
return self;
}
Slice<Type, P, Q, M, N> &operator/=(const Type &other)
SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
{
return operator*=(Type(1) / other);
}
Matrix<Type, P, Q> operator*(const Type &other) const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
Matrix<Type, P, Q> res;
for (size_t i = 0; i < P; i++) {
@ -228,25 +243,25 @@ public:
Matrix<Type, P, Q> operator/(const Type &other) const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
return self * (Type(1) / other);
}
template<size_t R, size_t S>
const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
const SliceT<MatrixT, Type, R, S, M, N> slice(size_t x0, size_t y0) const
{
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
return SliceT<MatrixT, Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
}
template<size_t R, size_t S>
Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
SliceT<MatrixT, Type, R, S, M, N> slice(size_t x0, size_t y0)
{
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
return SliceT<MatrixT, Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
}
void copyTo(Type dst[P * Q]) const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -257,7 +272,7 @@ public:
void copyToColumnMajor(Type dst[P * Q]) const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
@ -268,7 +283,7 @@ public:
Vector < Type, P < Q ? P : Q > diag() const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
Vector < Type, P < Q ? P : Q > res;
for (size_t j = 0; j < (P < Q ? P : Q); j++) {
@ -280,7 +295,7 @@ public:
Type norm_squared() const
{
const Slice<Type, P, Q, M, N> &self = *this;
const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
Type accum(0);
for (size_t i = 0; i < P; i++) {
@ -338,7 +353,13 @@ public:
private:
size_t _x0, _y0;
Matrix<Type, M, N> *_data;
MatrixT *_data;
};
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
using Slice = SliceT<Matrix<Type, M, N>, Type, P, Q, M, N>;
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
using ConstSlice = SliceT<const Matrix<Type, M, N>, Type, P, Q, M, N>;
}

8
src/lib/matrix/matrix/SquareMatrix.hpp
View File

@ -36,10 +36,8 @@ public:
{
}
template<size_t P, size_t Q>
SquareMatrix(const Slice<Type, M, M, P, Q> &in_slice) : Matrix<Type, M, M>(in_slice)
{
}
using base = Matrix<Type, M, M>;
using base::base;
SquareMatrix<Type, M> &operator=(const Matrix<Type, M, M> &other)
{
@ -55,7 +53,7 @@ public:
}
template<size_t P, size_t Q>
const Slice<Type, P, Q, M, M> slice(size_t x0, size_t y0) const
ConstSlice<Type, P, Q, M, M> slice(size_t x0, size_t y0) const
{
return {x0, y0, this};
}

16
src/lib/matrix/matrix/Vector.hpp
View File

@ -47,6 +47,22 @@ public:
}
}
template<size_t P, size_t Q>
Vector(const ConstSlice<Type, M, 1, P, Q> &slice_in) :
Matrix<Type, M, 1>(slice_in)
{
}
template<size_t P, size_t Q, size_t DUMMY = 1>
Vector(const ConstSlice<Type, 1, M, P, Q> &slice_in)
{
Vector &self(*this);
for (size_t i = 0; i < M; i++) {
self(i) = slice_in(0, i);
}
}
inline const Type &operator()(size_t i) const
{
assert(i < M);

11
src/lib/matrix/matrix/Vector2.hpp
View File

@ -40,15 +40,8 @@ public:
v(1) = y;
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 2, 1, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 1, 2, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{
}
using base = Vector<Type, 2>;
using base::base;
explicit Vector2(const Vector3 &other)
{

13
src/lib/matrix/matrix/Vector3.hpp
View File

@ -43,15 +43,8 @@ public:
v(2) = z;
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 3, 1, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{
}
using base = Vector<Type, 3>;
using base::base;
Vector3 cross(const Matrix31 &b) const
{
@ -103,7 +96,7 @@ public:
return (*this).cross(b);
}
const Slice<Type, 2, 1, 3, 1> xy() const
ConstSlice<Type, 2, 1, 3, 1> xy() const
{
return {0, 0, this};
}