From 33fd2fe40e33087e4e1a28ec18a8eb83a49d9e43 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Wed, 8 Apr 2026 08:48:37 -0700
Subject: [PATCH] Improve `mju_addToSclSparseInc` with two-pointer loop.

Profiling `leaves.xml` with contacts disabled, this function takes up most of the time (in the implicit integrator). Total `testspeed` run time:

BEFORE: 18.5s
AFTER: 16.5
PiperOrigin-RevId: 896525902
Change-Id: I57f4a984d178956a2902212deb857bf53f53f01b
---
 src/engine/engine_util_sparse.c | 53 +++++++++------------------------
 1 file changed, 14 insertions(+), 39 deletions(-)

diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c
index 9d9c2146..9d78e409 100644
--- a/src/engine/engine_util_sparse.c
+++ b/src/engine/engine_util_sparse.c
@@ -339,51 +339,26 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu
 
 
 // dst += scl*src, only at common non-zero indices
-void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src,
-                           int nnzdst, const int* inddst,
-                           int nnzsrc, const int* indsrc, mjtNum scl) {
+void mju_addToSclSparseInc(mjtNum* restrict dst, const mjtNum* restrict src,
+                           int nnzdst, const int* restrict inddst,
+                           int nnzsrc, const int* restrict indsrc, mjtNum scl) {
   if (!nnzdst || !nnzsrc) {
     return;
   }
 
-  int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0];
-  while (1) {
-    // common non-zero index
+  int adrs = 0, adrd = 0;
+  while (adrs < nnzsrc && adrd < nnzdst) {
+    int inds = indsrc[adrs];
+    int indd = inddst[adrd];
+
     if (inds == indd) {
-      // add
       dst[adrd] += scl * src[adrs];
-
-      // advance src
-      if (++adrs < nnzsrc) {
-        inds = indsrc[adrs];
-      } else {
-        return;
-      }
-
-      // advance dst
-      if (++adrd < nnzdst) {
-        indd = inddst[adrd];
-      } else {
-        return;
-      }
-    }
-
-    // src non-zero index smaller: advance src
-    else if (inds < indd) {
-      if (++adrs < nnzsrc) {
-        inds = indsrc[adrs];
-      } else {
-        return;
-      }
-    }
-
-    // dst non-zero index smaller: advance dst
-    else {
-      if (++adrd < nnzdst) {
-        indd = inddst[adrd];
-      } else {
-        return;
-      }
+      adrs++;
+      adrd++;
+    } else if (inds < indd) {
+      adrs++;
+    } else {
+      adrd++;
     }
   }
 }