diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index ea78147a8c..7170f31f82 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -24,6 +24,12 @@ namespace matrix template class Vector; +template +class Matrix; + +template +class Slice; + template class Matrix { @@ -50,6 +56,17 @@ public: memcpy(_data, other._data, sizeof(_data)); } + template + Matrix(const Slice& 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. */ @@ -353,27 +370,15 @@ public: } template - Matrix slice(size_t x0, size_t y0) const + const Slice slice(size_t x0, size_t y0) const { - const Matrix &self = *this; - Matrix res; //default constructed - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - res(i, j) = self(i + x0, j + y0); - } - } - return res; + return Slice(x0, y0, this); } template - void set(const Matrix &m, size_t x0, size_t y0) + Slice slice(size_t x0, size_t y0) { - Matrix &self = *this; - for (size_t i = 0; i < P; i++) { - for (size_t j = 0; j < Q; j++) { - self(i + x0, j + y0) = m(i, j); - } - } + return Slice(x0, y0, this); } void setRow(size_t i, const Matrix &row) @@ -517,7 +522,6 @@ public: } return result; } - }; template diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index 3063051145..7b0a46023f 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -20,6 +20,9 @@ class Matrix; template class Vector; +template +class Slice; + template class SquareMatrix : public Matrix { @@ -36,6 +39,24 @@ public: { } + template + SquareMatrix(const Slice& in_slice) : Matrix(in_slice) + { + } + + SquareMatrix& operator=(const Matrix& other) + { + Matrix::operator=(other); + return *this; + } + + template + SquareMatrix & operator=(const Slice& in_slice) + { + Matrix::operator=(in_slice); + return *this; + } + // inverse alias inline SquareMatrix I() const { @@ -93,7 +114,6 @@ public: } return res; } - }; typedef SquareMatrix SquareMatrix3f; diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index c6f0c24698..edac621ca6 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -34,6 +34,12 @@ public: { } + template + Vector(const Slice& slice_in) : + Matrix(slice_in) + { + } + inline Type operator()(size_t i) const { const MatrixM1 &v = *this; diff --git a/matrix/Vector2.hpp b/matrix/Vector2.hpp index dc42c5d5e9..5e209bbbf2 100644 --- a/matrix/Vector2.hpp +++ b/matrix/Vector2.hpp @@ -43,6 +43,11 @@ public: v(1) = y; } + template + Vector2(const Slice& slice_in) : Vector(slice_in) + { + } + explicit Vector2(const Vector3 & other) { Vector2 &v(*this); diff --git a/matrix/Vector3.hpp b/matrix/Vector3.hpp index dea65de5f4..71e20cfb06 100644 --- a/matrix/Vector3.hpp +++ b/matrix/Vector3.hpp @@ -19,9 +19,12 @@ class Matrix; template class Vector; -template +template class Dcm; +template +class Vector2; + template class Vector3 : public Vector { @@ -48,6 +51,11 @@ public: v(2) = z; } + template + Vector3(const Slice& slice_in) : Vector(slice_in) + { + } + Vector3 cross(const Matrix31 & b) const { const Vector3 &a(*this); Vector3 c; diff --git a/matrix/math.hpp b/matrix/math.hpp index 30479f26e0..9626b78d08 100644 --- a/matrix/math.hpp +++ b/matrix/math.hpp @@ -6,6 +6,7 @@ #endif #include "Matrix.hpp" #include "SquareMatrix.hpp" +#include "Slice.hpp" #include "Vector.hpp" #include "Vector2.hpp" #include "Vector3.hpp" diff --git a/test/slice.cpp b/test/slice.cpp index f1b422064c..0c7c35d386 100644 --- a/test/slice.cpp +++ b/test/slice.cpp @@ -46,7 +46,7 @@ int main() }; Matrix C(data_2); - A.set(C, 1, 1); + A.slice<2, 2>(1, 1) = C; float data_2_check[9] = { 0, 2, 3, diff --git a/test/squareMatrix.cpp b/test/squareMatrix.cpp index 861139ecc0..3ad6b1a832 100644 --- a/test/squareMatrix.cpp +++ b/test/squareMatrix.cpp @@ -26,6 +26,19 @@ int main() SquareMatrix eA = expm(SquareMatrix(A*dt), 5); SquareMatrix eA_check(data_check); TEST((eA - eA_check).abs().max() < 1e-3f); + + SquareMatrix A_bottomright = A.slice<2,2>(1,1); + SquareMatrix A_bottomright2; + A_bottomright2 = A.slice<2,2>(1,1); + + float data_bottomright[4] = {5, 6, + 8, 10 + }; + SquareMatrix bottomright_check(data_bottomright); + TEST(isEqual(A_bottomright, bottomright_check)); + TEST(isEqual(A_bottomright2, bottomright_check)); + + return 0; }