Matrix 4 multiplication test (#191)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
gazebosim · Feb 11, 2021 · 92e972e · 92e972e
1 parent 00088ff
commit 92e972e
Showing 1 changed file with 33 additions and 0 deletions.
diff --git a/src/Matrix4_TEST.cc b/src/Matrix4_TEST.cc
@@ -211,6 +211,39 @@ TEST(Matrix4dTest, Multiply4)
   EXPECT_EQ(mat2, mat4);
 }
 
+/////////////////////////////////////////////////
+TEST(Matrix4dTest, Multiply)
+{
+  math::Matrix4d mat1(
+      0, 1, 0, 1,
+      0, 1, 1, 0,
+      0, 1, 0, 1,
+      0, 1, 1, 0);
+  math::Matrix4d mat2(
+      2, 3, 5, 7,
+      5, 7, 2, 3,
+      7, 2, 3, 5,
+      3, 5, 7, 2);
+
+  math::Matrix4d mat3 = mat1 * mat2;
+
+  EXPECT_EQ(mat3,
+    math::Matrix4d(
+      8, 12, 9, 5,
+      12, 9, 5, 8,
+      8, 12, 9, 5,
+      12, 9, 5, 8));
+
+  math::Matrix4d mat4 = mat2 * mat1;
+
+  EXPECT_EQ(mat4,
+    math::Matrix4d(
+      0, 17, 10, 7,
+      0, 17, 10, 7,
+      0, 17, 7, 10,
+      0, 17, 7, 10));
+}
+
 /////////////////////////////////////////////////
 TEST(Matrix4dTest, Inverse)
 {