Add debugging method for printing matrices to stderr to fixture.h

PiperOrigin-RevId: 702301299
Change-Id: I01738b6634a91cde6b146475d06731471dcf3764

test/fixture.h

@@ -17,6 +17,8 @@
 
 #include <csetjmp>
 #include <cstring>
+#include <iomanip>
+#include <iostream>
 #include <string>
 #include <string_view>
 #include <vector>
@@ -110,6 +112,18 @@ inline std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
   return std::vector<mjtNum>(array, array + n);
 }
 
+// Prints a matrix to stderr, useful for debugging.
+inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5) {
+  std::cerr.precision(p);
+  std::cerr << "\n";
+  for (int r = 0; r < nrow; r++) {
+    for (int c = 0; c < ncol; c++) {
+      std::cerr << std::fixed << std::setw(4 + p) << mat[c + r*ncol] << " ";
+    }
+    std::cerr << "\n";
+  }
+}
+
 // Installs a mock filesystem via a resource provider. To obtain thread safety,
 // each filesystem is scoped for individual unit tests with destructive
 // operations not permitted.