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

@@ -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,
+void mju_addToSclSparseInc(mjtNum* restrict dst, const mjtNum* restrict src,
-                           int nnzdst, const int* inddst,
+                           int nnzdst, const int* restrict inddst,
-                           int nnzsrc, const int* indsrc, mjtNum scl) {
+                           int nnzsrc, const int* restrict indsrc, mjtNum scl) {
   if (!nnzdst || !nnzsrc) {
     return;
   }
 
-  int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0];
+  int adrs = 0, adrd = 0;
-  while (1) {
+  while (adrs < nnzsrc && adrd < nnzdst) {
-    // common non-zero index
+    int inds = indsrc[adrs];
+    int indd = inddst[adrd];
+
     if (inds == indd) {
-      // add
       dst[adrd] += scl * src[adrs];
-
+      adrs++;
-      // advance src
+      adrd++;
-      if (++adrs < nnzsrc) {
+    } else if (inds < indd) {
-        inds = indsrc[adrs];
+      adrs++;
-      } else {
+    } else {
-        return;
+      adrd++;
-      }
-
-      // 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;
-      }
     }
   }
 }