Represent only the lower triangle in Newton solver's reduced dof-dof matrix.

PiperOrigin-RevId: 712488529 Change-Id: Iad91c72654376539791d7856765a0d0ac9088251
2025-01-06 04:58:06 -08:00
parent ee6f4837f3
commit 7eb8231fda
11 changed files with 112 additions and 76 deletions
@@ -725,8 +725,12 @@ TEST_F(InertiaTest, DenseSameAsSparse) {
  // dense addM
  mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);

-  // dense comparison, should be same matrix
-  EXPECT_THAT(dst_dense, ElementsAreArray(dst_sparse));
+  // dense comparison, lower triangle should match
+  for (int i=0; i < nv; i++) {
+    for (int j=0; j < i; j++) {
+      EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]);
+    }
+  }

  // clean up
  mj_deleteData(d);