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

Fix bug with the position of bounding sphere in mj_multiRay.

Browse Source 
PiperOrigin-RevId: 529665457
Change-Id: I486a094303b3b26c8067f8be7aca03afbe8e6217
This commit is contained in:
Alessio Quaglino
2023-05-05 02:52:28 -07:00
committed by Copybara-Service
parent e38fb136e7
commit b798f89212
2 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_ray.c
+3 -1
View File
@@ -957,9 +957,11 @@ static mjtNum mju_singleRay(const mjModel* m, mjData* d, const mjtNum pnt[3], co
// exclude body using bounding sphere test // exclude body using bounding sphere test
if (m->body_bvhadr[b] != -1) { if (m->body_bvhadr[b] != -1) {
mjtNum* pos = m->bvh_aabb + 6*m->body_bvhadr[b]; mjtNum* pos = m->bvh_aabb + 6*m->body_bvhadr[b];
mjtNum center[3];
mjtNum* size = pos + 3; mjtNum* size = pos + 3;
mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2]; mjtNum ssz = size[0]*size[0] + size[1]*size[1] + size[2]*size[2];
if (ray_sphere(pos, NULL, ssz, pnt, vec)<0) { mju_add3(center, pos, d->xipos+3*b);
if (ray_sphere(center, NULL, ssz, pnt, vec)<0) {
continue; continue;
} }
} }
test/engine/engine_ray_test.cc
+7 -5
View File
@@ -29,7 +29,9 @@ namespace {
static constexpr char kSingleGeomModel[] = R"( static constexpr char kSingleGeomModel[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
<geom type="sphere" size=".1" pos="0 0 0"/> <body pos="-1 0 0">
<geom type="sphere" size=".1"/>
</body>
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
@@ -200,7 +202,7 @@ TEST_F(RayTest, EdgeCases) {
int rgeomid; int rgeomid;
// pnt contained in bounding box // pnt contained in bounding box
mjtNum pnt1[] = {0, 0, 0}; mjtNum pnt1[] = {-1, 0, 0};
mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, geom_ba, NULL); mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, geom_ba, NULL);
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); EXPECT_FLOAT_EQ(geom_ba[0], -mjPI);
EXPECT_FLOAT_EQ(geom_ba[1], 0); EXPECT_FLOAT_EQ(geom_ba[1], 0);
@@ -211,13 +213,13 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_FLOAT_EQ(dist, 0.1); EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi // pnt at phi = Pi, -Pi
mjtNum pnt2[] = {1, 0, 0}; mjtNum pnt2[] = {-.5, 0, 0};
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, geom_ba, NULL); mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, geom_ba, NULL);
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0) EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0) EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
mjtNum vec2[] = {-1, 0, 0}; mjtNum vec2[] = {-1, 0, 0};
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1); mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, 1);
EXPECT_FLOAT_EQ(dist, 0.9); EXPECT_FLOAT_EQ(dist, 0.4);
// pnt on the boundary of the box // pnt on the boundary of the box
mjtNum pnt3[] = {.1, .1, .05}; mjtNum pnt3[] = {.1, .1, .05};
@@ -229,7 +231,7 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_FLOAT_EQ(dist, -1); EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom // size 0 geom
mjtNum pnt4[] = {-1, 0, 0}; mjtNum pnt4[] = {-2, 0, 0};
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0; m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0; m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, geom_ba, NULL); mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, geom_ba, NULL);