Clean up mj_jacSparse.

PiperOrigin-RevId: 725994792
Change-Id: I928a6312a6910231bfeb96ef7a3f29f1d128565c

Author: yuvaltassa

src/engine/engine_support.c

@@ -15,7 +15,6 @@
 #include "engine/engine_support.h"
 
 #include <stddef.h>
-#include <string.h>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
@@ -527,9 +526,6 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
 void mj_jacSparse(const mjModel* m, const mjData* d,
                   mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
                   int NV, const int* chain) {
-  int da, ci;
-  mjtNum offset[3], tmp[3], *cdof = d->cdof;
-
   // clear jacobians
   if (jacp) {
     mju_zero(jacp, 3*NV);
@@ -539,6 +535,7 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
   }
 
   // compute point-com offset
+  mjtNum offset[3];
   mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
 
   // skip fixed bodies
@@ -552,10 +549,10 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
   }
 
   // get last dof that affects this (as well as the original) body
-  da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
+  int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
 
   // start and the end of the chain (chain is in increasing order)
-  ci = NV-1;
+  int ci = NV-1;
 
   // backward pass over dof ancestor chain
   while (da >= 0) {
@@ -569,20 +566,23 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
       mjERROR("dof index %d not found in chain", da);
     }
 
+    const mjtNum* cdof = d->cdof + 6*da;
+
     // construct rotation jacobian
     if (jacr) {
-      jacr[ci] = cdof[6*da];
-      jacr[ci+NV] = cdof[6*da+1];
-      jacr[ci+2*NV] = cdof[6*da+2];
+      jacr[ci+0*NV] = cdof[0];
+      jacr[ci+1*NV] = cdof[1];
+      jacr[ci+2*NV] = cdof[2];
     }
 
     // construct translation jacobian (correct for rotation)
     if (jacp) {
-      mju_cross(tmp, cdof+6*da, offset);
+      mjtNum tmp[3];
+      mju_cross(tmp, cdof, offset);
 
-      jacp[ci] = cdof[6*da+3] + tmp[0];
-      jacp[ci+NV] = cdof[6*da+4] + tmp[1];
-      jacp[ci+2*NV] = cdof[6*da+5] + tmp[2];
+      jacp[ci+0*NV] = cdof[3] + tmp[0];
+      jacp[ci+1*NV] = cdof[4] + tmp[1];
+      jacp[ci+2*NV] = cdof[5] + tmp[2];
     }
 
     // advance to parent dof
@@ -596,9 +596,8 @@ void mj_jacSparse(const mjModel* m, const mjData* d,
 void mj_jacSparseSimple(const mjModel* m, const mjData* d,
                         mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point,
                         int body, int flg_second, int NV, int start) {
-  mjtNum offset[3], tmp[3];
-
   // compute point-com offset
+  mjtNum offset[3];
   mju_sub3(offset, point, d->subtree_com+3*m->body_rootid[body]);
 
   // skip fixed body
@@ -631,6 +630,7 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d,
 
     // construct translation jacobian (correct for rotation)
     if (jacdifp) {
+      mjtNum tmp[3];
       mju_cross(tmp, cdof, offset);
 
       // plus sign