diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index a19059db..af21d5cb 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -130,6 +130,112 @@ mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2,
using MjGjkTest = MujocoTest;
+TEST_F(MjGjkTest, SphereSphereIntersect) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+ )";
+
+ std::array error;
+ mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+ ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+ 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");
+ mjtNum dir[3], pos[3];
+ mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+ // penetration depth
+ EXPECT_NEAR(dist, -2, kTolerance);
+
+ // direction
+ EXPECT_NEAR(dir[0], 1, kTolerance);
+ EXPECT_NEAR(dir[1], 0, 0.001);
+ EXPECT_NEAR(dir[2], 0, 0.001);
+
+ // position
+ EXPECT_NEAR(pos[0], 1, kTolerance);
+ EXPECT_NEAR(pos[1], 0, kTolerance);
+ EXPECT_NEAR(pos[2], 0, kTolerance);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
+TEST_F(MjGjkTest, BoxBoxIntersect) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+ )";
+
+ std::array error;
+ mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+ ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+ 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");
+ mjtNum dir[3], pos[3];
+ mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+ EXPECT_NEAR(dist, -1, kTolerance);
+ EXPECT_NEAR(dir[0], 1, kTolerance);
+ EXPECT_NEAR(dir[1], 0, kTolerance);
+ EXPECT_NEAR(dir[2], 0, kTolerance);
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
+TEST_F(MjGjkTest, EllipsoidEllipsoidTouching) {
+ static constexpr char xml[] = R"()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+)";
+
+ std::array error;
+ mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+ ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+ mjData* data = mj_makeData(model);
+ mj_resetDataKeyframe(model, data, 0);
+ mj_forward(model, data);
+
+ int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+ int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+ mjtNum dir[3], pos[3];
+ mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+ EXPECT_NEAR(dist, -0.0044873597898091094, kTolerance);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
TEST_F(MjGjkTest, SphereSphere) {
static constexpr char xml[] = R"(