Replace mjMAXVAL with numerical max limit where necessary in nativeccd.

PiperOrigin-RevId: 744693156
Change-Id: I72c3122701be281d468291dfc2ad1e4bb1da5a1c

src/engine/engine_collision_gjk.c

@@ -14,7 +14,6 @@
 
 #include "engine/engine_collision_gjk.h"
 
-#include <float.h>
 #include <stddef.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -206,16 +205,16 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
         status->gjk_iterations = k;
         status->nsimplex = 0;
         status->nx = 0;
-        status->dist = mjMAXVAL;
+        status->dist = mjMAX_LIMIT;
         return;
       }
-    } else if (status->dist_cutoff < mjMAXVAL) {
+    } else if (status->dist_cutoff < mjMAX_LIMIT) {
       mjtNum vs = dot3(x_k, s_k), vv = dot3(x_k, x_k);
       if (dot3(x_k, s_k) > 0 && (vs*vs / vv) >= cutoff2) {
         status->gjk_iterations = k;
         status->nsimplex = 0;
         status->nx = 0;
-        status->dist = mjMAXVAL;
+        status->dist = mjMAX_LIMIT;
         return;
       }
     }
@@ -227,7 +226,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       int ret = gjkIntersect(status, obj1, obj2);
       if (ret != -1) {
         status->nx = 0;
-        status->dist = ret > 0 ? 0 : mjMAXVAL;
+        status->dist = ret > 0 ? 0 : mjMAX_LIMIT;
         return;
       }
       k = status->gjk_iterations;
@@ -251,7 +250,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       status->gjk_iterations = k;
       status->nsimplex = 0;
       status->nx = 0;
-      status->dist = mjMAXVAL;
+      status->dist = mjMAX_LIMIT;
       return;
     }
 
@@ -399,7 +398,7 @@ static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Ve
     scl3(normal, normal, norm);
     return dot3(normal, v1->vert);
   }
-  return mjMAXVAL;  // cannot recover normal (ignore face)
+  return mjMAX_LIMIT;  // cannot recover normal (ignore face)
 }
 
 
@@ -607,7 +606,7 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const
   }
 
   // find the smallest distance, and use the corresponding barycentric coordinates
-  mjtNum dmin = mjMAXVAL;
+  mjtNum dmin = mjMAX_LIMIT;
 
   if (!comp1) {
     mjtNum lambda_2d[3], x[3];
@@ -758,7 +757,7 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const
   }
 
   // find the smallest distance, and use the corresponding barycentric coordinates
-  mjtNum dmin = mjMAXVAL;
+  mjtNum dmin = mjMAX_LIMIT;
 
   if (!comp1) {
     mjtNum lambda_1d[2], x[3];
@@ -931,7 +930,7 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
   sub3(diff, v2, v1);
 
   // find component with smallest magnitude (so cross product is largest)
-  mjtNum value = mjMAXVAL;
+  mjtNum value = mjMAX_LIMIT;
   int index = 0;
   for (int i = 0; i < 3; i++) {
     if (mju_abs(diff[i]) < value) {
@@ -1351,7 +1350,7 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
 // return a face of the expanded polytope that best approximates the pentration depth
 // witness points are in status->{x1, x2}
 static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
-  mjtNum tolerance = status->tolerance, lower, upper = FLT_MAX;
+  mjtNum tolerance = status->tolerance, lower, upper = mjMAX_LIMIT;
   int k, kmax = status->max_iterations;
   Face* face = NULL, *pface = NULL;  // face closest to origin
 
@@ -1359,7 +1358,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
     pface = face;
 
     // find the face closest to the origin (lower bound for penetration depth)
-    lower = FLT_MAX;
+    lower = mjMAX_LIMIT;
     for (int i = 0; i < pt->nmap; i++) {
       if (pt->map[i]->dist < lower) {
         face = pt->map[i];
@@ -1577,7 +1576,7 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
   mjtNum ab[3];
   sub3(ab, b, a);
   mjtNum temp = dot3(pn, ab);
-  if (temp == 0.0) return mjMAXVAL;  // parallel; no intersection
+  if (temp == 0.0) return mjMAX_LIMIT;  // parallel; no intersection
   mjtNum t = (pd - dot3(pn, a)) / temp;
   if (t >= 0.0 && t <= 1.0) {
     res[0] = a[0] + t*ab[0];
@@ -2292,7 +2291,7 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
     if (status->dist > status->tolerance) {
       inflate(status, full_margin1, full_margin2);
       if (status->dist > status->dist_cutoff) {
-        status->dist = mjMAXVAL;
+        status->dist = mjMAX_LIMIT;
       }
       return status->dist;
     }

src/engine/engine_collision_gjk.h

@@ -15,6 +15,7 @@
 #ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_
 #define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_
 
+#include <float.h>
 #include <stddef.h>
 
 #include <mujoco/mjexport.h>
@@ -27,6 +28,13 @@
 extern "C" {
 #endif
 
+// numerical max limit
+#ifndef mjUSESINGLE
+  #define mjMAX_LIMIT DBL_MAX
+#else
+  #define mjMAX_LIMIT FLT_MAX
+#endif
+
 // max number of EPA iterations
 #define mjMAX_EPA_ITERATIONS 170
 

test/engine/engine_collision_gjk_test.cc

@@ -71,7 +71,7 @@ void CCDFree(void* data, void* buffer) {
 }
 
 mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
-                mjtNum x2[3], mjtNum cutoff = mjMAXVAL) {
+                mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
   mjCCDConfig config;
   mjCCDStatus status;
 
@@ -211,7 +211,7 @@ TEST_F(MjGjkTest, SphereSphereDistCutoff) {
   int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
   mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr, .999999);
 
-  EXPECT_EQ(dist, mjMAXVAL);
+  EXPECT_EQ(dist, mjMAX_LIMIT);
   mj_deleteData(data);
   mj_deleteModel(model);
 }