Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 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
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
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.
*/
@@ -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);
}
src/lib/matrix/matrix/Slice.hpp
+62 -41
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>;
}
src/lib/matrix/matrix/SquareMatrix.hpp
+3 -5
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};
}
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
{
assert(i < M);
src/lib/matrix/matrix/Vector2.hpp
+2 -9
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)
{
src/lib/matrix/matrix/Vector3.hpp
+3 -10
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};
}