Cosmetic clean-up and a few micro-optimisations for primitive colliders.

PiperOrigin-RevId: 543559919
Change-Id: I345ccbdca4303e82d1097a5a4bf9381c9f845617

src/engine/engine_collision_primitive.c

@@ -20,6 +20,7 @@
 #include <mujoco/mjmacro.h>
 #include <mujoco/mjmodel.h>
 #include "engine/engine_util_blas.h"
+#include "engine/engine_util_misc.h"
 #include "engine/engine_util_spatial.h"
 
 
@@ -27,19 +28,16 @@
 
 // plane : sphere (actual implementation, can be called with modified parameters)
 static int _PlaneSphere(mjContact* con, mjtNum margin,
-                        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) {
-  mjtNum tmp[3];
-  mjtNum cdist;
-
   // set normal
   con[0].frame[0] = mat1[2];
   con[0].frame[1] = mat1[5];
   con[0].frame[2] = mat1[8];
 
   // compute distance, return if too large
-  mju_sub3(tmp, pos2, pos1);
+  mjtNum tmp[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
-  cdist = mju_dot3(tmp, con[0].frame);
+  mjtNum cdist = mju_dot3(tmp, con[0].frame);
   if (cdist > margin + size2[0]) {
     return 0;
   }
@@ -68,22 +66,19 @@ int mjc_PlaneSphere(const mjModel* m, const mjData* d,
 int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
                      mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
-  mjtNum pos[3], axis[3], segment[3];
-  int n1, n2;
 
   // get capsule axis, segment = scaled axis
-  axis[0] = mat2[2];
+  mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
-  axis[1] = mat2[5];
+  mjtNum segment[3] = {size2[1]*axis[0], size2[1]*axis[1], size2[1]*axis[2]};
-  axis[2] = mat2[8];
-  mju_scl3(segment, axis, size2[1]);
 
   // get point 1, do sphere-plane test
+  mjtNum pos[3];
   mju_add3(pos, pos2, segment);
-  n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
+  int n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
 
   // get point 2, do sphere-plane test
   mju_sub3(pos, pos2, segment);
-  n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
+  int n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
 
   // align contact frames with capsule axis
   if (n1) {
@@ -104,29 +99,26 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
   mjGETINFO
   mjtNum normal[3] = {mat1[2], mat1[5], mat1[8]};
   mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
-  mjtNum vec[3], vec1[3];
-  mjtNum len, scl, dist0, prjaxis, prjvec, prjvec1;
-  int cnt = 0;
 
   // project, make sure axis points towards plane
-  prjaxis = mju_dot3(normal, axis);
+  mjtNum prjaxis = mju_dot3(normal, axis);
   if (prjaxis > 0) {
     mju_scl3(axis, axis, -1);
     prjaxis = -prjaxis;
   }
 
   // compute normal distance to cylinder center
-  mju_sub3(vec, pos2, pos1);
+  mjtNum vec[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
-  dist0 = mju_dot3(vec, normal);
+  mjtNum dist0 = mju_dot3(vec, normal);
 
   // remove component of -normal along axis, compute length
   mju_scl3(vec, axis, prjaxis);
   mju_subFrom3(vec, normal);
-  len = mju_norm3(vec);
+  mjtNum len_sqr = mju_dot3(vec, vec);
 
   // general configuration: normalize vector, scale by radius
-  if (len >= mjMINVAL) {
+  if (len_sqr >= mjMINVAL*mjMINVAL) {
-    scl = size2[0]/len;
+    mjtNum scl = size2[0]/mju_sqrt(len_sqr);
     vec[0] *= scl;
     vec[1] *= scl;
     vec[2] *= scl;
@@ -140,13 +132,14 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
   }
 
   // project vector on normal
-  prjvec = mju_dot3(vec, normal);
+  mjtNum prjvec = mju_dot3(vec, normal);
 
   // scale axis by half-length
   mju_scl3(axis, axis, size2[1]);
   prjaxis *= size2[1];
 
   // check first point, construct contact
+  int cnt = 0;
   if (dist0 + prjaxis + prjvec <= margin) {
     con[cnt].dist = dist0 + prjaxis + prjvec;
     mju_add3(con[cnt].pos, pos2, vec);
@@ -171,9 +164,10 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
   }
 
   // try to add triangle points on side closer to plane
-  prjvec1 = -prjvec*0.5;
+  mjtNum prjvec1 = -prjvec*0.5;
   if (dist0 + prjaxis + prjvec1 <= margin) {
     // compute sideways vector: vec1
+    mjtNum vec1[3];
     mju_cross(vec1, vec, axis);
     mju_normalize3(vec1);
     mju_scl3(vec1, vec1, size2[0]*mju_sqrt(3.0)/2);
@@ -208,26 +202,27 @@ int mjc_PlaneCylinder(const mjModel* m, const mjData* d,
 int mjc_PlaneBox(const mjModel* m, const mjData* d,
                  mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
-  int cnt = 0;
 
   // get normal, difference between centers, normal distance
   mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]};
-  mjtNum dif[3], vec[3], corner[3], dist, ldist;
+  mjtNum dif[3] = {pos2[0] - pos1[0], pos2[1] - pos1[1], pos2[2] - pos1[2]};
-  mju_sub3(dif, pos2, pos1);
+  mjtNum dist = mju_dot3(dif, norm);
-  dist = mju_dot3(dif, norm);
 
   // test all corners, pick bottom 4
+  int cnt = 0;
   for (int i=0; i < 8; i++) {
     // get corner in local coordinates
+    mjtNum vec[3];
     vec[0] = (i&1 ? size2[0] : -size2[0]);
     vec[1] = (i&2 ? size2[1] : -size2[1]);
     vec[2] = (i&4 ? size2[2] : -size2[2]);
 
     // get corner in global coordinates relative to box center
+    mjtNum corner[3];
     mju_rotVecMat(corner, vec, mat2);
 
     // compute distance to plane, skip if too far or pointing up
-    ldist = mju_dot3(norm, corner);
+    mjtNum ldist = mju_dot3(norm, corner);
     if (dist + ldist > margin || ldist > 0) {
       continue;
     }
@@ -255,31 +250,26 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
 
 // sphere : sphere (actual implementation, can be called with modified parameters)
 static int _SphereSphere(mjContact* con, mjtNum margin,
-                         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) {
-  mjtNum len, cdist;
-  mjtNum axis1[3], axis2[3];
-
   // check bounding spheres (this is called from other functions)
-  cdist = mju_dist3(pos1, pos2);
+  mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
-  if (cdist > margin + size1[0] + size2[0]) {
+  mjtNum cdist_sqr = mju_dot3(dif, dif);
+  mjtNum min_dist = margin + size1[0] + size2[0];
+  if (cdist_sqr > min_dist*min_dist) {
     return 0;
   }
 
   // depth and normal
-  con[0].dist = cdist - size1[0] - size2[0];
+  con[0].dist = mju_sqrt(cdist_sqr) - size1[0] - size2[0];
   mju_sub3(con[0].frame, pos2, pos1);
-  len = mju_normalize3(con[0].frame);
+  mjtNum len = mju_normalize3(con[0].frame);
 
   // if centers are the same, norm = cross-product of z axes
   //  if z axes are parallel, norm = [1;0;0]
   if (len < mjMINVAL) {
-    axis1[0] = mat1[2];
+    mjtNum axis1[3] = {mat1[2], mat1[5], mat1[8]};
-    axis1[1] = mat1[5];
+    mjtNum axis2[3] = {mat2[2], mat2[5], mat2[8]};
-    axis1[2] = mat1[8];
-    axis2[0] = mat2[2];
-    axis2[1] = mat2[5];
-    axis2[2] = mat2[8];
     mju_cross(con[0].frame, axis1, axis2);
     mju_normalize3(con[0].frame);
   }
@@ -307,21 +297,14 @@ int mjc_SphereSphere(const mjModel* m, const mjData* d,
 int mjc_SphereCapsule(const mjModel* m, const mjData* d,
                       mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
-  mjtNum x, axis[3], vec[3];
 
-  // get capsule axis (scaled)
+  // get capsule length and axis
-  axis[0] = mat2[2] * size2[1];
+  mjtNum len = size2[1];
-  axis[1] = mat2[5] * size2[1];
+  mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
-  axis[2] = mat2[8] * size2[1];
 
   // find projection, clip to segment
-  mju_sub3(vec, pos1, pos2);
+  mjtNum vec[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
-  x = mju_dot3(axis, vec) / mju_dot3(axis, axis);
+  mjtNum x = mju_clip(mju_dot3(axis, vec), -len, len);
-  if (x > 1) {
-    x = 1;
-  } else if (x < -1) {
-    x = -1;
-  }
 
   // find nearest point on segment, do sphere-sphere test
   mju_scl3(vec, axis, x);
@@ -335,59 +318,43 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d,
 int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
                        mjContact* con, int g1, int g2, mjtNum margin) {
   mjGETINFO
-  mjtNum axis1[3], axis2[3], dif[3], vec1[3], vec2[3];
-  mjtNum ma, mb, mc, u, v, det, x1, x2;
-  int n1, n2, n3, n4;
 
   // get capsule axes (scaled) and center difference
-  axis1[0] = mat1[2] * size1[1];
+  mjtNum axis1[3] = {mat1[2] * size1[1], mat1[5] * size1[1], mat1[8] * size1[1]};
-  axis1[1] = mat1[5] * size1[1];
+  mjtNum axis2[3] = {mat2[2] * size2[1], mat2[5] * size2[1], mat2[8] * size2[1]};
-  axis1[2] = mat1[8] * size1[1];
+  mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
-  axis2[0] = mat2[2] * size2[1];
-  axis2[1] = mat2[5] * size2[1];
-  axis2[2] = mat2[8] * size2[1];
-  mju_sub3(dif, pos1, pos2);
 
   // compute matrix coefficients and determinant
-  ma =  mju_dot3(axis1, axis1);
+  mjtNum ma =  mju_dot3(axis1, axis1);
-  mb = -mju_dot3(axis1, axis2);
+  mjtNum mb = -mju_dot3(axis1, axis2);
-  mc =  mju_dot3(axis2, axis2);
+  mjtNum mc =  mju_dot3(axis2, axis2);
-  u =  -mju_dot3(axis1, dif);
+  mjtNum u  = -mju_dot3(axis1, dif);
-  v =   mju_dot3(axis2, dif);
+  mjtNum v  =  mju_dot3(axis2, dif);
-  det = ma*mc - mb*mb;
+  mjtNum det = ma*mc - mb*mb;
 
   // general configuration (non-parallel axes)
   if (fabs(det) >= mjMINVAL) {
     // find projections, clip to segments
-    x1 = (mc*u - mb*v) / det;
+    mjtNum x1 = (mc*u - mb*v) / det;
-    x2 = (ma*v - mb*u) / det;
+    mjtNum x2 = (ma*v - mb*u) / det;
 
     if (x1 > 1) {
       x1 = 1;
-      x2 = (v-mb)/mc;
+      x2 = (v - mb) / mc;
     } else if (x1 < -1) {
       x1 = -1;
-      x2 = (v+mb)/mc;
+      x2 = (v + mb) / mc;
     }
     if (x2 > 1) {
       x2 = 1;
-      x1 = (u-mb)/ma;
+      x1 = mju_clip((u - mb) / ma, -1, 1);
-      if (x1 > 1) {
-        x1 = 1;
-      } else if (x1 < -1) {
-        x1 = -1;
-      }
     } else if (x2 < -1) {
       x2 = -1;
-      x1 = (u+mb)/ma;
+      x1 = mju_clip((u + mb) / ma, -1, 1);
-      if (x1 > 1) {
-        x1 = 1;
-      } else if (x1 < -1) {
-        x1 = -1;
-      }
     }
 
     // find nearest points, do sphere-sphere test
+    mjtNum vec1[3], vec2[3];
     mju_scl3(vec1, axis1, x1);
     mju_addTo3(vec1, pos1);
     mju_scl3(vec2, axis2, x2);
@@ -399,28 +366,21 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
   // parallel axes
   else {
     // x1 = 1
+    mjtNum vec1[3];
     mju_add3(vec1, pos1, axis1);
-    x2 = (v - mb) / mc;
+    mjtNum x2 = mju_clip((v - mb) / mc, -1, 1);
-    if (x2 > 1) {
+
-      x2 = 1;
+    mjtNum vec2[3];
-    } else if (x2 < -1) {
-      x2 = -1;
-    }
     mju_scl3(vec2, axis2, x2);
     mju_addTo3(vec2, pos2);
-    n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     // x1 = -1
     mju_sub3(vec1, pos1, axis1);
-    x2 = (v + mb) / mc;
+    x2 = mju_clip((v + mb) / mc, -1, 1);
-    if (x2 > 1) {
-      x2 = 1;
-    } else if (x2 < -1) {
-      x2 = -1;
-    }
     mju_scl3(vec2, axis2, x2);
     mju_addTo3(vec2, pos2);
-    n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     // return if two contacts already found
     if (n1+n2 >= 2) {
@@ -429,15 +389,10 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
 
     // x2 = 1
     mju_add3(vec2, pos2, axis2);
-    x1 = (u - mb) / ma;
+    mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
-    if (x1 > 1) {
-      x1 = 1;
-    } else if (x1 < -1) {
-      x1 = -1;
-    }
     mju_scl3(vec1, axis1, x1);
     mju_addTo3(vec1, pos1);
-    n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     // return if two contacts already found
     if (n1+n2+n3 >= 2) {
@@ -446,15 +401,10 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
 
     // x2 = -1
     mju_sub3(vec2, pos2, axis2);
-    x1 = (u + mb) / ma;
+    x1 = mju_clip((u + mb) / ma, -1, 1);
-    if (x1 > 1) {
-      x1 = 1;
-    } else if (x1 < -1) {
-      x1 = -1;
-    }
     mju_scl3(vec1, axis1, x1);
     mju_addTo3(vec1, pos1);
-    n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
+    int n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
 
     return n1+n2+n3+n4;
   }