From b176445a3cf65c686218d58bc172bdd23f780bb9 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Thu, 11 Feb 2021 10:17:49 -0800
Subject: [PATCH] Matrix 4 multiplication test

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 src/Matrix4_TEST.cc | 33 +++++++++++++++++++++++++++++++++
 1 file changed, 33 insertions(+)

diff --git a/src/Matrix4_TEST.cc b/src/Matrix4_TEST.cc
index caed9c174..125cd5f22 100644
--- 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)
 {