diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c
index e1cca49e..ce1b44ad 100644
--- a/src/engine/engine_support.c
+++ b/src/engine/engine_support.c
@@ -486,10 +486,10 @@ mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2,
return dist;
}
- // use nativecdd if flag is enabled
+ // use nativeccd if flag is enabled
if (!mjDISABLED(mjDSBL_NATIVECCD)) {
if (func == mjc_Convex || func == mjc_BoxBox) {
- return mj_geomDistanceCCD(m, d, g1, g2, distmax, fromto);
+ return mj_geomDistanceCCD(m, d, geom1, geom2, distmax, fromto);
}
}
diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc
index db97910f..89b2bd8d 100644
--- a/test/engine/engine_support_test.cc
+++ b/test/engine/engine_support_test.cc
@@ -980,7 +980,7 @@ TEST_F(InertiaTest, FullM) {
mj_deleteModel(m);
}
-static constexpr char GeomDistanceTestingModel[] = R"(
+static constexpr char GeomDistanceTestingModel1[] = R"(
)";
+static constexpr char GeomDistanceTestingModel2[] = R"(
+
+
+
+
+
+
+)";
+
TEST_F(SupportTest, GeomDistance) {
char error[1024];
mjModel* model =
- LoadModelFromString(GeomDistanceTestingModel, error, sizeof(error));
+ LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
@@ -1042,19 +1051,45 @@ TEST_F(SupportTest, GeomDistance) {
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
DoubleNear(0.7, eps));
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
- vector{0, 0, .8, 0, 0, .1}));
+ vector{0, 0, .1, 0, 0, .8}));
// mesh-sphere (far distmax)
distmax = 1.0;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
DoubleNear(0.7, eps));
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
- vector{0, 0, .8, 0, 0, .1}));
+ vector{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
mj_deleteModel(model);
}
+TEST_F(SupportTest, GeomDistanceFromToFlipped) {
+ mjtNum distmax = 10.0;
+ char error[1024];
+ mjModel* model =
+ LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error));
+ ASSERT_THAT(model, NotNull()) << error;
+ mjData* data = mj_makeData(model);
+ mj_kinematics(model, data);
+
+ mjtNum fromto01[6];
+ mjtNum fromto10[6];
+
+ for (int flag : {0, (int)mjDSBL_NATIVECCD}) {
+ model->opt.disableflags = flag;
+ mj_geomDistance(model, data, 0, 1, distmax, fromto01);
+ mj_geomDistance(model, data, 1, 0, distmax, fromto10);
+ mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5],
+ fromto10[0], fromto10[1], fromto10[2]};
+
+ EXPECT_THAT(AsVector(fromto10flipped, 6),
+ Pointwise(DoubleNear(1.0e-12), fromto01));
+ }
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
static constexpr char kSetKeyframeTestingModel[] = R"(