fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 13:58:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
to avoid casting const to non-const with
`const_cast<Matrix<Type, M, N>*>(data)`
This commit is contained in:
Matthias Grob
2024-01-30 11:46:33 -05:00
committed by Daniel Agar
parent 88102d82db
commit 3e183feb49
6 changed files with 101 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Matrix.hpp
+15 -3
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. * Accessors/ Assignment etc.
*/ */
@@ -438,7 +450,7 @@ public:
} }
template<size_t P, size_t Q> 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}; return {x0, y0, this};
} }
@@ -449,7 +461,7 @@ public:
return {x0, y0, this}; 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); return slice<1, N>(i, 0);
} }
@@ -459,7 +471,7 @@ public:
return slice<1, N>(i, 0); 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); return slice<M, 1>(0, j);
} }
src/lib/matrix/matrix/Slice.hpp
+62 -41
View File
@@ -21,14 +21,16 @@ class Matrix;
template<typename Type, size_t M> template<typename Type, size_t M>
class Vector; class Vector;
template <typename Type, size_t P, size_t Q, size_t M, size_t N> template <typename MatrixT, typename Type, size_t P, size_t Q, size_t M, size_t N>
class Slice class SliceT
{ {
public: 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), _x0(x0),
_y0(y0), _y0(y0),
_data(const_cast<Matrix<Type, M, N>*>(data)) _data(data)
{ {
static_assert(P <= M, "Slice rows bigger than backing matrix"); static_assert(P <= M, "Slice rows bigger than backing matrix");
static_assert(Q <= N, "Slice cols bigger than backing matrix"); static_assert(Q <= N, "Slice cols bigger than backing matrix");
@@ -36,7 +38,7 @@ public:
assert(y0 + Q <= N); 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 const Type &operator()(size_t i, size_t j) const
{ {
@@ -47,7 +49,6 @@ public:
} }
Type &operator()(size_t i, size_t j) Type &operator()(size_t i, size_t j)
{ {
assert(i < P); assert(i < P);
assert(j < Q); assert(j < Q);
@@ -56,15 +57,15 @@ public:
} }
// Separate function needed otherwise the default copy constructor matches before the deep copy implementation // 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); return this->operator=<M, N>(other);
} }
template<size_t MM, size_t NN> 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -75,9 +76,10 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -88,9 +90,22 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -103,9 +118,9 @@ public:
// allow assigning vectors to a slice that are in the axis // 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 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++) { for (size_t j = 0; j < Q; j++) {
self(0, j) = other(j); self(0, j) = other(j);
@@ -115,9 +130,9 @@ public:
} }
template<size_t MM, size_t NN> 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -128,9 +143,9 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -141,9 +156,9 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -155,9 +170,9 @@ public:
} }
template<size_t MM, size_t NN> 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -168,9 +183,9 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -181,9 +196,9 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -194,9 +209,9 @@ public:
return self; 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -207,14 +222,14 @@ public:
return self; 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); return operator*=(Type(1) / other);
} }
Matrix<Type, P, Q> operator*(const Type &other) const 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; Matrix<Type, P, Q> res;
for (size_t i = 0; i < P; i++) { for (size_t i = 0; i < P; i++) {
@@ -228,25 +243,25 @@ public:
Matrix<Type, P, Q> operator/(const Type &other) const 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); return self * (Type(1) / other);
} }
template<size_t R, size_t S> 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> 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 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -257,7 +272,7 @@ public:
void copyToColumnMajor(Type dst[P * Q]) const 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 i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) { for (size_t j = 0; j < Q; j++) {
@@ -268,7 +283,7 @@ public:
Vector < Type, P < Q ? P : Q > diag() const 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; Vector < Type, P < Q ? P : Q > res;
for (size_t j = 0; j < (P < Q ? P : Q); j++) { for (size_t j = 0; j < (P < Q ? P : Q); j++) {
@@ -280,7 +295,7 @@ public:
Type norm_squared() const 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); Type accum(0);
for (size_t i = 0; i < P; i++) { for (size_t i = 0; i < P; i++) {
@@ -338,7 +353,13 @@ public:
private: private:
size_t _x0, _y0; 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>;
} }
src/lib/matrix/matrix/SquareMatrix.hpp
+3 -5
View File
@@ -36,10 +36,8 @@ public:
{ {
} }
template<size_t P, size_t Q> using base = Matrix<Type, M, M>;
SquareMatrix(const Slice<Type, M, M, P, Q> &in_slice) : Matrix<Type, M, M>(in_slice) using base::base;
{
}
SquareMatrix<Type, M> &operator=(const Matrix<Type, M, M> &other) SquareMatrix<Type, M> &operator=(const Matrix<Type, M, M> &other)
{ {
@@ -55,7 +53,7 @@ public:
} }
template<size_t P, size_t Q> 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}; return {x0, y0, this};
} }
src/lib/matrix/matrix/Vector.hpp
+16
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 inline const Type &operator()(size_t i) const
{ {
assert(i < M); assert(i < M);
src/lib/matrix/matrix/Vector2.hpp
+2 -9
View File
@@ -40,15 +40,8 @@ public:
v(1) = y; v(1) = y;
} }
template<size_t P, size_t Q> using base = Vector<Type, 2>;
Vector2(const Slice<Type, 2, 1, P, Q> &slice_in) : Vector<Type, 2>(slice_in) using base::base;
{
}
template<size_t P, size_t Q>
Vector2(const Slice<Type, 1, 2, P, Q> &slice_in) : Vector<Type, 2>(slice_in)
{
}
explicit Vector2(const Vector3 &other) explicit Vector2(const Vector3 &other)
{ {
src/lib/matrix/matrix/Vector3.hpp
+3 -10
View File
@@ -43,15 +43,8 @@ public:
v(2) = z; v(2) = z;
} }
template<size_t P, size_t Q> using base = Vector<Type, 3>;
Vector3(const Slice<Type, 3, 1, P, Q> &slice_in) : Vector<Type, 3>(slice_in) using base::base;
{
}
template<size_t P, size_t Q>
Vector3(const Slice<Type, 1, 3, P, Q> &slice_in) : Vector<Type, 3>(slice_in)
{
}
Vector3 cross(const Matrix31 &b) const Vector3 cross(const Matrix31 &b) const
{ {
@@ -103,7 +96,7 @@ public:
return (*this).cross(b); 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}; return {0, 0, this};
} }