diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index c662529b..dac58cfb 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1538,6 +1538,9 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
sub3(diff1, v2, v1);
sub3(diff2, v3, v1);
cross3(res, diff1, diff2);
+
+ // normalize isn't needed (cancelled out), but done to avoid asymmetric rounding later on
+ mju_normalize3(res);
return dot3(res, v1);
}
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 71eca6b1..64c51937 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -411,6 +411,65 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
EXPECT_NEAR(dir[2], -1, kTolerance);
}
+
+TEST_F(MjGjkTest, BoxBoxSize05) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+ )";
+
+ TestModel model = LoadModel(xml);
+ TestData data = MakeData(model.get());
+ mj_forward(model.get(), data.get());
+
+ mjtNum* xmat = data->geom_xmat;
+ mjtNum* xpos = data->geom_xpos;
+
+ xmat[0] = 1.000000000000000;
+ xmat[1] = 0.000000047289880;
+ xmat[2] = -0.000000050905665;
+ xmat[3] = -0.000000047289880;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = 0.000000017136196;
+ xmat[6] = 0.000000050905665;
+ xmat[7] = -0.000000017136193;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000000009724202;
+ xpos[1] = -0.000000014139289;
+ xpos[2] = 7.369161128997803;
+
+ xmat = data->geom_xmat + 9;
+ xpos = data->geom_xpos + 3;
+
+ xmat[0] = 1.000000000000000;
+ xmat[1] = -0.000000013726950;
+ xmat[2] = 0.000000008946020;
+ xmat[3] = 0.000000013726950;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000000012039017;
+ xmat[6] = -0.000000008946020;
+ xmat[7] = 0.000000012039017;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000000013445962;
+ xpos[1] = -0.000000019194527;
+ xpos[2] = 8.264492034912109;
+
+ int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+ int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+
+ mjCCDStatus status;
+ std::vector dir, pos;
+ mjtNum dist;
+ int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
+
+ ASSERT_EQ(ncons, 4);
+}
+
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(
@@ -424,13 +483,13 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
TestData data = MakeData(model.get());
mj_forward(model.get(), data.get());
- int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
- int geom2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+ int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+ int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
mjCCDStatus status;
std::vector dir, pos;
mjtNum dist;
- int ncons = Penetration(status, dist, dir, pos, model, data, geom1, geom2);
+ int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2);
ASSERT_EQ(ncons, 0);
EXPECT_EQ(status.epa_status, -1);