chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update mj_geomDistance: don't flip geoms for nativeccd.

Browse Source 
PiperOrigin-RevId: 824463375
Change-Id: Ic9c4cac916129b88575e0366e1b95353ce716ca2
This commit is contained in:
Taylor Howell
2025-10-27 04:47:45 -07:00
committed by Copybara-Service
parent 8c5154c9a3
commit 85e9cff0c1
2 changed files with 41 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+2 -2
View File
@@ -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);
}
}
test/engine/engine_support_test.cc
+39 -4
View File
@@ -980,7 +980,7 @@ TEST_F(InertiaTest, FullM) {
mj_deleteModel(m);
}
static constexpr char GeomDistanceTestingModel[] = R"(
static constexpr char GeomDistanceTestingModel1[] = R"(
<mujoco>
<option>
<flag nativeccd="enable"/>
@@ -999,10 +999,19 @@ static constexpr char GeomDistanceTestingModel[] = R"(
</mujoco>
)";
static constexpr char GeomDistanceTestingModel2[] = R"(
<mujoco>
<worldbody>
<geom type="sphere" size=".1"/>
<geom type="ellipsoid" size=".1 .1 .1" pos="0 0 1"/>
</worldbody>
</mujoco>
)";
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<mjtNum>{0, 0, .8, 0, 0, .1}));
vector<mjtNum>{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<mjtNum>{0, 0, .8, 0, 0, .1}));
vector<mjtNum>{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"(
<mujoco>
<size nkey="2"/>