Add CSR implementation of mj_factorI

PiperOrigin-RevId: 712498431 Change-Id: I13b52e53482ed97da8788875d4d95e2beb5ca7c1
2025-01-06 05:47:13 -08:00
parent 7eb8231fda
commit ac11e5faa6
9 changed files with 514 additions and 14 deletions
@@ -118,7 +118,13 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) {
  std::cerr << "\n";
  for (int r = 0; r < nrow; r++) {
    for (int c = 0; c < ncol; c++) {
-      std::cerr << std::fixed << std::setw(3 + p) << mat[c + r*ncol] << " ";
+      mjtNum val = mat[c + r*ncol];
+      if (val) {
+        std::cerr << std::fixed << std::setw(5 + p) << val << " ";
+      } else {
+        // don't print exact zeros
+        std::cerr << std::string(6 + p, ' ');
+      }
    }
    std::cerr << "\n";
  }