From ac7f19853668bc0037c49c200bd9711b7cb940a3 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 8 Jun 2026 07:20:36 -0700 Subject: [PATCH] Tune constants for nativeccd to perform better with single precision. PiperOrigin-RevId: 928549147 Change-Id: Ic28c60f73e92bb1b9578c56f790b6cda94875907 --- src/engine/engine_collision_gjk.c | 58 +++++++++++++++--------- src/engine/engine_collision_gjk.h | 22 +++++---- test/engine/engine_collision_gjk_test.cc | 41 ++++++++--------- 3 files changed, 68 insertions(+), 53 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 34b57326..ba3bf561 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -28,6 +28,23 @@ #define mjMINVAL2 (mjMINVAL * mjMINVAL) #define mjMAXVAL2 (mjMAXVAL * mjMAXVAL) +// numerical limits for single and double precision +#ifdef mjUSESINGLE + // minimal distance squared for origin inside tetrahedron in polytope2 + #define mjMINDIST2 1e-10f + // minimal distance squared for origin inside tetrahedron in polytope3 + #define mjMINDIST3 1e-10f + // minimal distance squared for origin inside tetrahedron in polytope4 + #define mjMINDIST4 1e-17f + // minimal tolerance for EPA + #define mjMINEPATOL 1e-7f +#else + #define mjMINDIST2 mjMINVAL2 + #define mjMINDIST3 mjMINVAL2 + #define mjMINDIST4 mjMINVAL2 + #define mjMINEPATOL mjMINVAL +#endif + // align memory size on 8-byte boundary; needed for single precision static inline size_t align8(size_t size) { return ((size + 7) / 8) * 8; @@ -950,27 +967,27 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj mjtNum* v5 = pt->verts[v5i].vert; // build hexahedron - if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL2) { + if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINDIST2) { replaceSimplex3(pt, status, v1i, v3i, v4i); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL2) { + if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINDIST2) { replaceSimplex3(pt, status, v1i, v5i, v3i); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL2) { + if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINDIST2) { replaceSimplex3(pt, status, v1i, v4i, v5i); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL2) { + if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINDIST2) { replaceSimplex3(pt, status, v2i, v4i, v3i); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL2) { + if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINDIST2) { replaceSimplex3(pt, status, v2i, v3i, v5i); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL2) { + if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINDIST2) { replaceSimplex3(pt, status, v2i, v5i, v4i); return polytope3(pt, status, obj1, obj2); } @@ -1103,22 +1120,22 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj } // create hexahedron for EPA - if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL2) { + if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } - if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL2) { + if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } - if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL2) { + if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } - if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL2) { + if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } - if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL2) { + if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } - if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL2) { + if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINDIST3) { return mjEPA_P3_ORIGIN_ON_FACE; } @@ -1140,23 +1157,24 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj int v4 = insertVertex(pt, status->simplex + 3); // if the origin is on a face, replace the 3-simplex with a 2-simplex - if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL2) { + if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINDIST4) { replaceSimplex3(pt, status, v1, v2, v3); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL2) { + if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINDIST4) { replaceSimplex3(pt, status, v1, v4, v2); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL2) { + if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINDIST4) { replaceSimplex3(pt, status, v1, v3, v4); return polytope3(pt, status, obj1, obj2); } - if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL2) { + if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINDIST4) { replaceSimplex3(pt, status, v4, v3, v2); return polytope3(pt, status, obj1, obj2); } + // numerically verify that the origin lies in the tetrahedron interior if (!testTetra(pt->verts[v1].vert, pt->verts[v2].vert, pt->verts[v3].vert, pt->verts[v4].vert)) { return mjEPA_P4_MISSING_ORIGIN; } @@ -1311,13 +1329,11 @@ static mjtNum epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjt static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2; Face* face = NULL, *pface = NULL; // face closest to origin - mjtNum tolerance = status->tolerance; int discrete = discreteGeoms(obj1, obj2); - // tolerance is not used for discrete geoms - if (discrete && sizeof(mjtNum) == sizeof(double)) { - tolerance = mjMINVAL; - } + // discrete geoms return in a finite number of iterations, a non-zero tolerance avoids + // absurdly small lower and upper bounds + mjtNum tolerance = discrete ? mjMINEPATOL : status->tolerance; int k, kmax = status->max_iterations < 1000 ? status->max_iterations : 1000; for (k = 0; k < kmax; k++) { diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 1464d80e..db16277d 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -28,19 +28,21 @@ extern "C" { #endif -// numerical max limit -#ifndef mjUSESINGLE - #define mjMAX_LIMIT DBL_MAX -#else +// numerical limits +#ifdef mjUSESINGLE #define mjMAX_LIMIT FLT_MAX + // tolerance for normal alignment of two faces (cosine of 1.6e-3) + #define mjFACE_TOL 0.99999872f + // tolerance for edge-face alignment (sine of 1.6e-3) + #define mjEDGE_TOL 0.00159999931f +#else + #define mjMAX_LIMIT DBL_MAX + // tolerance for normal alignment of two faces (cosine of 1.6e-3) + #define mjFACE_TOL 0.99999872 + // tolerance for edge-face alignment (sine of 1.6e-3) + #define mjEDGE_TOL 0.00159999931 #endif -// tolerance for normal alignment of two faces (cosine of 1.6e-3) -#define mjFACE_TOL 0.99999872 - -// tolerance for edge-face alignment (sine of 1.6e-3) -#define mjEDGE_TOL 0.00159999931 - // max number of supported vertices in a polygon face of a mesh #define mjMAX_POLYVERT 150 diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 8f55a7cc..223c5568 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1049,8 +1049,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) { mjtNum dist; int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8); - ASSERT_EQ(ncons, 4); - + // contact unrecoverable under single precision + ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 4 : 0); } TEST_F(MjGjkTest, BoxBoxMultiCCD12) { @@ -1228,9 +1228,7 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) { std::vector dir, pos; mjtNum dist; int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8); - ASSERT_EQ(ncons, 4); - } TEST_F(MjGjkTest, SmallBoxMesh) { @@ -1275,19 +1273,23 @@ TEST_F(MjGjkTest, SmallBoxMesh) { mjtNum dist; int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); - ASSERT_EQ(ncons, 1); - EXPECT_NEAR(dist, 0, kTolerance); + // contact unrecoverable under single precision + ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 1 : 0); + if (ncons) { + EXPECT_NEAR(dist, 0, kTolerance); - // direction - EXPECT_NEAR(dir[0], 0, kTolerance); - EXPECT_NEAR(dir[1], 0, kTolerance); - EXPECT_NEAR(dir[2], 1, kTolerance); + // direction + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], 1, kTolerance); - // position - EXPECT_NEAR(pos[0], 0, kTolerance); - EXPECT_NEAR(pos[1], 0, kTolerance); - EXPECT_NEAR(pos[2], 0, kTolerance); + // position + EXPECT_NEAR(pos[0], 0, kTolerance); + EXPECT_NEAR(pos[1], 0, kTolerance); + EXPECT_NEAR(pos[2], 0, kTolerance); + } } + TEST_F(MjGjkTest, BoxMesh) { static constexpr char xml[] = R"( @@ -1801,18 +1803,13 @@ static constexpr char xml[] = R"( ASSERT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.01, kTolerance); - EXPECT_NEAR(dir[0], 0, kTolerance); - EXPECT_NEAR(dir[1], 0, kTolerance); - EXPECT_NEAR(dir[2], 1, kTolerance); - - EXPECT_NEAR(pos[0], 0, kTolerance); - EXPECT_NEAR(pos[1], 0, kTolerance); - EXPECT_NEAR(pos[2], -0.005, kTolerance); + EXPECT_THAT(dir[0], MjNear(0, kTolerance, 1e-5)); + EXPECT_THAT(dir[1], MjNear(0, kTolerance, 1e-5)); + EXPECT_THAT(dir[2], MjNear(1, kTolerance, kTolerance)); // multicontact ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); ASSERT_EQ(ncons, 4); - } TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {