diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index d3b97765d6..e3eecf2dd8 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -67,6 +67,18 @@ public: } } + template + Matrix(const ConstSlice &in_slice) + { + Matrix &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 - const Slice slice(size_t x0, size_t y0) const + ConstSlice slice(size_t x0, size_t y0) const { return {x0, y0, this}; } @@ -449,7 +461,7 @@ public: return {x0, y0, this}; } - const Slice row(size_t i) const + ConstSlice row(size_t i) const { return slice<1, N>(i, 0); } @@ -459,7 +471,7 @@ public: return slice<1, N>(i, 0); } - const Slice col(size_t j) const + ConstSlice col(size_t j) const { return slice(0, j); } diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index 3a7b51c485..57f7cfcb70 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -21,14 +21,16 @@ class Matrix; template class Vector; -template -class Slice +template +class SliceT { public: - Slice(size_t x0, size_t y0, const Matrix *data) : + using Self = SliceT; + + SliceT(size_t x0, size_t y0, MatrixT *data) : _x0(x0), _y0(y0), - _data(const_cast*>(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 &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 &operator=(const Slice &other) + Self &operator=(const Self &other) { return this->operator=(other); } template - Slice &operator=(const Slice &other) + Self &operator=(const SliceT, Type, P, Q, MM, NN> &other) { - Slice &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 &operator=(const Matrix &other) + template + SliceT &operator=(const SliceT, Type, P, Q, MM, NN> &other) { - Slice &self = *this; + SliceT &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 &operator=(const Type &other) + SliceT &operator=(const Matrix &other) { - Slice &self = *this; + SliceT &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 &operator=(const Type &other) + { + SliceT &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 // make this a template function since it only exists for some instantiations - Slice &operator=(const Vector &other) + SliceT &operator=(const Vector &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t j = 0; j < Q; j++) { self(0, j) = other(j); @@ -115,9 +130,9 @@ public: } template - Slice &operator+=(const Slice &other) + SliceT &operator+=(const SliceT &other) { - Slice &self = *this; + SliceT &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 &operator+=(const Matrix &other) + SliceT &operator+=(const Matrix &other) { - Slice &self = *this; + SliceT &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 &operator+=(const Type &other) + SliceT &operator+=(const Type &other) { - Slice &self = *this; + SliceT &self = *this; for (size_t i = 0; i < P; i++) { for (size_t j = 0; j < Q; j++) { @@ -155,9 +170,9 @@ public: } template - Slice &operator-=(const Slice &other) + SliceT &operator-=(const SliceT &other) { - Slice &self = *this; + SliceT &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 &operator-=(const Matrix &other) + SliceT &operator-=(const Matrix &other) { - Slice &self = *this; + SliceT &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 &operator-=(const Type &other) + SliceT &operator-=(const Type &other) { - Slice &self = *this; + SliceT &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 &operator*=(const Type &other) + SliceT &operator*=(const Type &other) { - Slice &self = *this; + SliceT &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 &operator/=(const Type &other) + SliceT &operator/=(const Type &other) { return operator*=(Type(1) / other); } Matrix operator*(const Type &other) const { - const Slice &self = *this; + const SliceT &self = *this; Matrix res; for (size_t i = 0; i < P; i++) { @@ -228,25 +243,25 @@ public: Matrix operator/(const Type &other) const { - const Slice &self = *this; + const SliceT &self = *this; return self * (Type(1) / other); } template - const Slice slice(size_t x0, size_t y0) const + const SliceT slice(size_t x0, size_t y0) const { - return Slice(x0 + _x0, y0 + _y0, _data); + return SliceT(x0 + _x0, y0 + _y0, _data); } template - Slice slice(size_t x0, size_t y0) + SliceT slice(size_t x0, size_t y0) { - return Slice(x0 + _x0, y0 + _y0, _data); + return SliceT(x0 + _x0, y0 + _y0, _data); } void copyTo(Type dst[P * Q]) const { - const Slice &self = *this; + const SliceT &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 &self = *this; + const SliceT &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 &self = *this; + const SliceT &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 &self = *this; + const SliceT &self = *this; Type accum(0); for (size_t i = 0; i < P; i++) { @@ -338,7 +353,13 @@ public: private: size_t _x0, _y0; - Matrix *_data; + MatrixT *_data; }; +template +using Slice = SliceT, Type, P, Q, M, N>; + +template +using ConstSlice = SliceT, Type, P, Q, M, N>; + } diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 07ccd8cd67..76cb9a3ebc 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -36,10 +36,8 @@ public: { } - template - SquareMatrix(const Slice &in_slice) : Matrix(in_slice) - { - } + using base = Matrix; + using base::base; SquareMatrix &operator=(const Matrix &other) { @@ -55,7 +53,7 @@ public: } template - const Slice slice(size_t x0, size_t y0) const + ConstSlice slice(size_t x0, size_t y0) const { return {x0, y0, this}; } diff --git a/src/lib/matrix/matrix/Vector.hpp b/src/lib/matrix/matrix/Vector.hpp index 87b98afa71..ba0d0b51ce 100644 --- a/src/lib/matrix/matrix/Vector.hpp +++ b/src/lib/matrix/matrix/Vector.hpp @@ -47,6 +47,22 @@ public: } } + template + Vector(const ConstSlice &slice_in) : + Matrix(slice_in) + { + } + + template + Vector(const ConstSlice &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); diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 27b9ee90fd..3b5bbadca8 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -40,15 +40,8 @@ public: v(1) = y; } - template - Vector2(const Slice &slice_in) : Vector(slice_in) - { - } - - template - Vector2(const Slice &slice_in) : Vector(slice_in) - { - } + using base = Vector; + using base::base; explicit Vector2(const Vector3 &other) { diff --git a/src/lib/matrix/matrix/Vector3.hpp b/src/lib/matrix/matrix/Vector3.hpp index c901219bef..ccfece92f0 100644 --- a/src/lib/matrix/matrix/Vector3.hpp +++ b/src/lib/matrix/matrix/Vector3.hpp @@ -43,15 +43,8 @@ public: v(2) = z; } - template - Vector3(const Slice &slice_in) : Vector(slice_in) - { - } - - template - Vector3(const Slice &slice_in) : Vector(slice_in) - { - } + using base = Vector; + using base::base; Vector3 cross(const Matrix31 &b) const { @@ -103,7 +96,7 @@ public: return (*this).cross(b); } - const Slice xy() const + ConstSlice xy() const { return {0, 0, this}; }