From b817e8677d4a7683da2a3f97dbfee1b476ba0618 Mon Sep 17 00:00:00 2001
From: Julian Kent <jkflying@gmail.com>
Date: Mon, 16 Sep 2019 15:55:51 +0200
Subject: [PATCH] Add helpers based on Slice: row(), col(), xy()

---
 matrix/Dcm.hpp     |  6 +++---
 matrix/Matrix.hpp  | 35 +++++++++++++++++++++++++----------
 matrix/Vector3.hpp | 10 ++++++++++
 test/slice.cpp     | 41 +++++++++++++++++++++++++++++++++++++++++
 test/vector3.cpp   | 23 +++++++++++++++++++++++
 5 files changed, 102 insertions(+), 13 deletions(-)

diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp
index 43d3cee1d3..31ea35da5c 100644
--- a/matrix/Dcm.hpp
+++ b/matrix/Dcm.hpp
@@ -166,9 +166,9 @@ public:
     void renormalize()
     {
         /* renormalize rows */
-        for (size_t row = 0; row < 3; row++) {
-            matrix::Vector3f rvec(this->_data[row]);
-            this->setRow(row, rvec.normalized());
+        for (size_t r = 0; r < 3; r++) {
+            matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
+            this->Matrix<Type,3,3>::row(r) = rvec.normalized();
         }
     }
 };
diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index 7170f31f82..b6a9322c46 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -381,20 +381,35 @@ public:
         return Slice<Type, P, Q, M, N>(x0, y0, this);
     }
 
-    void setRow(size_t i, const Matrix<Type, N, 1> &row)
+    const Slice<Type, 1, N, M, N> row(size_t i) const
     {
-        Matrix<Type, M, N> &self = *this;
-        for (size_t j = 0; j < N; j++) {
-            self(i, j) = row(j, 0);
-        }
+        return slice<1, N>(i,0);
     }
 
-    void setCol(size_t j, const Matrix<Type, M, 1> &col)
+    Slice<Type, 1, N, M, N> row(size_t i)
     {
-        Matrix<Type, M, N> &self = *this;
-        for (size_t i = 0; i < M; i++) {
-            self(i, j) = col(i, 0);
-        }
+        return slice<1, N>(i,0);
+    }
+
+    const Slice<Type, M, 1, M, N> col(size_t j) const
+    {
+        return slice<M, 1>(0,j);
+    }
+
+    Slice<Type, M, 1, M, N> col(size_t j)
+    {
+        return slice<M, 1>(0,j);
+    }
+
+    void setRow(size_t i, const Matrix<Type, N, 1> &row_in)
+    {
+        slice<1,N>(i,0) = row_in.transpose();
+    }
+
+
+    void setCol(size_t j, const Matrix<Type, M, 1> &column)
+    {
+        slice<M,1>(0,j) = column;
     }
 
     void setZero()
diff --git a/matrix/Vector3.hpp b/matrix/Vector3.hpp
index 71e20cfb06..4f2de37f0b 100644
--- a/matrix/Vector3.hpp
+++ b/matrix/Vector3.hpp
@@ -109,6 +109,16 @@ public:
         return unit();
     }
 
+    const Slice<Type, 2, 1, 3, 1> xy() const
+    {
+        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+    }
+
+    Slice<Type, 2, 1, 3, 1> xy()
+    {
+        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+    }
+
 
     Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
         const Vector3 &v(*this);
diff --git a/test/slice.cpp b/test/slice.cpp
index 0c7c35d386..f92fef99df 100644
--- a/test/slice.cpp
+++ b/test/slice.cpp
@@ -56,6 +56,47 @@ int main()
     Matrix<float, 3, 3> D(data_2_check);
     TEST(isEqual(A, D));
 
+    //Test writing to slices
+    Matrix<float, 3, 1> E;
+    E(0,0) = -1;
+    E(1,0) = 1;
+    E(2,0) = 3;
+
+    Matrix<float, 2, 1> F;
+    F(0,0) = 9;
+    F(1,0) = 11;
+
+    E.slice<2,1>(0,0) = F;
+
+    float data_3_check[3] = {9, 11, 3};
+    Matrix<float, 3, 1> G (data_3_check);
+    TEST(isEqual(E, G));
+    TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
+
+    Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
+    TEST(isEqual(H,F));
+
+    float data_4_check[5] = {3, 11, 9, 0, 0};
+    {   // assigning row slices to each other
+        const Matrix<float, 3, 1> J (data_3_check);
+        Matrix<float, 5, 1> K;
+        K.row(2) = J.row(0);
+        K.row(1) = J.row(1);
+        K.row(0) = J.row(2);
+
+        Matrix<float, 5, 1> K_check(data_4_check);
+        TEST(isEqual(K, K_check));
+    }
+    {   // assigning col slices to each other
+        const Matrix<float, 1, 3> J (data_3_check);
+        Matrix<float, 1, 5> K;
+        K.col(2) = J.col(0);
+        K.col(1) = J.col(1);
+        K.col(0) = J.col(2);
+
+        Matrix<float, 1, 5> K_check(data_4_check);
+        TEST(isEqual(K, K_check));
+    }
     return 0;
 }
 
diff --git a/test/vector3.cpp b/test/vector3.cpp
index ebe8a2ca6f..b5cef674b9 100644
--- a/test/vector3.cpp
+++ b/test/vector3.cpp
@@ -27,6 +27,29 @@ int main()
     TEST(isEqual(a.unit(), a.normalized()));
     TEST(isEqual(a*2.0, Vector3f(2, 0, 0)));
 
+    Vector2f g2(1,3);
+    Vector3f g3(7, 11, 17);
+    g3.xy() = g2;
+    TEST(isEqual(g3, Vector3f(1, 3, 17)));
+
+    const Vector3f g4(g3);
+    Vector2f g5 = g4.xy();
+    TEST(isEqual(g5,g2));
+    TEST(isEqual(g2,Vector2f(g4.xy())));
+
+    Vector3f h;
+    TEST(isEqual(h,Vector3f(0,0,0)));
+
+    Vector<float, 4> j;
+    j(0) = 1;
+    j(1) = 2;
+    j(2) = 3;
+    j(3) = 4;
+
+    Vector3f k = j.slice<3,1>(0,0);
+    Vector3f k_test(1,2,3);
+    TEST(isEqual(k,k_test));
+
     return 0;
 }