Add support for Slice to Matrix, SquareMatrix, Vector

matrix/Matrix.hpp

@@ -24,6 +24,12 @@ namespace matrix
 template <typename Type, size_t M>
 class Vector;
 
+template<typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t P, size_t Q, size_t M, size_t N>
+class Slice;
+
 template<typename Type, size_t M, size_t N>
 class Matrix
 {
@@ -50,6 +56,17 @@ public:
         memcpy(_data, other._data, sizeof(_data));
     }
 
+    template<size_t P, size_t Q>
+    Matrix(const Slice<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.
      */
@@ -353,27 +370,15 @@ public:
     }
 
     template<size_t P, size_t Q>
-    Matrix<Type, P, Q> slice(size_t x0, size_t y0) const
+    const Slice<Type, P, Q, M, N> slice(size_t x0, size_t y0) const
     {
-        const Matrix<Type, M, N> &self = *this;
-        Matrix<Type, P, Q> 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<Type, P, Q, M, N>(x0, y0, this);
     }
 
     template<size_t P, size_t Q>
-    void set(const Matrix<Type, P, Q> &m, size_t x0, size_t y0)
+    Slice<Type, P, Q, M, N> slice(size_t x0, size_t y0)
     {
-        Matrix<Type, M, N> &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<Type, P, Q, M, N>(x0, y0, this);
     }
 
     void setRow(size_t i, const Matrix<Type, N, 1> &row)
@@ -517,7 +522,6 @@ public:
         }
         return result;
     }
-
 };
 
 template<typename Type, size_t M, size_t N>

matrix/SquareMatrix.hpp

@@ -20,6 +20,9 @@ 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 Type, size_t  M>
 class SquareMatrix : public Matrix<Type, M, M>
 {
@@ -36,6 +39,24 @@ public:
     {
     }
 
+    template<size_t P, size_t Q>
+    SquareMatrix(const Slice<Type, M, M, P, Q>& in_slice) : Matrix<Type, M, M>(in_slice)
+    {
+    }
+
+    SquareMatrix<Type, M>& operator=(const Matrix<Type, M, M>& other)
+    {
+        Matrix<Type, M, M>::operator=(other);
+        return *this;
+    }
+
+    template <size_t P, size_t Q>
+    SquareMatrix<Type, M> & operator=(const Slice<Type, M, M, P, Q>& in_slice)
+    {
+        Matrix<Type, M, M>::operator=(in_slice);
+        return *this;
+    }
+
     // inverse alias
     inline SquareMatrix<Type, M> I() const
     {
@@ -93,7 +114,6 @@ public:
         }
         return res;
     }
-
 };
 
 typedef SquareMatrix<float, 3> SquareMatrix3f;

matrix/Vector.hpp

@@ -34,6 +34,12 @@ public:
     {
     }
 
+    template<size_t P, size_t Q>
+    Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
+        Matrix<Type, M, 1>(slice_in)
+    {
+    }
+
     inline Type operator()(size_t i) const
     {
         const MatrixM1 &v = *this;

matrix/Vector2.hpp

@@ -43,6 +43,11 @@ 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)
+    {
+    }
+
     explicit Vector2(const Vector3 & other)
     {
         Vector2 &v(*this);

matrix/Vector3.hpp

@@ -19,9 +19,12 @@ class Matrix;
 template <typename Type, size_t M>
 class Vector;
 
-template<typename Type>
+template <typename Type>
 class Dcm;
 
+template <typename Type>
+class Vector2;
+
 template<typename Type>
 class Vector3 : public Vector<Type, 3>
 {
@@ -48,6 +51,11 @@ 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)
+    {
+    }
+
     Vector3 cross(const Matrix31 & b) const {
         const Vector3 &a(*this);
         Vector3 c;

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"

test/slice.cpp

@@ -46,7 +46,7 @@ int main()
     };
 
     Matrix<float, 2, 2> C(data_2);
-    A.set(C, 1, 1);
+    A.slice<2, 2>(1, 1) = C;
 
     float data_2_check[9] = {
         0, 2, 3,

test/squareMatrix.cpp

@@ -26,6 +26,19 @@ int main()
     SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5);
     SquareMatrix<float, 3> eA_check(data_check);
     TEST((eA - eA_check).abs().max() < 1e-3f);
+
+    SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1);
+    SquareMatrix<float, 2> A_bottomright2;
+    A_bottomright2 = A.slice<2,2>(1,1);
+
+    float data_bottomright[4] = {5, 6,
+                                 8, 10
+                                };
+    SquareMatrix<float, 2> bottomright_check(data_bottomright);
+    TEST(isEqual(A_bottomright, bottomright_check));
+    TEST(isEqual(A_bottomright2, bottomright_check));
+
+
     return 0;
 }