Add mju_mulVecMatVec, mutiplies a square matrix M by a vector x on both sides. Returns x^T * M * x.

PiperOrigin-RevId: 474292806 Change-Id: I3432469dbe1f02ccf5a13241c7aa12d824cbe034
2022-09-14 08:04:40 -07:00
parent ee9eccc992
commit f4e7fa97af
12 changed files with 109 additions and 20 deletions
@@ -41,5 +41,17 @@ TEST_F(EngineUtilBlasTest, MjuDot) {
  EXPECT_EQ(mju_dot(a, b, 7), 7 + 2*6 + 3*5 + 4*4 + 5*3 + 6*2 + 7);
 }

+TEST_F(EngineUtilBlasTest, MjuMulVecMatVec) {
+  mjtNum vec1[] = {1, 2, 3};
+  mjtNum vec2[] = {3, 2, 1};
+  mjtNum mat[] = {
+    1, 2, 3,
+    4, 5, 6,
+    7, 8, 9
+  };
+
+  EXPECT_EQ(mju_mulVecMatVec(vec1, mat, vec2, 3), 204);
+}
+
 }  // namespace
 }  // namespace mujoco