Update gtest_state_composite.cpp

test/gtest_state_composite.cpp

@@ -9,6 +9,7 @@
 #include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
 
+#include "core/common/wolf.h"
 #include "core/state_block/state_composite.h"
 
 using namespace wolf;
@@ -44,9 +45,6 @@ using namespace std;
 TEST(StateComposite, operatorStream)
 {
     StateComposite x;
-    unsigned int psize, osize;
-    psize = 2;
-    osize = 3;
 
     x.emplace("P", Vector2d(1,1));
     x.emplace("O", Vector3d(2,2,2));
@@ -54,6 +52,7 @@ TEST(StateComposite, operatorStream)
     WOLF_DEBUG("x = " , x);
 
 }
+
 TEST(MatrixComposite, emplace_operatorStream)
 {
     MatrixComposite M;
@@ -83,6 +82,58 @@ TEST(MatrixComposite, emplace_operatorStream)
     WOLF_DEBUG("M = " , M);
 }
 
+TEST(MatrixComposite, operatorParenthesis)
+{
+    MatrixComposite M;
+
+    unsigned int psize, osize;
+    psize = 2;
+    osize = 3;
+
+    MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+    Mpp.setOnes();
+    Mpo.setOnes(); Mpo *= 2;
+    Mop.setOnes(); Mop *= 3;
+    Moo.setOnes(); Moo *= 4;
+
+    M.emplace("P", "P", Mpp);
+    ASSERT_MATRIX_APPROX( M("P", "P"), Mpp, 1e-20);
+
+    M.emplace("P", "O", Mpo);
+    ASSERT_MATRIX_APPROX( M("P", "O"), Mpo, 1e-20);
+
+    // return default empty matrix if block not present
+    MatrixXd N = M("O", "O");
+    ASSERT_EQ(N.rows(), 0);
+    ASSERT_EQ(N.cols(), 0);
+}
+
+TEST(MatrixComposite, operatorAt)
+{
+    MatrixComposite M;
+
+    unsigned int psize, osize;
+    psize = 2;
+    osize = 3;
+
+    MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+    Mpp.setOnes();
+    Mpo.setOnes(); Mpo *= 2;
+    Mop.setOnes(); Mop *= 3;
+    Moo.setOnes(); Moo *= 4;
+
+    M.emplace("P", "P", Mpp);
+    ASSERT_MATRIX_APPROX( M.at("P", "P"), Mpp, 1e-20);
+
+    M.emplace("P", "O", Mpo);
+    ASSERT_MATRIX_APPROX( M.at("P", "O"), Mpo, 1e-20);
+
+    // error if block not present
+    ASSERT_DEATH(MatrixXd N = M.at("O", "O"), "");
+}
+
 TEST(MatrixComposite, productVector)
 {
     unsigned int psize, osize;
@@ -115,6 +166,21 @@ TEST(MatrixComposite, productVector)
 
     WOLF_DEBUG("y = M * x = " , y);
 
+    /*        M      *    x  =    y
+     *     p    o
+     * p [1 1 2 2 2]   p [1]   p [14]
+     *   [1 1 2 2 2]     [1]     [14]
+     *   [3 3 4 4 4] *   [2] =   [30]
+     * o [3 3 4 4 4]   o [2]   o [30]
+     *   [3 3 4 4 4]     [2]     [30]
+     */
+
+    Vector2d yp(14,14);
+    Vector3d yo(30,30,30);
+
+    ASSERT_MATRIX_APPROX(y.at("P"), yp, 1e-20);
+    ASSERT_MATRIX_APPROX(y.at("O"), yo, 1e-20);
+
 }
 
 TEST(MatrixComposite, productMatrix)
@@ -156,6 +222,13 @@ TEST(MatrixComposite, productMatrix)
 
     WOLF_DEBUG("MN = M * N = " , MN);
 
+    /*      M    *      N    =      MN
+     *    p  o        o  v         o   v
+     * p [1 1 2]   p [3 4 4]   p [ 8 12 12]
+     *   [1 1 2] *   [3 4 4] =   [ 8 12 12]
+     * o [3 3 4]   o [1 2 2]   o [22 32 32]
+     */
+
     MatrixXd MNpo(MatrixXd::Ones(psize,osize) *  8);
     MatrixXd MNpv(MatrixXd::Ones(psize,vsize) * 12);
     MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);