Add matrix arithmetic

.gitignore

@@ -33,3 +33,4 @@
 
 build/
 .vscode/
+.DS_Store

docker/Dockerfile

@@ -34,11 +34,8 @@ RUN /bin/bash -c 'apt install -y libopencv-dev python3-opencv'
 # Prompt formatting
 RUN /bin/bash -c 'apt install -y vim'
 RUN echo "parse_git_branch() {\n git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/(\\\1)/' \n}" >> /home/dev/.bashrc
-RUN echo "export PS1=\"\[\e[34m\]\u@\[\e[34m\]\h \[\e[32m\]\w \[\e[91m\]\$(parse_git_branch)\[\e[00m\]$ \" " >> /home/dev/.bashrc
+RUN echo "export PS1='\[\e[34m\]\u@\[\e[34m\]\h \[\e[32m\]\w \[\e[91m\]\$(parse_git_branch)\[\e[00m\]$ ' " >> /home/dev/.bashrc
 RUN echo "source /opt/ros/foxy/setup.bash" >> /home/dev/.bashrc
 #-------------------------------------------------
 
-RUN /bin/bash -c 'apt install -y apt ros-foxy-plotjuggler-ros'
-
-
 USER dev

include/types/matrix.hpp

@@ -6,6 +6,7 @@
 #include "memory"
 #include "tuple"
 #include "vector"
+#include "type_traits"
 
 namespace stdmath
 {
@@ -51,7 +52,7 @@ class MatrixXX
     {
         return data[row * numberOfColumns + col];
     }
-    T operator()(size_t row, size_t col) const
+    const T& operator()(size_t row, size_t col) const
     {
         return data[row * numberOfColumns + col];
     }
@@ -70,6 +71,86 @@ class MatrixXX
         numberOfColumns = cols;
         // TODO: What happens when the assertion fails?
     }
+    MatrixXX<T, numberOfRows, numberOfColumns, rowMajor> operator+(
+        const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat) const
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        MatrixXX<T, numberOfRows, numberOfColumns, rowMajor> res{};
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+            res.data[i] = data[i] + mat.data[i];
+        return res;
+    }
+
+    MatrixXX<T, numberOfRows, numberOfColumns, rowMajor> operator-(
+        const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat) const
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        MatrixXX<T, numberOfRows, numberOfColumns, rowMajor> res{};
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+            res.data[i] = data[i] - mat.data[i];
+        return res;
+    }
+
+    MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& operator+=(
+        const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat)
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+            data[i] = data[i] + mat.data[i];
+        return *this;
+    }
+
+    MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& operator-=(
+        const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat)
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+            data[i] = data[i] - mat.data[i];
+        return *this;
+    }
+
+    MatrixXX<T, numberOfRows, numberOfColumns, rowMajor> operator-()
+    {
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+            data[i] = -data[i];
+        return *this;
+    }
+
+    bool operator==(const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat)
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        bool res{true};
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+        {
+            res &= data[i] == mat.data[i];
+            if (!res) break;
+        }
+        return res;
+    }
+
+    bool operator!=(const MatrixXX<T, numberOfRows, numberOfColumns, rowMajor>& mat)
+    {
+        assert(this->rows() == mat.rows() && this->cols() == mat.cols());
+        static_assert( std::is_same<decltype(this->data), decltype(mat.data)>::value == true );
+
+        bool res{false};
+        for(size_t i = 0; i < numberOfRows * numberOfColumns; i++)
+        {
+            res |= data[i] != mat.data[i];
+            if (res) break;
+        }
+        return res;
+    }
 };
 
 template <size_t numberOfRows, size_t numberOfColumns>

include/types/vector.hpp

@@ -38,7 +38,7 @@ class VectorXX : public stdmath::matrix::MatrixXX<T, numberOfRows, 1>
     {
         return data[index];
     }
-    T operator[](size_t index) const
+    const T& operator[](size_t index) const
     {
         return data[index];
     }

tests/matrix_test.cpp

@@ -59,6 +59,75 @@ TEST(MatrixTest, DoubleSquareMatrixTest)
     EXPECT_EQ(typeid(test_matrix(0, 0)), typeid(double));
 }
 
+TEST(MatrixTest, EqualityTest)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix1{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix2{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix3{{{2, 1}, {3, 0}}};
+
+    EXPECT_TRUE(test_matrix1 == test_matrix2);
+    EXPECT_FALSE(test_matrix1 == test_matrix3);
+
+    EXPECT_FALSE(test_matrix1 != test_matrix2);
+    EXPECT_TRUE(test_matrix1 != test_matrix3);
+}
+
+TEST(MatrixTest, AddTwoMatrix)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix1{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix2{{{2, 1}, {3, 0}}};
+    stdmath::matrix::MatrixXd<2, 2> res1{{{3, 3}, {7, 5}}};
+    EXPECT_TRUE(res1 == (test_matrix1 + test_matrix2));
+
+    stdmath::matrix::MatrixXd<2, 2> res2{{{3, 1}, {7, 0}}};
+    EXPECT_FALSE(res2 == (test_matrix1 + test_matrix2));
+}
+
+TEST(MatrixTest, SubtractTwoMatrix)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix1{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix2{{{2, 1}, {3, 0}}};
+    stdmath::matrix::MatrixXd<2, 2> res1{{{-1, 1}, {1, 5}}};
+    EXPECT_TRUE(res1 == (test_matrix1 - test_matrix2));
+
+    stdmath::matrix::MatrixXd<2, 2> res2{{{3, 1}, {7, 0}}};
+    EXPECT_FALSE(res2 == (test_matrix1 - test_matrix2));
+}
+
+TEST(MatrixTest, AddCompoundOperator)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix1{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix2{{{2, 1}, {3, 0}}};
+    test_matrix1 += test_matrix2;
+    stdmath::matrix::MatrixXd<2, 2> res1{{{3, 3}, {7, 5}}};
+    EXPECT_TRUE(res1 == test_matrix1);
+
+    stdmath::matrix::MatrixXd<2, 2> res2{{{3, 1}, {7, 0}}};
+    EXPECT_FALSE(res2 == test_matrix1);
+}
+
+TEST(MatrixTest, SubtractCompoundOperator)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix1{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> test_matrix2{{{2, 1}, {3, 0}}};
+    test_matrix1 -= test_matrix2;
+    stdmath::matrix::MatrixXd<2, 2> res1{{{-1, 1}, {1, 5}}};
+    EXPECT_TRUE(res1 == test_matrix1);
+
+    stdmath::matrix::MatrixXd<2, 2> res2{{{3, 1}, {7, 0}}};
+    EXPECT_FALSE(res2 == test_matrix1);
+}
+
+TEST(MatrixTest, UnarySubtractOperator)
+{
+    stdmath::matrix::MatrixXd<2, 2> test_matrix{{{1, 2}, {4, 5}}};
+    stdmath::matrix::MatrixXd<2, 2> res1{{{-1, -2}, {-4, -5}}};
+    EXPECT_TRUE(res1 == -test_matrix);
+
+    // stdmath::matrix::MatrixXd<2, 2> res2{{{3, 1}, {7, 0}}};
+    // EXPECT_FALSE(res2 == test_matrix);
+}
+
 int main(int argc, char** argv)
 {
     ::testing::InitGoogleTest(&argc, argv);

tests/vector_test.cpp

@@ -44,6 +44,14 @@ TEST(VectorTest, DoubleVectorTest)
     EXPECT_EQ(typeid(test_vector[0]), typeid(double));
 }
 
+TEST(VectorTest, AddTwoVectors)
+{
+    stdmath::vector::VectorXd<2> test_vector1{{1, 2}};
+    stdmath::vector::VectorXd<2> test_vector2{{3, 4}};
+    // stdmath::vector::VectorXd<2> res = test_vector1 + test_vector2;
+    // EXPECT_EQ(res, stdmath::vector::VectorXd<2>{{4, 6}});
+}
+
 int main(int argc, char** argv)
 {
     ::testing::InitGoogleTest(&argc, argv);