Add more assignment operators for slices

PX4 · Apr 7, 2020 · d613055 · d613055
1 parent 976ada4
commit d613055
Show file tree

Hide file tree

Showing 2 changed files with 138 additions and 0 deletions.
diff --git a/matrix/Slice.hpp b/matrix/Slice.hpp
@@ -100,6 +100,90 @@ class Slice {
         return self;
     }
 
+    template<size_t MM, size_t NN>
+    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) += other(i, j);
+            }
+        }
+        return self;
+    }
+
+    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) += other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator+=(const Type& 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) += other;
+            }
+        }
+        return self;
+    }
+
+    template<size_t MM, size_t NN>
+    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) -= other(i, j);
+            }
+        }
+        return self;
+    }
+
+    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) -= other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator-=(const Type& 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) -= other;
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator*=(const Type& 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) *= other;
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator/=(const Type& other)
+    {
+        return operator*=(Type(1) / other);
+    }
+
     template<size_t R, size_t S>
     const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
     {

diff --git a/test/slice.cpp b/test/slice.cpp
@@ -154,6 +154,60 @@ int main()
     Vector2f v8_check = {2, 6};
     TEST(isEqual(v8,v8_check));
 
+    // Different assignment operators
+    SquareMatrix3f O(data);
+    float operand_data [4] = {2, 1, -3, -1};
+    const SquareMatrix<float, 2> operand(operand_data);
+
+    O.slice<2,2>(1,0) += operand;
+    float O_check_data_1 [9] = {0, 2, 3,  6, 6, 6,  4, 7, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) += operand.slice<2,1>(0,0);
+    float O_check_data_2 [9] = {0, 2, 3,  4, 7, 6,  7, 5, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
+
+    O = SquareMatrix3f(data);
+    O.slice<3,3>(0,0) += -1;
+    float O_check_data_3 [9] = {-1, 1, 2,  3, 4, 5,  6, 7, 9};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
+
+    O = SquareMatrix3f(data);
+    O.col(1) += Vector3f{1, -2, 3};
+    float O_check_data_4 [9] = {0, 3, 3,  4, 3, 6,  7, 11, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,2>(1,0) -= operand;
+    float O_check_data_5 [9] = {0, 2, 3,  2, 4, 6,  10, 9, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0);
+    float O_check_data_6 [9] = {0, 2, 3,  4, 3, 6,  7, 11, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
+
+    O = SquareMatrix3f(data);
+    O.slice<3,3>(0,0) -= -1;
+    float O_check_data_7 [9] = {1, 3, 4,  5, 6, 7,  8, 9, 11};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
+
+    O = SquareMatrix3f(data);
+    O.col(1) -= Vector3f{1, -2, 3};
+    float O_check_data_8 [9] = {0, 1, 3,  4, 7, 6,  7, 5, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) *= 5.f;
+    float O_check_data_9 [9] = {0, 2, 3,  4, 25, 6,  7, 40, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) /= 2.f;
+    float O_check_data_10 [9] = {0, 2, 3,  4, 2.5, 6,  7, 4, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
+
     return 0;
 }