From a7fe06c7174aee6b03498596d28ac1d421b913c8 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Sun, 12 Oct 2025 01:42:45 -0700
Subject: [PATCH] Further simplification to `mj_crb`

PiperOrigin-RevId: 818241421
Change-Id: I67dd2a60f415eb17b41f5ee553939118ab124123
---
 src/engine/engine_core_smooth.c | 16 +++++-----------
 1 file changed, 5 insertions(+), 11 deletions(-)

diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c
index 3ed6f063..d55b9483 100644
--- a/src/engine/engine_core_smooth.c
+++ b/src/engine/engine_core_smooth.c
@@ -1546,21 +1546,15 @@ void mj_crb(const mjModel* m, mjData* d) {
 
   // dense forward pass over dofs
   for (int i=0; i < nv; i++) {
-    // process block of diagonals (simple bodies)
+    // simple dof: fixed diagonal inertia
+    int adr = rowadr[i];
     if (dof_simplenum[i]) {
-      int n = i + dof_simplenum[i];
-      for (; i < n; i++) {
-        M[rowadr[i]] = dof_M0[i];
-      }
-
-      // finish or else fall through with next row
-      if (i == nv) {
-        break;
-      }
+      M[adr] = dof_M0[i];
+      continue;
     }
 
     // init M(i,i) with armature inertia
-    int Madr_ij = rowadr[i] + rownnz[i] - 1;
+    int Madr_ij = adr + rownnz[i] - 1;
     M[Madr_ij] = dof_armature[i];
 
     // precompute buf = crb_body_i * cdof_i