From 92d1c8761ec8e5dbe9ba3f756cebd5e90956a1b2 Mon Sep 17 00:00:00 2001
From: Julian Kent <julian@auterion.com>
Date: Thu, 3 Oct 2019 13:47:36 +0200
Subject: [PATCH] More features: longerThan, norm, copyTo and slice on a Slice
 (#97)

* Allow slices of slices, add longerThan to Vector/Slice
---
 matrix/Matrix.hpp         |  4 +--
 matrix/Slice.hpp          | 69 +++++++++++++++++++++++++++++++++++----
 matrix/Vector.hpp         |  5 +++
 test/copyto.cpp           | 14 ++++++++
 test/matrixAssignment.cpp |  2 ++
 test/slice.cpp            | 28 ++++++++++++++++
 test/vector.cpp           |  7 ++++
 7 files changed, 121 insertions(+), 8 deletions(-)

diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 79831f7a7b..155ae0736e 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -90,12 +90,12 @@ public:
         return (*this);
     }
 
-    void copyTo(Type dst [M*N]) const
+    void copyTo(Type dst[M*N]) const
     {
         memcpy(dst, _data, sizeof(Type)*M*N);
     }
 
-    void copyToColumnMajor(Type (&dst)[M*N]) const
+    void copyToColumnMajor(Type dst[M*N]) const
     {
         const Matrix<Type, M, N> &self = *this;
 
diff --git a/matrix/Slice.hpp b/matrix/Slice.hpp
index cff1521690..3a1008c21a 100644
--- a/matrix/Slice.hpp
+++ b/matrix/Slice.hpp
@@ -41,38 +41,95 @@ public:
     }
 
     template<size_t MM, size_t NN>
-    Slice<Type, P, Q, M, N>& operator=(const Slice<Type, P, Q, MM, NN>& in_matrix)
+    Slice<Type, P, Q, M, N>& operator=(const Slice<Type, P, Q, MM, NN>& other)
     {
         Slice<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) = in_matrix(i, j);
+                self(i, j) = other(i, j);
             }
         }
         return self;
     }
 
-    Slice<Type, P, Q, M, N>& operator=(const Matrix<Type, P, Q>& in_matrix)
+    Slice<Type, P, Q, M, N>& operator=(const Matrix<Type, P, Q>& other)
     {
         Slice<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) = in_matrix(i, j);
+                self(i, j) = other(i, j);
             }
         }
         return self;
     }
 
     // allow assigning vectors to a slice that are in the axis
-    Slice<Type, 1, Q, M, N>& operator=(const Vector<Type, Q>& in_vector)
+    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)
     {
         Slice<Type, 1, Q, M, N>& self = *this;
         for (size_t j = 0; j < Q; j++) {
-            self(0, j) = in_vector(j);
+            self(0, j) = other(j);
         }
         return self;
     }
 
+    template<size_t R, size_t S>
+    const Slice<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);
+    }
+
+    template<size_t R, size_t S>
+    Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
+    {
+        return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
+    }
+
+    void copyTo(Type dst[M*N]) const
+    {
+        const Slice<Type, P, Q, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[i*N+j] = self(i, j);
+            }
+        }
+    }
+
+    void copyToColumnMajor(Type dst[M*N]) const
+    {
+        const Slice<Type, P, Q, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[i+(j*M)] = self(i, j);
+            }
+        }
+    }
+
+    Type norm_squared()
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        Type accum(0);
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                accum += self(i, j)*self(i, j);
+            }
+        }
+        return accum;
+    }
+
+    Type norm()
+    {
+        return matrix::sqrt(norm_squared());
+    }
+
+    bool longerThan(Type testVal)
+    {
+        return norm_squared() > testVal*testVal;
+    }
+
 private:
     size_t _x0, _y0;
     Matrix<Type,M,N>* _data;
diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp
index e46247539f..d517db45f6 100644
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@@ -104,6 +104,11 @@ public:
         return unit();
     }
 
+    bool longerThan(Type testVal)
+    {
+        return norm_squared() > testVal*testVal;
+    }
+
     Vector sqrt() const {
         const Vector &a(*this);
         Vector r;
diff --git a/test/copyto.cpp b/test/copyto.cpp
index 47a209edc6..4989adb0f1 100644
--- a/test/copyto.cpp
+++ b/test/copyto.cpp
@@ -52,6 +52,20 @@ int main()
         TEST(fabs(array_A[i] - array_column[i]) < eps);
     }
 
+    // Slice copyTo
+    float dst5[2] = {};
+    v.slice<2,1>(0,0).copyTo(dst5);
+    for (size_t i = 0; i < 2; i++) {
+        TEST(fabs(v(i) - dst5[i]) < eps);
+    }
+
+    float subarray_A[4] = {};
+    A.slice<2,2>(0,0).copyToColumnMajor(subarray_A);
+    float subarray_column[4] = {1,4,2,5};
+    for (size_t i = 0; i < 4; i++) {
+        TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
+    }
+
     return 0;
 }
 
diff --git a/test/matrixAssignment.cpp b/test/matrixAssignment.cpp
index 00b6e88ab1..070d1f1840 100644
--- a/test/matrixAssignment.cpp
+++ b/test/matrixAssignment.cpp
@@ -3,6 +3,8 @@
 
 using namespace matrix;
 
+template class matrix::Matrix<float, 3, 2>;
+
 int main()
 {
     Matrix3f m;
diff --git a/test/slice.cpp b/test/slice.cpp
index f92fef99df..36c252a742 100644
--- a/test/slice.cpp
+++ b/test/slice.cpp
@@ -3,6 +3,9 @@
 
 using namespace matrix;
 
+template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage results
+
+
 int main()
 {
     float data[9] = {0, 2, 3,
@@ -97,6 +100,31 @@ int main()
         Matrix<float, 1, 5> K_check(data_4_check);
         TEST(isEqual(K, K_check));
     }
+
+    // check that slice of a slice works for reading
+    const Matrix<float, 3, 3> cm33(data);
+    Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
+    float top_right_check[2] = {3,6};
+    TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
+
+    // check that slice of a slice works for writing
+    Matrix<float, 3, 3> m33(data);
+    m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
+    const float data_check[9] = {0, 2, 0,
+                                 4, 5, 0,
+                                 7, 8, 10
+                                };
+    TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
+
+    // longerThan
+    Vector3f v5;
+    v5(0) = 3;
+    v5(1) = 4;
+    v5(2) = 9;
+    TEST(v5.xy().longerThan(4.99f));
+    TEST(!v5.xy().longerThan(5.f));
+    TEST(isEqualF(5.f, v5.xy().norm()));
+
     return 0;
 }
 
diff --git a/test/vector.cpp b/test/vector.cpp
index c9463064b8..638ae1220f 100644
--- a/test/vector.cpp
+++ b/test/vector.cpp
@@ -35,6 +35,13 @@ int main()
     Vector<float, 5> v4(data1_sq);
     TEST(isEqual(v1, v4.sqrt()));
 
+    // longerThan
+    Vector<float, 2> v5;
+    v5(0) = 3;
+    v5(1) = 4;
+    TEST(v5.longerThan(4.99f));
+    TEST(!v5.longerThan(5.f));
+
     return 0;
 }