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

Fix bug in raycast to hfield. Fixes #2765

Browse Source 
PiperOrigin-RevId: 790784429
Change-Id: Ib2319722796ed510afed960bbf5cebc86cf185a9
This commit is contained in:
Yuval Tassa
2025-08-04 09:39:45 -07:00
committed by Copybara-Service
parent f2a2afeb03
commit 7b9f5bdae7
2 changed files with 62 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+9 -8
View File
@@ -497,7 +497,8 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) && mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
if (mju_abs(lvec[0]) >= mju_abs(lvec[1]) &&
mju_abs(lvec[0]) >= mju_abs(lvec[2])) {
b0[0] = 0;
} else if (mju_abs(lvec[1]) >= mju_abs(lvec[2])) {
b0[1] = 0;
@@ -528,9 +529,9 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
}
// compute ranges, with +1 padding
int cmin = mjMAX(0, (int)mju_floor(mjMIN(SX[0], SX[1]))-1);
int cmin = mjMAX(0, (int)mju_floor(mjMIN(SX[0], SX[1]))-1);
int cmax = mjMIN(ncol-1, (int)mju_ceil(mjMAX(SX[0], SX[1]))+1);
int rmin = mjMAX(0, (int)mju_floor(mjMIN(SY[0], SY[1]))-1);
int rmin = mjMAX(0, (int)mju_floor(mjMIN(SY[0], SY[1]))-1);
int rmax = mjMIN(nrow-1, (int)mju_ceil(mjMAX(SY[0], SY[1]))+1);
// check triangles within bounds
@@ -540,7 +541,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
mjtNum va[3][3] = {
{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
{dx*(c+1)-size[0], dy*r-size[1], data[r*ncol+(c+1)]*size[2]}
{dx*(c+1)-size[0], dy*(r+0)-size[1], data[(r+0)*ncol+(c+1)]*size[2]}
};
mjtNum sol = ray_triangle(va, lpnt, lvec, b0, b1);
if (sol >= 0 && (x < 0 || sol < x)) {
@@ -551,7 +552,7 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
mjtNum vb[3][3] = {
{dx*c-size[0], dy*r-size[1], data[r*ncol+c]*size[2]},
{dx*(c+1)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+1)]*size[2]},
{dx*c-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+c]*size[2]}
{dx*(c+0)-size[0], dy*(r+1)-size[1], data[(r+1)*ncol+(c+0)]*size[2]}
};
sol = ray_triangle(vb, lpnt, lvec, b0, b1);
if (sol >= 0 && (x < 0 || sol < x)) {
@@ -573,15 +574,15 @@ mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int id,
if (i < 2) {
y = (lpnt[1] + all[i]*lvec[1] + size[1]) / dy;
y0 = mjMAX(0, mjMIN(nrow-2, mju_floor(y)));
z0 = (mjtNum)data[mju_round(y0)*nrow + (i == 1 ? ncol-1 : 0)];
z1 = (mjtNum)data[mju_round(y0+1)*nrow + (i == 1 ? ncol-1 : 0)];
z0 = (mjtNum)data[mju_round(y0+0)*ncol + (i == 1 ? ncol-1 : 0)];
z1 = (mjtNum)data[mju_round(y0+1)*ncol + (i == 1 ? ncol-1 : 0)];
}
// side normal to y-axis
else {
y = (lpnt[0] + all[i]*lvec[0] + size[0]) / dx;
y0 = mjMAX(0, mjMIN(ncol-2, mju_floor(y)));
z0 = (mjtNum)data[mju_round(y0) + (i == 3 ? (nrow-1)*ncol : 0)];
z0 = (mjtNum)data[mju_round(y0+0) + (i == 3 ? (nrow-1)*ncol : 0)];
z1 = (mjtNum)data[mju_round(y0+1) + (i == 3 ? (nrow-1)*ncol : 0)];
}
test/engine/engine_ray_test.cc
+53
View File
@@ -73,6 +73,7 @@ static constexpr char kCubeletModel[] = R"(
</mujoco>
)";
using ::testing::DoubleNear;
using ::testing::NotNull;
using RayTest = MujocoTest;
@@ -403,5 +404,57 @@ TEST_F(RayTest, RayMeshPruning) {
mj_deleteModel(m);
}
TEST_F(RayTest, RayHfield) {
const char xml[] = R"(
<mujoco>
<asset>
<hfield name="J" size="2 3 1 1" nrow="4" ncol="3"
elevation="0 0 0
0 0 0
1 1 1
1 1 1"/>
</asset>
<worldbody>
<light pos="0 0 1"/>
<body name="dummy">
<geom type="hfield" hfield="J" pos="0 0 .5"/>
</body>
<site name="1" pos="3 -2 1" zaxis="-1 0 0"/>
<site name="2" pos="3 -2 0" zaxis="-1 0 0"/>
<site name="3" pos="3 1 0" zaxis="-1 0 0"/>
<body mocap="true" pos="1 0 1.5">
<geom type="box" size=".2 .2 .2"/>
<site name="4" zaxis="0 0 -1"/>
</body>
</worldbody>
<sensor>
<rangefinder site="1"/>
<rangefinder site="2"/>
<rangefinder site="3"/>
<rangefinder site="4"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error), 0);
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
double tol = 1e-8;
EXPECT_THAT(data->sensordata[0], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[1], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[2], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[3], DoubleNear(0.5, tol));
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco