From 9ecec6f238e5a1932dc3c79ac4e4f65f5d53fd5f Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 1 Apr 2025 06:10:30 -0700 Subject: [PATCH] Fix bad contact from passing through polytope2 in nativeccd. PiperOrigin-RevId: 742666700 Change-Id: I76034579b4e663f9abfa265ec3e2bc608f631f59 --- src/engine/engine_collision_gjk.c | 26 ++- src/engine/engine_collision_gjk.h | 2 +- test/engine/engine_collision_gjk_test.cc | 205 +++++++++++++++-------- 3 files changed, 155 insertions(+), 78 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 16859bf5..1fcc03d9 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -903,6 +903,26 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) { +// return nonzero if the ray v1v2 intersects the triangle v3v4v5 +static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3], + const mjtNum v4[3], const mjtNum v5[3]) { + mjtNum diff12[3], diff13[3], diff14[3], diff15[3]; + sub3(diff12, v2, v1); + sub3(diff13, v3, v1); + sub3(diff14, v4, v1); + sub3(diff15, v5, v1); + + mjtNum vol1 = det3(diff13, diff14, diff12); + mjtNum vol2 = det3(diff14, diff15, diff12); + mjtNum vol3 = det3(diff15, diff13, diff12); + + if (vol1 >= 0 && vol2 >= 0 && vol3 >= 0) return 1; + if (vol1 <= 0 && vol2 <= 0 && vol3 <= 0) return -1; + return 0; +} + + + // create a polytope from a 1-simplex (returns 0 on success) static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert; @@ -970,9 +990,9 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj return polytope3(pt, status, obj1, obj2); } - // check that origin is in the hexahedron - if (status->dist > 10*mjMINVAL && !testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) { - return mjEPA_P2_MISSING_ORIGIN; + // check hexahedron is convex + if (!rayTriangle(v1, v2, v3, v4, v5)) { + return mjEPA_P2_NONCONVEX; } for (int i = 0; i < 6; i++) { diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 027929e3..a8f2224b 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -44,7 +44,7 @@ typedef enum { mjEPA_NOCONTACT = -1, mjEPA_SUCCESS = 0, mjEPA_P2_INVALID_FACES, - mjEPA_P2_MISSING_ORIGIN, + mjEPA_P2_NONCONVEX, mjEPA_P2_ORIGIN_ON_FACE, mjEPA_P3_BAD_NORMAL, mjEPA_P3_INVALID_V4, diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 6db923f2..dfaebaad 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -93,7 +93,7 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], return dist; } -int Penetration(mjtNum& depth, std::vector& dir, +int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector& dir, std::vector& pos, mjModel* model, mjData* data, int g1, int g2, mjtNum margin = 0, int max_contacts = 1) { mjCCDObj obj1, obj2; @@ -128,7 +128,6 @@ int Penetration(mjtNum& depth, std::vector& dir, #endif mjCCDConfig config; - mjCCDStatus status; // set config config.max_iterations = kMaxIterations; @@ -235,9 +234,11 @@ TEST_F(MjGjkTest, SphereSphereNoDist) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 0); mj_deleteData(data); @@ -262,9 +263,11 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 1); @@ -304,9 +307,10 @@ TEST_F(MjGjkTest, BoxBoxDepth) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 1); @@ -354,9 +358,11 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); if (ncons == 1) { EXPECT_NEAR(dist, -0.033401579411886845, kTolerance); @@ -421,9 +427,11 @@ TEST_F(MjGjkTest, BoxBoxDepth3) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.003066, kTolerance); @@ -453,11 +461,14 @@ TEST_F(MjGjkTest, BoxBoxTouching) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 0); + EXPECT_GT(status.epa_status, 0); mj_deleteData(data); mj_deleteModel(model); @@ -479,11 +490,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); EXPECT_NEAR(dist, -.1, kTolerance); @@ -517,11 +530,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); EXPECT_NEAR(dist, -.1, kTolerance); @@ -573,11 +588,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) { xpos[2] = 1.095456702630382306296041861060; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -634,11 +651,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) { xpos[1] = -0.023500601273213628239489025873; xpos[2] = -4.958782854594746325460619118530; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 8); EXPECT_NEAR(dist, -0.00060425119242707459, kTolerance); @@ -701,11 +720,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) { xpos[2] = -4.659108354876987156956147373421; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 8); EXPECT_NEAR(dist, -0.0001077858631973211, kTolerance); @@ -750,11 +771,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) { xpos[1] = 0.190777715293135141649827346555; xpos[2] = 0.100006658017411736993906856696; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 5); EXPECT_NEAR(dist, -0.00009843, kTolerance); @@ -817,11 +840,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) { xpos[2] = -4.958375812037025376355359185254; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 8); mj_deleteData(data); @@ -878,11 +903,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) { xpos[1] = -0.023505499999999998617106200527; xpos[2] = -4.958574289672835533338002278470; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -940,11 +967,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) { xpos[2] = 0.2156259187793853615566774806211469694972; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -979,11 +1008,13 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) { xpos[1] = -0.0765140000000000264357424839545274153352; xpos[2] = 0.1751399999999999623767621415026951581240; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 8); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8); EXPECT_EQ(ncons, 4); @@ -1030,9 +1061,11 @@ TEST_F(MjGjkTest, SmallBoxMesh) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, 0, kTolerance); @@ -1072,11 +1105,13 @@ TEST_F(MjGjkTest, BoxMesh) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 1000); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -1105,11 +1140,13 @@ TEST_F(MjGjkTest, BoxMesh2) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 1000); EXPECT_EQ(ncons, 5); mj_deleteData(data); @@ -1138,11 +1175,13 @@ TEST_F(MjGjkTest, BoxMeshPrune) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom2, geom1, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g2, g1, 0, 4); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -1173,11 +1212,13 @@ TEST_F(MjGjkTest, MeshMesh) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 5); mj_deleteData(data); @@ -1208,11 +1249,13 @@ TEST_F(MjGjkTest, MeshMeshPrune) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -1246,11 +1289,13 @@ TEST_F(MjGjkTest, BoxEdge) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); EXPECT_EQ(ncons, 2); mj_deleteData(data); @@ -1319,11 +1364,13 @@ TEST_F(MjGjkTest, BoxEdge2) { xpos[1] = 0.9828851949225971829093850828940048813820; xpos[2] = 3.0930077345364814789263618877157568931580; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); EXPECT_EQ(ncons, 2); mj_deleteData(data); @@ -1391,11 +1438,13 @@ TEST_F(MjGjkTest, BoxEdgeEdge) { xpos[1] = -0.0000000000000000008679606505055748997840; xpos[2] = 2.8141526153588731773425024584867060184479; - int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); EXPECT_EQ(ncons, 2); mj_deleteData(data); @@ -1439,11 +1488,13 @@ TEST_F(MjGjkTest, MeshEdge) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "box2"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "box3"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); EXPECT_EQ(ncons, 2); mj_deleteData(data); @@ -1461,9 +1512,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2); EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.00022548856248122027, kTolerance); @@ -1542,11 +1595,13 @@ static constexpr char xml[] = R"( mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2); EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.01, kTolerance); @@ -1560,7 +1615,7 @@ static constexpr char xml[] = R"( EXPECT_NEAR(pos[2], -0.005, kTolerance); // multicontact - ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000); EXPECT_EQ(ncons, 4); mj_deleteData(data); @@ -1583,11 +1638,13 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { mjData* data = mj_makeData(model); mj_forward(model, data); - int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); - int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; std::vector dir, pos; mjtNum dist; - int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 15); + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 15); EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -14.245732934582151, kTolerance);