From 58234b2217eee793143f395ab2dbd83b84d5bf60 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 7 Apr 2025 05:58:00 -0700 Subject: [PATCH] Replace mjMAXVAL with numerical max limit where necessary in nativeccd. PiperOrigin-RevId: 744693156 Change-Id: I72c3122701be281d468291dfc2ad1e4bb1da5a1c --- src/engine/engine_collision_gjk.c | 27 ++++++++++++------------ src/engine/engine_collision_gjk.h | 8 +++++++ test/engine/engine_collision_gjk_test.cc | 4 ++-- 3 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 92d49a6a..807942d2 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -14,7 +14,6 @@ #include "engine/engine_collision_gjk.h" -#include #include #include #include @@ -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; } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index a8f2224b..55ee5f8b 100644 --- a/src/engine/engine_collision_gjk.h +++ b/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 #include #include @@ -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 diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 325f6fd7..f4fcb141 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/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); }