Tighten const qualifiers in collision functions.

PiperOrigin-RevId: 543789453
Change-Id: I929c68b9100a093738c0de6c0965dab4ded1084b

src/engine/engine_collision_box.c

@@ -37,8 +37,8 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
 
 
 static int _SphereBox(mjContact* con, mjtNum mindist,
-                      mjtNum* pos1, mjtNum* mat1, mjtNum* size1,
+                      const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
-                      mjtNum* pos2, mjtNum* mat2, mjtNum* size2)
+                      const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
 {
   int i, k;
   mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3];

src/engine/engine_collision_convex.c

@@ -578,7 +578,7 @@ static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) {
 // entry point for heightfield collisions
 int mjc_ConvexHField(const mjModel* m, const mjData* d,
                      mjContact* con, int g1, int g2, mjtNum margin) {
-  mjGETINFO
+  mjGETINFO_HFIELD
   mjtNum mat[9], savemat2[9], savepos2[3], pos[3], vec[3], r2, dx, dy;
   mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
   int hid = m->geom_dataid[g1];

src/engine/engine_collision_convex.h

@@ -26,6 +26,14 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
 
+#define mjGETINFO_HFIELD \
+    const mjtNum* pos1  = d->geom_xpos + 3*g1; \
+    const mjtNum* mat1  = d->geom_xmat + 9*g1; \
+    const mjtNum* size1 = m->geom_size + 3*g1; \
+          mjtNum* pos2  = d->geom_xpos + 3*g2; \
+          mjtNum* mat2  = d->geom_xmat + 9*g2;
+// mjc_ConvexHField modifies and then restores pos2 and mat2
+
 #ifdef __cplusplus
 extern "C" {
 #endif

src/engine/engine_collision_primitive.c

@@ -353,12 +353,12 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
 
   // cap collision: use plane-sphere
   if (collide_cap) {
-    mjtNum flipmat[9] = {
+    const mjtNum flipmat[9] = {
       -mat2[0], mat2[1], -mat2[2],
       -mat2[3], mat2[4], -mat2[5],
       -mat2[6], mat2[7], -mat2[8]
     };
-    mjtNum* mat_cap;
+    const mjtNum* mat_cap;
     mjtNum pos_cap[3];
     if (x > 0) {  // top cap
       mju_addScl3(pos_cap, pos2, axis, height);

src/engine/engine_collision_primitive.h

@@ -20,13 +20,13 @@
 
 // define and extract geom info
 #define mjGETINFO \
-    mjtNum* pos1 = d->geom_xpos + 3*g1; \
+    const mjtNum* pos1  = d->geom_xpos + 3*g1; \
-    mjtNum* mat1 = d->geom_xmat + 9*g1; \
+    const mjtNum* mat1  = d->geom_xmat + 9*g1; \
-    mjtNum* size1= m->geom_size + 3*g1; \
+    const mjtNum* size1 = m->geom_size + 3*g1; \
-    mjtNum* pos2 = d->geom_xpos + 3*g2; \
+    const mjtNum* pos2  = d->geom_xpos + 3*g2; \
-    mjtNum* mat2 = d->geom_xmat + 9*g2; \
+    const mjtNum* mat2  = d->geom_xmat + 9*g2; \
-    mjtNum* size2= m->geom_size + 3*g2; \
+    const mjtNum* size2 = m->geom_size + 3*g2; \
-    (void) size1; (void) size2;
+    (void) size1; (void) size2;  // size1 and size2 are sometimes unused
 
 #ifdef __cplusplus
 extern "C" {