Added set function.

matrix/Matrix.hpp

@@ -72,6 +72,13 @@ public:
         return _data[i][j];
     }
 
+    void operator=(const Matrix<Type, M, N> &other)
+    {
+        if (this != &other) {
+            memcpy(_data, other._data, sizeof(_data));
+        }
+    }
+
     /**
      * Matrix Operations
      */
@@ -324,6 +331,17 @@ public:
         return res;
     }
 
+    template<size_t P, size_t Q>
+    void set(const Matrix<Type, P, Q> &m, 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);
+            }
+        }
+    }
+
     void setZero()
     {
         memset(_data, 0, sizeof(_data));

test/attitude.cpp

@@ -107,7 +107,7 @@ int main()
                 if (yaw_expected < -180) yaw_expected += 360;
                 if (yaw_expected > 180) yaw_expected -= 360;
 
-                printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+                //printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
                 Euler<double> euler_expected(
                     deg2rad*double(roll_expected),
                     deg2rad*double(pitch),
@@ -117,7 +117,7 @@ int main()
                     deg2rad*double(pitch),
                     deg2rad*double(yaw));
                 Dcm<double> dcm_from_euler(euler);
-                dcm_from_euler.print();
+                //dcm_from_euler.print();
                 Euler<double> euler_out(dcm_from_euler);
                 TEST(isEqual(rad2deg*euler_expected, rad2deg*euler_out));
 

test/slice.cpp

@@ -19,6 +19,23 @@ int main()
     Matrix<float, 2, 3> B_check(data_check);
     Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));
     TEST(isEqual(B, B_check));
+
+    float data_2[4] = {
+        11, 12,
+        13, 14
+    };
+
+    Matrix<float, 2, 2> C(data_2);
+    A.set(C, 1, 1);
+
+    float data_2_check[9] = {
+        0, 2, 3,
+        4, 11, 12,
+        7, 13, 14
+    };
+    Matrix<float, 3, 3> D(data_2_check);
+    TEST(isEqual(A, D));
+
     return 0;
 }